mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-20 10:54:14 +01:00
LP-512 HwTinyFISHSettings cleanup. First attempt at shared SBus/SensorHub port.
This commit is contained in:
parent
e8bccefb9d
commit
45448875d4
@ -66,12 +66,21 @@
|
||||
// Private variables
|
||||
static bool batteryEnabled = false;
|
||||
|
||||
#ifndef PIOS_ADC_VOLTAGE_PIN
|
||||
#define PIOS_ADC_VOLTAGE_PIN -1
|
||||
#endif
|
||||
|
||||
#ifndef PIOS_ADC_CURRENT_PIN
|
||||
#define PIOS_ADC_CURRENT_PIN -1
|
||||
#endif
|
||||
|
||||
|
||||
// THESE COULD BE BETTER AS SOME KIND OF UNION OR STRUCT, BY WHICH 4 BITS ARE USED FOR EACH
|
||||
// PIN VARIABLE, ONE OF WHICH INDICATES SIGN, AND THE OTHER 3 BITS INDICATE POSITION. THIS WILL
|
||||
// WORK FOR QUITE SOMETIME, UNTIL MORE THAN 8 ADC ARE AVAILABLE. EVEN AT THIS POINT, THE STRUCTURE
|
||||
// CAN SIMPLY BE MODIFIED TO SUPPORT 15 ADC PINS, BY USING ALL AVAILABLE BITS.
|
||||
static int8_t voltageADCPin = -1; // ADC pin for voltage
|
||||
static int8_t currentADCPin = -1; // ADC pin for current
|
||||
static int8_t voltageADCPin = PIOS_ADC_VOLTAGE_PIN; // ADC pin for voltage
|
||||
static int8_t currentADCPin = PIOS_ADC_CURRENT_PIN; // ADC pin for current
|
||||
|
||||
// Private functions
|
||||
static void onTimer(UAVObjEvent *ev);
|
||||
|
@ -266,8 +266,6 @@ static int32_t PIOS_DSM_Init_Helper(uint32_t *id, const struct pios_com_driver *
|
||||
# ifdef PIOS_INCLUDE_SBUS
|
||||
static int32_t PIOS_SBus_Init_Helper(uint32_t *id, const struct pios_com_driver *driver, uint32_t lower_id)
|
||||
{
|
||||
// sbus-noninverted
|
||||
|
||||
HwSettingsSBusModeOptions hwsettings_SBusMode;
|
||||
|
||||
HwSettingsSBusModeGet(&hwsettings_SBusMode);
|
||||
@ -278,7 +276,23 @@ static int32_t PIOS_SBus_Init_Helper(uint32_t *id, const struct pios_com_driver
|
||||
|
||||
return PIOS_SBus_Init(id, &sbus_cfg, driver, lower_id);
|
||||
}
|
||||
# endif
|
||||
static int32_t PIOS_SBus_Normal_Init_Helper(uint32_t *id, const struct pios_com_driver *driver, uint32_t lower_id)
|
||||
{
|
||||
struct pios_sbus_cfg sbus_cfg = {
|
||||
.non_inverted = false,
|
||||
};
|
||||
|
||||
return PIOS_SBus_Init(id, &sbus_cfg, driver, lower_id);
|
||||
}
|
||||
static int32_t PIOS_SBus_Not_Inverted_Init_Helper(uint32_t *id, const struct pios_com_driver *driver, uint32_t lower_id)
|
||||
{
|
||||
struct pios_sbus_cfg sbus_cfg = {
|
||||
.non_inverted = true,
|
||||
};
|
||||
|
||||
return PIOS_SBus_Init(id, &sbus_cfg, driver, lower_id);
|
||||
}
|
||||
# endif /* ifdef PIOS_INCLUDE_SBUS */
|
||||
#endif /* ifdef PIOS_INCLUDE_RCVR */
|
||||
|
||||
struct uart_function {
|
||||
@ -293,120 +307,129 @@ struct uart_function {
|
||||
};
|
||||
|
||||
static const struct uart_function uart_function_map[] = {
|
||||
[PIOS_BOARD_IO_UART_TELEMETRY] = {
|
||||
[PIOS_BOARD_IO_UART_TELEMETRY] = {
|
||||
.com_id = &pios_com_telem_rf_id,
|
||||
.com_rx_buf_len = PIOS_COM_TELEM_RF_RX_BUF_LEN,
|
||||
.com_tx_buf_len = PIOS_COM_TELEM_RF_TX_BUF_LEN,
|
||||
},
|
||||
|
||||
[PIOS_BOARD_IO_UART_MAVLINK] = {
|
||||
[PIOS_BOARD_IO_UART_MAVLINK] = {
|
||||
.com_id = &pios_com_mavlink_id,
|
||||
.com_rx_buf_len = PIOS_COM_MAVLINK_RX_BUF_LEN,
|
||||
.com_tx_buf_len = PIOS_COM_MAVLINK_TX_BUF_LEN,
|
||||
},
|
||||
|
||||
[PIOS_BOARD_IO_UART_MSP] = {
|
||||
[PIOS_BOARD_IO_UART_MSP] = {
|
||||
.com_id = &pios_com_msp_id,
|
||||
.com_rx_buf_len = PIOS_COM_MSP_RX_BUF_LEN,
|
||||
.com_tx_buf_len = PIOS_COM_MSP_TX_BUF_LEN,
|
||||
},
|
||||
#ifdef PIOS_INCLUDE_GPS
|
||||
[PIOS_BOARD_IO_UART_GPS] = {
|
||||
[PIOS_BOARD_IO_UART_GPS] = {
|
||||
.com_id = &pios_com_gps_id,
|
||||
.com_rx_buf_len = PIOS_COM_GPS_RX_BUF_LEN,
|
||||
.com_tx_buf_len = PIOS_COM_GPS_TX_BUF_LEN,
|
||||
},
|
||||
#endif
|
||||
[PIOS_BOARD_IO_UART_OSDHK] = {
|
||||
[PIOS_BOARD_IO_UART_OSDHK] = {
|
||||
.com_id = &pios_com_hkosd_id,
|
||||
.com_rx_buf_len = PIOS_COM_HKOSD_RX_BUF_LEN,
|
||||
.com_tx_buf_len = PIOS_COM_HKOSD_TX_BUF_LEN,
|
||||
},
|
||||
#ifdef PIOS_INCLUDE_FRSKY_SENSORHUB
|
||||
[PIOS_BOARD_IO_UART_FRSKY_SENSORHUB] = {
|
||||
[PIOS_BOARD_IO_UART_FRSKY_SENSORHUB] = {
|
||||
.com_id = &pios_com_frsky_sensorhub_id,
|
||||
.com_rx_buf_len = PIOS_COM_FRSKY_SENSORHUB_RX_BUF_LEN,
|
||||
.com_tx_buf_len = PIOS_COM_FRSKY_SENSORHUB_TX_BUF_LEN,
|
||||
},
|
||||
#endif
|
||||
#ifdef PIOS_INCLUDE_HOTT_BRIDGE
|
||||
[PIOS_BOARD_IO_UART_HOTT_BRIDGE] = {
|
||||
[PIOS_BOARD_IO_UART_HOTT_BRIDGE] = {
|
||||
.com_id = &pios_com_hott_id,
|
||||
.com_rx_buf_len = PIOS_COM_HOTT_BRIDGE_RX_BUF_LEN,
|
||||
.com_tx_buf_len = PIOS_COM_HOTT_BRIDGE_TX_BUF_LEN,
|
||||
},
|
||||
#endif
|
||||
#ifdef PIOS_INCLUDE_DEBUG_CONSOLE
|
||||
[PIOS_BOARD_IO_UART_DEBUGCONSOLE] = {
|
||||
[PIOS_BOARD_IO_UART_DEBUGCONSOLE] = {
|
||||
.com_id = &pios_com_debug_id,
|
||||
.com_tx_buf_len = PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN,
|
||||
},
|
||||
#endif
|
||||
[PIOS_BOARD_IO_UART_COMBRIDGE] = {
|
||||
[PIOS_BOARD_IO_UART_COMBRIDGE] = {
|
||||
.com_id = &pios_com_bridge_id,
|
||||
.com_rx_buf_len = PIOS_COM_BRIDGE_RX_BUF_LEN,
|
||||
.com_tx_buf_len = PIOS_COM_BRIDGE_TX_BUF_LEN,
|
||||
},
|
||||
#ifdef PIOS_INCLUDE_RCVR
|
||||
# ifdef PIOS_INCLUDE_IBUS
|
||||
[PIOS_BOARD_IO_UART_IBUS] = {
|
||||
[PIOS_BOARD_IO_UART_IBUS] = {
|
||||
.rcvr_init = &PIOS_IBUS_Init,
|
||||
.rcvr_driver = &pios_ibus_rcvr_driver,
|
||||
.rcvr_group = MANUALCONTROLSETTINGS_CHANNELGROUPS_IBUS,
|
||||
},
|
||||
# endif /* PIOS_INCLUDE_IBUS */
|
||||
# ifdef PIOS_INCLUDE_EXBUS
|
||||
[PIOS_BOARD_IO_UART_EXBUS] = {
|
||||
[PIOS_BOARD_IO_UART_EXBUS] = {
|
||||
.rcvr_init = &PIOS_EXBUS_Init,
|
||||
.rcvr_driver = &pios_exbus_rcvr_driver,
|
||||
.rcvr_group = MANUALCONTROLSETTINGS_CHANNELGROUPS_EXBUS,
|
||||
},
|
||||
# endif /* PIOS_INCLUDE_EXBUS */
|
||||
# ifdef PIOS_INCLUDE_SRXL
|
||||
[PIOS_BOARD_IO_UART_SRXL] = {
|
||||
[PIOS_BOARD_IO_UART_SRXL] = {
|
||||
.rcvr_init = &PIOS_SRXL_Init,
|
||||
.rcvr_driver = &pios_srxl_rcvr_driver,
|
||||
.rcvr_group = MANUALCONTROLSETTINGS_CHANNELGROUPS_SRXL,
|
||||
},
|
||||
# endif /* PIOS_INCLUDE_SRXL */
|
||||
# ifdef PIOS_INCLUDE_HOTT
|
||||
[PIOS_BOARD_IO_UART_HOTT_SUMD] = {
|
||||
[PIOS_BOARD_IO_UART_HOTT_SUMD] = {
|
||||
.rcvr_init = &PIOS_HOTT_Init_SUMD,
|
||||
.rcvr_driver = &pios_hott_rcvr_driver,
|
||||
.rcvr_group = MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTT,
|
||||
},
|
||||
|
||||
[PIOS_BOARD_IO_UART_HOTT_SUMH] = {
|
||||
[PIOS_BOARD_IO_UART_HOTT_SUMH] = {
|
||||
.rcvr_init = &PIOS_HOTT_Init_SUMH,
|
||||
.rcvr_driver = &pios_hott_rcvr_driver,
|
||||
.rcvr_group = MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTT,
|
||||
},
|
||||
# endif /* PIOS_INCLUDE_HOTT */
|
||||
# ifdef PIOS_INCLUDE_DSM
|
||||
[PIOS_BOARD_IO_UART_DSM_MAIN] = {
|
||||
[PIOS_BOARD_IO_UART_DSM_MAIN] = {
|
||||
.rcvr_init = &PIOS_DSM_Init_Helper,
|
||||
.rcvr_driver = &pios_dsm_rcvr_driver,
|
||||
.rcvr_group = MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT,
|
||||
},
|
||||
|
||||
[PIOS_BOARD_IO_UART_DSM_FLEXI] = {
|
||||
[PIOS_BOARD_IO_UART_DSM_FLEXI] = {
|
||||
.rcvr_init = &PIOS_DSM_Init_Helper,
|
||||
.rcvr_driver = &pios_dsm_rcvr_driver,
|
||||
.rcvr_group = MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT,
|
||||
},
|
||||
|
||||
[PIOS_BOARD_IO_UART_DSM_RCVR] = {
|
||||
[PIOS_BOARD_IO_UART_DSM_RCVR] = {
|
||||
.rcvr_init = &PIOS_DSM_Init_Helper,
|
||||
.rcvr_driver = &pios_dsm_rcvr_driver,
|
||||
.rcvr_group = MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMRCVRPORT,
|
||||
},
|
||||
# endif /* PIOS_INCLUDE_DSM */
|
||||
# ifdef PIOS_INCLUDE_SBUS
|
||||
[PIOS_BOARD_IO_UART_SBUS] = {
|
||||
[PIOS_BOARD_IO_UART_SBUS] = {
|
||||
.rcvr_init = &PIOS_SBus_Init_Helper,
|
||||
.rcvr_driver = &pios_sbus_rcvr_driver,
|
||||
.rcvr_group = MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS,
|
||||
},
|
||||
[PIOS_BOARD_IO_UART_SBUS_NORMAL] = {
|
||||
.rcvr_init = &PIOS_SBus_Normal_Init_Helper,
|
||||
.rcvr_driver = &pios_sbus_rcvr_driver,
|
||||
.rcvr_group = MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS,
|
||||
},
|
||||
[PIOS_BOARD_IO_UART_SBUS_NOT_INVERTED] = {
|
||||
.rcvr_init = &PIOS_SBus_Not_Inverted_Init_Helper,
|
||||
.rcvr_driver = &pios_sbus_rcvr_driver,
|
||||
.rcvr_group = MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS,
|
||||
},
|
||||
# endif /* PIOS_INCLUDE_SBUS */
|
||||
#endif /* PIOS_INCLUDE_RCVR */
|
||||
};
|
||||
|
@ -775,7 +775,7 @@ static bool PIOS_MPU6000_HandleData(uint32_t gyro_read_timestamp_p)
|
||||
queue_data->sample[1].x = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_X)); // chip X
|
||||
break;
|
||||
}
|
||||
if((dev->cfg->orientation & PIOS_MPU6000_LOCATION_MASK) == PIOS_MPU6000_LOCATION_TOP) {
|
||||
if ((dev->cfg->orientation & PIOS_MPU6000_LOCATION_MASK) == PIOS_MPU6000_LOCATION_TOP) {
|
||||
queue_data->sample[0].z = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_Z));
|
||||
queue_data->sample[1].z = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_Z));
|
||||
} else {
|
||||
|
@ -172,9 +172,6 @@ extern uint32_t pios_com_frsky_sensorhub_id;
|
||||
#ifndef PIOS_COM_FRSKY_SENSORHUB_TX_BUF_LEN
|
||||
# define PIOS_COM_FRSKY_SENSORHUB_TX_BUF_LEN 128
|
||||
#endif
|
||||
#ifndef PIOS_COM_FRSKY_SENSORHUB_RX_BUF_LEN
|
||||
# define PIOS_COM_FRSKY_SENSORHUB_RX_BUF_LEN 128
|
||||
#endif
|
||||
|
||||
/* USB VCP */
|
||||
extern uint32_t pios_com_vcp_id;
|
||||
@ -207,7 +204,9 @@ typedef enum {
|
||||
PIOS_BOARD_IO_UART_COMBRIDGE, /* com */
|
||||
PIOS_BOARD_IO_UART_DEBUGCONSOLE, /* com */
|
||||
PIOS_BOARD_IO_UART_OSDHK, /* com */
|
||||
PIOS_BOARD_IO_UART_SBUS, /* rcvr */
|
||||
PIOS_BOARD_IO_UART_SBUS, /* rcvr, normal vs not-inverted from HwSettings.SBusMode */
|
||||
PIOS_BOARD_IO_UART_SBUS_NORMAL, /* helper only */
|
||||
PIOS_BOARD_IO_UART_SBUS_NOT_INVERTED, /* helper only */
|
||||
PIOS_BOARD_IO_UART_DSM_MAIN, /* rcvr */
|
||||
PIOS_BOARD_IO_UART_DSM_FLEXI, /* rcvr */
|
||||
PIOS_BOARD_IO_UART_DSM_RCVR, /* rcvr */
|
||||
|
@ -32,17 +32,21 @@
|
||||
#define PIOS_DEBUG_H
|
||||
|
||||
#ifdef PIOS_INCLUDE_DEBUG_CONSOLE
|
||||
#ifndef DEBUG_LEVEL
|
||||
#define DEBUG_LEVEL 0
|
||||
#endif
|
||||
#define DEBUG_PRINTF(level, ...) \
|
||||
# ifndef PIOS_COM_DEBUG
|
||||
extern uint32_t pios_com_debug_id;
|
||||
# define PIOS_COM_DEBUG (pios_com_debug_id)
|
||||
# endif
|
||||
# ifndef DEBUG_LEVEL
|
||||
# define DEBUG_LEVEL 0
|
||||
# endif
|
||||
# define DEBUG_PRINTF(level, ...) \
|
||||
{ \
|
||||
if ((level <= DEBUG_LEVEL) && (PIOS_COM_DEBUG > 0)) { \
|
||||
PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_DEBUG, __VA_ARGS__); \
|
||||
PIOS_COM_SendFormattedString(PIOS_COM_DEBUG, __VA_ARGS__); \
|
||||
} \
|
||||
}
|
||||
#else
|
||||
#define DEBUG_PRINTF(level, ...)
|
||||
# define DEBUG_PRINTF(level, ...)
|
||||
#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */
|
||||
|
||||
extern const char *PIOS_DEBUG_AssertMsg;
|
||||
|
@ -126,10 +126,10 @@ enum pios_mpu6000_accel_range {
|
||||
};
|
||||
|
||||
|
||||
#define PIOS_MPU6000_LOCATION_TOP 0x00
|
||||
#define PIOS_MPU6000_LOCATION_BOTTOM 0x10
|
||||
#define PIOS_MPU6000_LOCATION_TOP 0x00
|
||||
#define PIOS_MPU6000_LOCATION_BOTTOM 0x10
|
||||
|
||||
#define PIOS_MPU6000_LOCATION_MASK 0xf0
|
||||
#define PIOS_MPU6000_LOCATION_MASK 0xf0
|
||||
|
||||
enum pios_mpu6000_orientation { // clockwise rotation from board forward, when looking at sensor itself, which can be also on the bottom side
|
||||
PIOS_MPU6000_TOP_0DEG = 0 | PIOS_MPU6000_LOCATION_TOP,
|
||||
@ -143,7 +143,6 @@ enum pios_mpu6000_orientation { // clockwise rotation from board forward, when l
|
||||
};
|
||||
|
||||
|
||||
|
||||
struct pios_mpu6000_cfg {
|
||||
const struct pios_exti_cfg *exti_cfg; /* Pointer to the EXTI configuration */
|
||||
uint8_t i2c_addr;
|
||||
|
@ -174,6 +174,40 @@ int32_t PIOS_USART_Init(uint32_t *usart_id, const struct pios_usart_cfg *cfg)
|
||||
PIOS_DEBUG_Assert(usart_id);
|
||||
PIOS_DEBUG_Assert(cfg);
|
||||
|
||||
uint32_t *local_id;
|
||||
uint8_t irq_channel;
|
||||
|
||||
switch ((uint32_t)cfg->regs) {
|
||||
case (uint32_t)USART1:
|
||||
local_id = &PIOS_USART_1_id;
|
||||
irq_channel = USART1_IRQn;
|
||||
break;
|
||||
case (uint32_t)USART2:
|
||||
local_id = &PIOS_USART_2_id;
|
||||
irq_channel = USART2_IRQn;
|
||||
break;
|
||||
case (uint32_t)USART3:
|
||||
local_id = &PIOS_USART_3_id;
|
||||
irq_channel = USART3_IRQn;
|
||||
break;
|
||||
case (uint32_t)UART4:
|
||||
local_id = &PIOS_UART_4_id;
|
||||
irq_channel = UART4_IRQn;
|
||||
break;
|
||||
case (uint32_t)UART5:
|
||||
local_id = &PIOS_UART_5_id;
|
||||
irq_channel = UART5_IRQn;
|
||||
break;
|
||||
default:
|
||||
goto out_fail;
|
||||
}
|
||||
|
||||
if(*local_id) {
|
||||
/* this port is already open */
|
||||
*usart_id = *local_id;
|
||||
return 0;
|
||||
}
|
||||
|
||||
struct pios_usart_dev *usart_dev;
|
||||
|
||||
usart_dev = (struct pios_usart_dev *)PIOS_USART_alloc();
|
||||
@ -183,6 +217,7 @@ int32_t PIOS_USART_Init(uint32_t *usart_id, const struct pios_usart_cfg *cfg)
|
||||
|
||||
/* Bind the configuration to the device instance */
|
||||
usart_dev->cfg = cfg;
|
||||
usart_dev->irq_channel = irq_channel;
|
||||
|
||||
/* Set some sane defaults */
|
||||
USART_StructInit(&usart_dev->init);
|
||||
@ -191,30 +226,7 @@ int32_t PIOS_USART_Init(uint32_t *usart_id, const struct pios_usart_cfg *cfg)
|
||||
usart_dev->init.USART_Mode = 0;
|
||||
|
||||
*usart_id = (uint32_t)usart_dev;
|
||||
|
||||
/* Configure USART Interrupts */
|
||||
switch ((uint32_t)usart_dev->cfg->regs) {
|
||||
case (uint32_t)USART1:
|
||||
PIOS_USART_1_id = (uint32_t)usart_dev;
|
||||
usart_dev->irq_channel = USART1_IRQn;
|
||||
break;
|
||||
case (uint32_t)USART2:
|
||||
PIOS_USART_2_id = (uint32_t)usart_dev;
|
||||
usart_dev->irq_channel = USART2_IRQn;
|
||||
break;
|
||||
case (uint32_t)USART3:
|
||||
PIOS_USART_3_id = (uint32_t)usart_dev;
|
||||
usart_dev->irq_channel = USART3_IRQn;
|
||||
break;
|
||||
case (uint32_t)UART4:
|
||||
PIOS_UART_4_id = (uint32_t)usart_dev;
|
||||
usart_dev->irq_channel = UART4_IRQn;
|
||||
break;
|
||||
case (uint32_t)UART5:
|
||||
PIOS_UART_5_id = (uint32_t)usart_dev;
|
||||
usart_dev->irq_channel = UART5_IRQn;
|
||||
break;
|
||||
}
|
||||
*local_id = (uint32_t)usart_dev;
|
||||
|
||||
PIOS_USART_SetIrqPrio(usart_dev, PIOS_IRQ_PRIO_MID);
|
||||
|
||||
|
@ -388,8 +388,8 @@ static const struct pios_tim_channel pios_tim_servoport_pins[] = {
|
||||
TIM_SERVO_CHANNEL_CONFIG(TIM15, 1, A, 2), // bank 3
|
||||
|
||||
// following two are here so we can have two additional PWM outputs instead of UART3
|
||||
TIM_SERVO_CHANNEL_CONFIG(TIM2, 4, B, 11), // bank 4 (UART3 RX)
|
||||
TIM_SERVO_CHANNEL_CONFIG(TIM2, 3, B, 10), // bank 4 (UART3 TX)
|
||||
TIM_SERVO_CHANNEL_CONFIG(TIM2, 4, B, 11), // bank 4 (UART3 RX)
|
||||
TIM_SERVO_CHANNEL_CONFIG(TIM2, 3, B, 10), // bank 4 (UART3 TX)
|
||||
};
|
||||
|
||||
#if defined(PIOS_INCLUDE_USART)
|
||||
@ -404,7 +404,7 @@ static const struct pios_tim_channel pios_tim_servoport_pins[] = {
|
||||
|
||||
static const struct pios_usart_cfg pios_usart_cfg[] = {
|
||||
USART_CONFIG(USART1, B, 7, B, 6), /* RX_DEBUG, TX_??? */
|
||||
USART_CONFIG(USART2, A, 15, A, 14), /* RX_SBUS, TX_FRSKY_TELEMETRY */
|
||||
USART_CONFIG(USART2, A, 15, A, 14), /* RX_SBUS, TX_FRSKY_TELEMETRY */
|
||||
USART_CONFIG(USART3, B, 11, B, 10),
|
||||
};
|
||||
|
||||
|
@ -47,6 +47,8 @@ MODULES += FirmwareIAP
|
||||
#MODULES += Logging
|
||||
MODULES += Telemetry
|
||||
MODULES += Notify
|
||||
MODULES += Battery
|
||||
MODULES += UAVOFrSKYSensorHubBridge
|
||||
|
||||
OPTMODULES += Airspeed
|
||||
OPTMODULES += AutoTune
|
||||
@ -54,7 +56,6 @@ OPTMODULES += GPS
|
||||
OPTMODULES += TxPID
|
||||
OPTMODULES += CameraStab
|
||||
OPTMODULES += CameraControl
|
||||
OPTMODULES += Battery
|
||||
OPTMODULES += ComUsbBridge
|
||||
OPTMODULES += UAVOMSPBridge
|
||||
OPTMODULES += UAVOMavlinkBridge
|
||||
|
@ -37,8 +37,8 @@
|
||||
* details.
|
||||
*/
|
||||
|
||||
/* #define PIOS_INCLUDE_DEBUG_CONSOLE */
|
||||
/* #define DEBUG_LEVEL 0 */
|
||||
#define PIOS_INCLUDE_DEBUG_CONSOLE
|
||||
#define DEBUG_LEVEL 0
|
||||
/* #define PIOS_ENABLE_DEBUG_PINS */
|
||||
|
||||
/* PIOS FreeRTOS support */
|
||||
@ -114,6 +114,8 @@
|
||||
/* #define PIOS_INCLUDE_GCSRCVR */
|
||||
/* #define PIOS_INCLUDE_OPLINKRCVR */
|
||||
|
||||
#define PIOS_INCLUDE_FRSKY_SENSORHUB
|
||||
|
||||
/* PIOS abstract receiver interface */
|
||||
#define PIOS_INCLUDE_RCVR
|
||||
|
||||
|
@ -120,7 +120,7 @@ void PIOS_Board_Init(void)
|
||||
}
|
||||
|
||||
/* init SPI flash here */
|
||||
|
||||
|
||||
#endif /* if defined(PIOS_INCLUDE_FLASH) */
|
||||
|
||||
/* Initialize the task monitor */
|
||||
@ -179,12 +179,12 @@ void PIOS_Board_Init(void)
|
||||
|
||||
PIOS_TIM_InitClock(&tim_1_cfg);
|
||||
PIOS_TIM_InitClock(&tim_2_cfg);
|
||||
// PIOS_TIM_InitClock(&tim_3_cfg);
|
||||
// PIOS_TIM_InitClock(&tim_3_cfg);
|
||||
PIOS_TIM_InitClock(&tim_4_cfg);
|
||||
PIOS_TIM_InitClock(&tim_8_cfg);
|
||||
PIOS_TIM_InitClock(&tim_15_cfg);
|
||||
// PIOS_TIM_InitClock(&tim_16_cfg);
|
||||
// PIOS_TIM_InitClock(&tim_17_cfg);
|
||||
// PIOS_TIM_InitClock(&tim_16_cfg);
|
||||
// PIOS_TIM_InitClock(&tim_17_cfg);
|
||||
|
||||
#if defined(PIOS_INCLUDE_USB)
|
||||
PIOS_BOARD_IO_Configure_USB();
|
||||
@ -195,42 +195,67 @@ void PIOS_Board_Init(void)
|
||||
HwTinyFISHSettingsGet(&boardHwSettings);
|
||||
|
||||
static const PIOS_BOARD_IO_UART_Function uart_function_map[] = {
|
||||
[HWTINYFISHSETTINGS_UARTPORT_TELEMETRY] = PIOS_BOARD_IO_UART_TELEMETRY,
|
||||
[HWTINYFISHSETTINGS_UARTPORT_GPS] = PIOS_BOARD_IO_UART_GPS,
|
||||
[HWTINYFISHSETTINGS_UARTPORT_SBUS] = PIOS_BOARD_IO_UART_SBUS,
|
||||
[HWTINYFISHSETTINGS_UARTPORT_DSM] = PIOS_BOARD_IO_UART_DSM_MAIN, // single DSM instance? ok.
|
||||
[HWTINYFISHSETTINGS_UARTPORT_EXBUS] = PIOS_BOARD_IO_UART_EXBUS,
|
||||
[HWTINYFISHSETTINGS_UARTPORT_HOTTSUMD] = PIOS_BOARD_IO_UART_HOTT_SUMD,
|
||||
[HWTINYFISHSETTINGS_UARTPORT_HOTTSUMH] = PIOS_BOARD_IO_UART_HOTT_SUMH,
|
||||
[HWTINYFISHSETTINGS_UARTPORT_SRXL] = PIOS_BOARD_IO_UART_SRXL,
|
||||
[HWTINYFISHSETTINGS_UARTPORT_IBUS] = PIOS_BOARD_IO_UART_IBUS,
|
||||
[HWTINYFISHSETTINGS_UARTPORT_DEBUGCONSOLE] = PIOS_BOARD_IO_UART_DEBUGCONSOLE,
|
||||
[HWTINYFISHSETTINGS_UARTPORT_COMBRIDGE] = PIOS_BOARD_IO_UART_COMBRIDGE,
|
||||
[HWTINYFISHSETTINGS_UARTPORT_MSP] = PIOS_BOARD_IO_UART_MSP,
|
||||
[HWTINYFISHSETTINGS_UARTPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK,
|
||||
[HWTINYFISHSETTINGS_UART3PORT_TELEMETRY] = PIOS_BOARD_IO_UART_TELEMETRY,
|
||||
[HWTINYFISHSETTINGS_UART3PORT_GPS] = PIOS_BOARD_IO_UART_GPS,
|
||||
[HWTINYFISHSETTINGS_UART3PORT_SBUS] = PIOS_BOARD_IO_UART_SBUS,
|
||||
[HWTINYFISHSETTINGS_UART3PORT_DSM] = PIOS_BOARD_IO_UART_DSM_MAIN, // single DSM instance? ok.
|
||||
[HWTINYFISHSETTINGS_UART3PORT_EXBUS] = PIOS_BOARD_IO_UART_EXBUS,
|
||||
[HWTINYFISHSETTINGS_UART3PORT_HOTTSUMD] = PIOS_BOARD_IO_UART_HOTT_SUMD,
|
||||
[HWTINYFISHSETTINGS_UART3PORT_HOTTSUMH] = PIOS_BOARD_IO_UART_HOTT_SUMH,
|
||||
[HWTINYFISHSETTINGS_UART3PORT_SRXL] = PIOS_BOARD_IO_UART_SRXL,
|
||||
[HWTINYFISHSETTINGS_UART3PORT_IBUS] = PIOS_BOARD_IO_UART_IBUS,
|
||||
[HWTINYFISHSETTINGS_UART3PORT_DEBUGCONSOLE] = PIOS_BOARD_IO_UART_DEBUGCONSOLE,
|
||||
[HWTINYFISHSETTINGS_UART3PORT_COMBRIDGE] = PIOS_BOARD_IO_UART_COMBRIDGE,
|
||||
[HWTINYFISHSETTINGS_UART3PORT_MSP] = PIOS_BOARD_IO_UART_MSP,
|
||||
[HWTINYFISHSETTINGS_UART3PORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK,
|
||||
[HWTINYFISHSETTINGS_UART3PORT_HOTTTELEMETRY] = PIOS_BOARD_IO_UART_HOTT_BRIDGE,
|
||||
[HWTINYFISHSETTINGS_UART3PORT_FRSKYSENSORHUB] = PIOS_BOARD_IO_UART_FRSKY_SENSORHUB,
|
||||
};
|
||||
|
||||
for (unsigned int i = 0; i < HWTINYFISHSETTINGS_UARTPORT_NUMELEM; ++i) {
|
||||
if (boardHwSettings.UARTPort[i] < NELEMENTS(uart_function_map)) {
|
||||
PIOS_BOARD_IO_Configure_UART(&pios_usart_cfg[i], uart_function_map[boardHwSettings.UARTPort[i]]);
|
||||
}
|
||||
}
|
||||
|
||||
const struct pios_servo_cfg *servo_cfg = (boardHwSettings.UARTPort[2] == HWTINYFISHSETTINGS_UARTPORT_OUTPUTS) ? &pios_servo_uart3_cfg : &pios_servo_cfg;
|
||||
|
||||
if(boardHwSettings.UARTPort[2] == HWTINYFISHSETTINGS_UARTPORT_PPM) {
|
||||
PIOS_BOARD_IO_Configure_PPM_RCVR(&pios_ppm_cfg);
|
||||
if (boardHwSettings.UART3Port < NELEMENTS(uart_function_map)) {
|
||||
PIOS_BOARD_IO_Configure_UART(&pios_usart_cfg[2], uart_function_map[boardHwSettings.UART3Port]);
|
||||
}
|
||||
|
||||
#ifdef PIOS_INCLUDE_PPM
|
||||
PIOS_BOARD_IO_Configure_PPM_RCVR(&pios_ppm_cfg);
|
||||
if (boardHwSettings.UART3Port == HWTINYFISHSETTINGS_UART3PORT_PPM) {
|
||||
PIOS_BOARD_IO_Configure_PPM_RCVR(&pios_ppm_cfg);
|
||||
}
|
||||
#endif
|
||||
if(PIOS_COM_DEBUG) {
|
||||
PIOS_COM_ChangeBaud(PIOS_COM_DEBUG, 57600);
|
||||
}
|
||||
|
||||
/* Configure integrated RX protocols only if we did not already configure those (SBus rx/frsky telemetry) on external port
|
||||
* Note: We will first configure FRSKY_SENSORHUB on this port. That module is TX only and will set
|
||||
* baud rate to 9600. However, we will configure SBUS after that, which will reconfigure COM port there.
|
||||
*/
|
||||
PIOS_COM_SendFormattedString(PIOS_COM_DEBUG, "boardHwSettings.UART3Port = %d\r\n", boardHwSettings.UART3Port);
|
||||
PIOS_COM_SendFormattedString(PIOS_COM_DEBUG, "Another one\r\n");
|
||||
if (boardHwSettings.UART3Port != HWTINYFISHSETTINGS_UART3PORT_FRSKYSENSORHUB) {
|
||||
PIOS_COM_SendFormattedString(PIOS_COM_DEBUG, "initializing PIOS_BOARD_IO_UART_FRSKY_SENSORHUB\r\n");
|
||||
PIOS_BOARD_IO_Configure_UART(&pios_usart_cfg[1], PIOS_BOARD_IO_UART_FRSKY_SENSORHUB);
|
||||
PIOS_COM_SendFormattedString(PIOS_COM_DEBUG, "done: PIOS_COM_FRSKY_SENSORHUB = 0x%08x\r\n", PIOS_COM_FRSKY_SENSORHUB);
|
||||
}
|
||||
|
||||
#ifndef PIOS_ENABLE_DEBUG_PINS
|
||||
PIOS_Servo_Init(servo_cfg);
|
||||
#else
|
||||
if (boardHwSettings.UART3Port != HWTINYFISHSETTINGS_UART3PORT_SBUS) {
|
||||
PIOS_COM_SendFormattedString(PIOS_COM_DEBUG, "initializing PIOS_BOARD_IO_UART_SBUS_NOT_INVERTED\r\n");
|
||||
PIOS_BOARD_IO_Configure_UART(&pios_usart_cfg[1], PIOS_BOARD_IO_UART_SBUS_NOT_INVERTED);
|
||||
}
|
||||
PIOS_COM_SendFormattedString(PIOS_COM_DEBUG, "Done with UART2\r\n");
|
||||
/* and what to do with UART1? There is RX_DEBUG only connected. Maybe make it into configurable ComBridge? */
|
||||
if (boardHwSettings.UART1Port == HWTINYFISHSETTINGS_UART1PORT_COMBRIDGE) {
|
||||
PIOS_BOARD_IO_Configure_UART(&pios_usart_cfg[0], PIOS_BOARD_IO_UART_COMBRIDGE);
|
||||
}
|
||||
|
||||
#ifdef PIOS_ENABLE_DEBUG_PINS
|
||||
PIOS_DEBUG_Init(&pios_servo_cfg.channels, pios_servo_cfg.num_channels);
|
||||
#endif
|
||||
#else
|
||||
if (boardHwSettings.UART3Port == HWTINYFISHSETTINGS_UART3PORT_OUTPUTS) {
|
||||
PIOS_Servo_Init(&pios_servo_uart3_cfg);
|
||||
} else {
|
||||
PIOS_Servo_Init(&pios_servo_cfg);
|
||||
}
|
||||
#endif /* PIOS_ENABLE_DEBUG_PINS */
|
||||
|
||||
switch (boardHwSettings.LEDPort) {
|
||||
case HWTINYFISHSETTINGS_LEDPORT_WS2811:
|
||||
@ -249,9 +274,6 @@ void PIOS_Board_Init(void)
|
||||
PIOS_BOARD_Sensors_Configure();
|
||||
|
||||
PIOS_LED_On(PIOS_LED_HEARTBEAT);
|
||||
|
||||
/* Make sure we have at least one telemetry link configured or else fail initialization */
|
||||
// PIOS_Assert(pios_com_telem_rf_id || pios_com_telem_usb_id);
|
||||
}
|
||||
|
||||
|
||||
|
@ -113,7 +113,7 @@ extern uint32_t pios_ws2811_id;
|
||||
#define PIOS_DMA_PIN_CONFIG \
|
||||
{ \
|
||||
{ GPIOB, GPIO_Pin_0, ADC_Channel_12, false }, /* ADC_2 / Current */ \
|
||||
{ GPIOB, GPIO_Pin_1, ADC_Channel_1, false }, /* ADC_1 / Voltage */ \
|
||||
{ GPIOB, GPIO_Pin_1, ADC_Channel_1, false }, /* ADC_1 / Voltage */ \
|
||||
}
|
||||
|
||||
/* we have to do all this to satisfy the PIOS_ADC_MAX_SAMPLES define in pios_adc.h */
|
||||
@ -125,6 +125,9 @@ extern uint32_t pios_ws2811_id;
|
||||
// #define PIOS_ADC_USE_TEMP_SENSOR
|
||||
// #define PIOS_ADC_TEMPERATURE_PIN 4
|
||||
|
||||
#define PIOS_ADC_CURRENT_PIN 0
|
||||
#define PIOS_ADC_VOLTAGE_PIN 1
|
||||
|
||||
// ------------------------
|
||||
// PIOS_RCVR
|
||||
// See also pios_board.c
|
||||
|
@ -1,7 +1,8 @@
|
||||
<xml>
|
||||
<object name="HwTinyFISHSettings" singleinstance="true" settings="true" category="System">
|
||||
<description>fishpepper.de tinyFISH Flight Controller hardware configuration</description>
|
||||
<field name="UARTPort" units="function" type="enum" elements="3" options="Disabled,Telemetry,GPS,S.Bus,DSM,EX.Bus,HoTT SUMD,HoTT SUMH,SRXL,IBus,DebugConsole,ComBridge,MSP,MAVLink,PPM,Outputs" defaultvalue="Disabled" limits="%NE:PPM,Outputs;NE:PPM,Outputs;"/>
|
||||
<field name="UART1Port" units="function" type="enum" elements="1" options="Disabled,ComBridge" defaultvalue="Disabled"/>
|
||||
<field name="UART3Port" units="function" type="enum" elements="1" options="Disabled,Telemetry,GPS,S.Bus,DSM,EX.Bus,HoTT SUMD,HoTT SUMH,SRXL,IBus,DebugConsole,ComBridge,MSP,MAVLink,HoTT Telemetry,FrskySensorHub,PPM,Outputs" defaultvalue="Disabled"/>
|
||||
<field name="LEDPort" units="function" type="enum" elements="1" options="Disabled,WS2811" defaultvalue="Disabled"/>
|
||||
<field name="BuzzerPort" units="function" type="enum" elements="1" options="Disabled,Enabled" defaultvalue="Disabled"/>
|
||||
|
||||
|
Loading…
x
Reference in New Issue
Block a user