diff --git a/flight/libraries/auxmagsupport.c b/flight/libraries/auxmagsupport.c index d8011fae4..1c6239e50 100644 --- a/flight/libraries/auxmagsupport.c +++ b/flight/libraries/auxmagsupport.c @@ -27,9 +27,7 @@ #include #include "inc/auxmagsupport.h" #include "CoordinateConversions.h" -#if defined(PIOS_INCLUDE_HMC5X83) -#include "pios_hmc5x83.h" -#endif +#include "pios_hmc5x83.h" // this is needed for mag orientation even for other mag types #define assumptions \ ( \ diff --git a/flight/modules/System/systemmod.c b/flight/modules/System/systemmod.c index 8c94ec1b1..cf8e36225 100644 --- a/flight/modules/System/systemmod.c +++ b/flight/modules/System/systemmod.c @@ -183,6 +183,10 @@ static void systemTask(__attribute__((unused)) void *parameters) vTaskDelay(10); } +#ifndef PIOS_INCLUDE_WDG +// if no watchdog is enabled, don't reset watchdog in MODULE_TASKCREATE_ALL loop +#define PIOS_WDG_Clear() +#endif /* create all modules thread */ MODULE_TASKCREATE_ALL; diff --git a/flight/pios/inc/pios_initcall.h b/flight/pios/inc/pios_initcall.h index 52669e658..4f014f93b 100644 --- a/flight/pios/inc/pios_initcall.h +++ b/flight/pios/inc/pios_initcall.h @@ -92,7 +92,6 @@ extern void StartModules(); #define MODULE_INITCALL(ifn, sfn) __define_module_initcall("module", ifn, sfn) -#if 0 #define MODULE_INITIALISE_ALL \ { for (initmodule_t *fn = __module_initcall_start; fn < __module_initcall_end; fn++) { \ if (fn->fn_minit) { \ @@ -100,24 +99,7 @@ extern void StartModules(); } \ initTaskDone = 1; \ } -#else -#define MODULE_INITIALISE_ALL \ - { for (initmodule_t *fn = __module_initcall_start; fn < __module_initcall_end; fn++) { \ - if (fn->fn_minit) { \ - (fn->fn_minit)(); } \ - } \ - initTaskDone = 1; \ - } -#endif -#if 0 -#define MODULE_TASKCREATE_ALL \ - { for (initmodule_t *fn = __module_initcall_start; fn < __module_initcall_end; fn++) { \ - if (fn->fn_tinit) { \ - (fn->fn_tinit)(); } \ - } \ - } -#else #define MODULE_TASKCREATE_ALL \ { for (initmodule_t *fn = __module_initcall_start; fn < __module_initcall_end; fn++) { \ if (fn->fn_tinit) { \ @@ -125,7 +107,6 @@ extern void StartModules(); PIOS_WDG_Clear(); \ } \ } } -#endif #endif /* USE_SIM_POSIX */ diff --git a/flight/pios/stm32f10x/pios_i2c.c b/flight/pios/stm32f10x/pios_i2c.c index d68f5d071..77f150ac0 100644 --- a/flight/pios/stm32f10x/pios_i2c.c +++ b/flight/pios/stm32f10x/pios_i2c.c @@ -711,6 +711,12 @@ static bool i2c_adapter_wait_for_stopped(struct pios_i2c_adapter *i2c_adapter) static void i2c_adapter_reset_bus(struct pios_i2c_adapter *i2c_adapter) { + // retry with wait code from + // TauLabs 20150718 - Prevent F3 I2C Init Lockup #1728 + uint8_t retry_count; + uint8_t retry_count_clk; + static const uint8_t MAX_I2C_RETRY_COUNT = 10; + /* Reset the I2C block */ I2C_DeInit(i2c_adapter->cfg->regs); @@ -731,11 +737,13 @@ static void i2c_adapter_reset_bus(struct pios_i2c_adapter *i2c_adapter) /* have to be repeated (due to futher bus errors) but better than clocking 0xFF into an */ /* ESC */ // bool sda_hung = GPIO_ReadInputDataBit(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin) == Bit_RESET; - while (GPIO_ReadInputDataBit(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin) == Bit_RESET) { + retry_count_clk = 0; + while (GPIO_ReadInputDataBit(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin) == Bit_RESET && (retry_count_clk++ < MAX_I2C_RETRY_COUNT)) { + retry_count = 0; /* Set clock high and wait for any clock stretching to finish. */ GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); - while (GPIO_ReadInputDataBit(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin) == Bit_RESET) { - ; + while (GPIO_ReadInputDataBit(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin) == Bit_RESET && (retry_count++ < MAX_I2C_RETRY_COUNT)) { + PIOS_DELAY_WaituS(1); } PIOS_DELAY_WaituS(2); @@ -759,12 +767,14 @@ static void i2c_adapter_reset_bus(struct pios_i2c_adapter *i2c_adapter) /* Set data and clock high and wait for any clock stretching to finish. */ GPIO_SetBits(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin); GPIO_SetBits(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin); - while (GPIO_ReadInputDataBit(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin) == Bit_RESET) { - ; + retry_count = 0; + while (GPIO_ReadInputDataBit(i2c_adapter->cfg->scl.gpio, i2c_adapter->cfg->scl.init.GPIO_Pin) == Bit_RESET && (retry_count++ < MAX_I2C_RETRY_COUNT)) { + PIOS_DELAY_WaituS(1); } /* Wait for data to be high */ - while (GPIO_ReadInputDataBit(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin) != Bit_SET) { - ; + retry_count = 0; + while (GPIO_ReadInputDataBit(i2c_adapter->cfg->sda.gpio, i2c_adapter->cfg->sda.init.GPIO_Pin) != Bit_SET && (retry_count++ < MAX_I2C_RETRY_COUNT)) { + PIOS_DELAY_WaituS(1); } diff --git a/flight/targets/boards/coptercontrol/firmware/pios_board.c b/flight/targets/boards/coptercontrol/firmware/pios_board.c index 659980639..9d92f6e83 100644 --- a/flight/targets/boards/coptercontrol/firmware/pios_board.c +++ b/flight/targets/boards/coptercontrol/firmware/pios_board.c @@ -36,6 +36,7 @@ #include #include #include +#include #ifdef PIOS_INCLUDE_INSTRUMENTATION #include @@ -54,6 +55,24 @@ */ #include "../board_hw_defs.c" +#if defined(PIOS_INCLUDE_HMC5X83) +#include "pios_hmc5x83.h" +pios_hmc5x83_dev_t external_mag = 0; + +static const struct pios_hmc5x83_cfg pios_hmc5x83_external_cfg = { +#ifdef PIOS_HMC5X83_HAS_GPIOS + .exti_cfg = NULL, +#endif + .M_ODR = PIOS_HMC5x83_ODR_75, // if you change this for auxmag, change AUX_MAG_SKIP in sensors.c + .Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL, + .Gain = PIOS_HMC5x83_GAIN_1_9, + .Mode = PIOS_HMC5x83_MODE_CONTINUOUS, + .TempCompensation = false, + .Driver = &PIOS_HMC5x83_I2C_DRIVER, + .Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP, +}; +#endif /* PIOS_INCLUDE_HMC5X83 */ + /* One slot per selectable receiver group. * eg. PWM, PPM, GCS, DSMMAINPORT, DSMFLEXIPORT, SBUS * NOTE: No slot in this map for NONE. @@ -244,8 +263,13 @@ void PIOS_Board_Init(void) #ifndef ERASE_FLASH #ifdef PIOS_INCLUDE_WDG - /* Initialize watchdog as early as possible to catch faults during init */ - PIOS_WDG_Init(); + /* From TauLabs + * Initialize watchdog as early as possible to catch faults during init + * but do it only if there is no debugger connected + */ + if ((CoreDebug->DHCSR & CoreDebug_DHCSR_C_DEBUGEN_Msk) == 0) { + PIOS_WDG_Init(); + } #endif #endif @@ -718,11 +742,25 @@ void PIOS_Board_Init(void) break; case HWSETTINGS_CC_FLEXIPORT_I2C: #if defined(PIOS_INCLUDE_I2C) - { - if (PIOS_I2C_Init(&pios_i2c_flexi_adapter_id, &pios_i2c_flexi_adapter_cfg)) { - PIOS_Assert(0); - } + if (PIOS_I2C_Init(&pios_i2c_flexiport_adapter_id, &pios_i2c_flexiport_adapter_cfg)) { + PIOS_Assert(0); } + PIOS_DELAY_WaitmS(50); // this was after the other PIOS_I2C_Init() on Revo, so I copied it here too +#if defined(PIOS_INCLUDE_HMC5X83) + // get auxmag type + AuxMagSettingsTypeOptions option; + AuxMagSettingsInitialize(); + AuxMagSettingsTypeGet(&option); + // if the aux mag type is FlexiPort then set it up + if (option == AUXMAGSETTINGS_TYPE_FLEXI) { + // attach the 5x83 mag to the previously inited I2C2 + external_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_external_cfg, pios_i2c_flexiport_adapter_id, 0); + // add this sensor to the sensor task's list + PIOS_HMC5x83_Register(external_mag, PIOS_SENSORS_TYPE_3AXIS_AUXMAG); + // mag alarm is cleared later, so use I2C + AlarmsSet(SYSTEMALARMS_ALARM_I2C, (external_mag)?SYSTEMALARMS_ALARM_OK:SYSTEMALARMS_ALARM_WARNING); + } +#endif /* PIOS_INCLUDE_HMC5X83 */ #endif /* PIOS_INCLUDE_I2C */ break; case HWSETTINGS_CC_FLEXIPORT_OSDHK: diff --git a/flight/targets/boards/discoveryf4bare/firmware/inc/pios_config.h b/flight/targets/boards/discoveryf4bare/firmware/inc/pios_config.h index 2bc9c11b9..a551f858f 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/inc/pios_config.h +++ b/flight/targets/boards/discoveryf4bare/firmware/inc/pios_config.h @@ -84,7 +84,7 @@ // #define PIOS_INCLUDE_MPU6000 // #define PIOS_MPU6000_ACCEL /* #define PIOS_INCLUDE_HMC5843 */ -// #define PIOS_INCLUDE_HMC5X83 +#define PIOS_INCLUDE_HMC5X83 // #define PIOS_HMC5X83_HAS_GPIOS /* #define PIOS_INCLUDE_BMP085 */ // #define PIOS_INCLUDE_MS5611 diff --git a/flight/targets/boards/discoveryf4bare/firmware/pios_board.c b/flight/targets/boards/discoveryf4bare/firmware/pios_board.c index bb9f6c33d..108fb340e 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/pios_board.c +++ b/flight/targets/boards/discoveryf4bare/firmware/pios_board.c @@ -36,6 +36,7 @@ #include #include #include +#include #ifdef PIOS_INCLUDE_INSTRUMENTATION #include @@ -92,8 +93,16 @@ void PIOS_ADC_DMC_irq_handler(void) #if defined(PIOS_INCLUDE_HMC5X83) #include "pios_hmc5x83.h" +pios_hmc5x83_dev_t onboard_mag = 0; +pios_hmc5x83_dev_t external_mag = 0; + +bool pios_board_internal_mag_handler() +{ + return PIOS_HMC5x83_IRQHandler(onboard_mag); +} + static const struct pios_exti_cfg pios_exti_hmc5x83_cfg __exti_config = { - .vector = PIOS_HMC5x83_IRQHandler, + .vector = pios_board_internal_mag_handler, .line = EXTI_Line7, .pin = { .gpio = GPIOB, @@ -123,12 +132,28 @@ static const struct pios_exti_cfg pios_exti_hmc5x83_cfg __exti_config = { }, }; -static const struct pios_hmc5x83_cfg pios_hmc5x83_cfg = { +static const struct pios_hmc5x83_cfg pios_hmc5x83_internal_cfg = { +#ifdef PIOS_HMC5X83_HAS_GPIOS .exti_cfg = &pios_exti_hmc5x83_cfg, +#endif .M_ODR = PIOS_HMC5x83_ODR_75, .Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL, .Gain = PIOS_HMC5x83_GAIN_1_9, .Mode = PIOS_HMC5x83_MODE_CONTINUOUS, + .TempCompensation = false, + .Driver = &PIOS_HMC5x83_I2C_DRIVER, + .Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP, +}; + +static const struct pios_hmc5x83_cfg pios_hmc5x83_external_cfg = { +#ifdef PIOS_HMC5X83_HAS_GPIOS + .exti_cfg = NULL, +#endif + .M_ODR = PIOS_HMC5x83_ODR_75, // if you change this for auxmag, change AUX_MAG_SKIP in sensors.c + .Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL, + .Gain = PIOS_HMC5x83_GAIN_1_9, + .Mode = PIOS_HMC5x83_MODE_CONTINUOUS, + .TempCompensation = false, .Driver = &PIOS_HMC5x83_I2C_DRIVER, .Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP, }; @@ -397,7 +422,13 @@ void PIOS_Board_Init(void) PIOS_IAP_WriteBootCmd(2, 0); } #ifdef PIOS_INCLUDE_WDG - PIOS_WDG_Init(); + /* From TauLabs + * Initialize watchdog as early as possible to catch faults during init + * but do it only if there is no debugger connected + */ + if ((CoreDebug->DHCSR & CoreDebug_DHCSR_C_DEBUGEN_Msk) == 0) { + PIOS_WDG_Init(); + } #endif /* Initialize the task monitor */ @@ -654,11 +685,25 @@ void PIOS_Board_Init(void) break; case HWSETTINGS_RM_FLEXIPORT_I2C: #if defined(PIOS_INCLUDE_I2C) - { - if (PIOS_I2C_Init(&pios_i2c_flexiport_adapter_id, &pios_i2c_flexiport_adapter_cfg)) { - PIOS_Assert(0); - } + if (PIOS_I2C_Init(&pios_i2c_flexiport_adapter_id, &pios_i2c_flexiport_adapter_cfg)) { + PIOS_Assert(0); } + PIOS_DELAY_WaitmS(50); // this was after the other PIOS_I2C_Init(), so I copied it here too +#if defined(PIOS_INCLUDE_HMC5X83) + // get auxmag type + AuxMagSettingsTypeOptions option; + AuxMagSettingsInitialize(); + AuxMagSettingsTypeGet(&option); + // if the aux mag type is FlexiPort then set it up + if (option == AUXMAGSETTINGS_TYPE_FLEXI) { + // attach the 5x83 mag to the previously inited I2C2 + external_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_external_cfg, pios_i2c_flexiport_adapter_id, 0); + // add this sensor to the sensor task's list + PIOS_HMC5x83_Register(external_mag, PIOS_SENSORS_TYPE_3AXIS_AUXMAG); + // mag alarm is cleared later, so use I2C + AlarmsSet(SYSTEMALARMS_ALARM_I2C, (external_mag)?SYSTEMALARMS_ALARM_OK:SYSTEMALARMS_ALARM_WARNING); + } +#endif /* PIOS_INCLUDE_HMC5X83 */ #endif /* PIOS_INCLUDE_I2C */ break; case HWSETTINGS_RM_FLEXIPORT_GPS: @@ -882,6 +927,7 @@ void PIOS_Board_Init(void) }; GPIO_Init(GPIOA, &gpioA8); + // init I2C1 for use with the internal mag and baro if (PIOS_I2C_Init(&pios_i2c_mag_pressure_adapter_id, &pios_i2c_mag_pressure_adapter_cfg)) { PIOS_DEBUG_Assert(0); } @@ -893,7 +939,10 @@ void PIOS_Board_Init(void) #endif #if defined(PIOS_INCLUDE_HMC5X83) - PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, pios_i2c_mag_pressure_adapter_id, 0); + // attach the 5x83 mag to the previously inited I2C1 + onboard_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_internal_cfg, pios_i2c_mag_pressure_adapter_id, 0); + // add this sensor to the sensor task's list + PIOS_HMC5x83_Register(onboard_mag, PIOS_SENSORS_TYPE_3AXIS_MAG); #endif #if defined(PIOS_INCLUDE_MS5611) diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index 2d339d223..660e0d194 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -38,7 +38,6 @@ #include #include - #ifdef PIOS_INCLUDE_INSTRUMENTATION #include #endif @@ -1024,4 +1023,3 @@ void PIOS_Board_Init(void) * @} * @} */ - diff --git a/flight/targets/boards/revolution/firmware/revolution.cpp b/flight/targets/boards/revolution/firmware/revolution.cpp index be85f08c4..268325b26 100644 --- a/flight/targets/boards/revolution/firmware/revolution.cpp +++ b/flight/targets/boards/revolution/firmware/revolution.cpp @@ -128,16 +128,7 @@ void initTask(__attribute__((unused)) void *parameters) PIOS_Board_Init(); /* Initialize modules */ -#if 1 MODULE_INITIALISE_ALL; -#else - for (initmodule_t *fn = __module_initcall_start; fn < __module_initcall_end; fn++) { - if (fn->fn_minit) { - (fn->fn_minit)(); - } - } - initTaskDone = 1; -#endif /* terminate this task */ vTaskDelete(NULL); diff --git a/flight/targets/boards/revonano/firmware/inc/pios_config.h b/flight/targets/boards/revonano/firmware/inc/pios_config.h index 16803c751..e14b9aaf9 100644 --- a/flight/targets/boards/revonano/firmware/inc/pios_config.h +++ b/flight/targets/boards/revonano/firmware/inc/pios_config.h @@ -84,7 +84,7 @@ // #define PIOS_INCLUDE_MPU6000 // #define PIOS_MPU6000_ACCEL /* #define PIOS_INCLUDE_HMC5843 */ -// #define PIOS_INCLUDE_HMC5X83 +#define PIOS_INCLUDE_HMC5X83 // #define PIOS_HMC5X83_HAS_GPIOS /* #define PIOS_INCLUDE_BMP085 */ #define PIOS_INCLUDE_MS5611 diff --git a/flight/targets/boards/revonano/firmware/pios_board.c b/flight/targets/boards/revonano/firmware/pios_board.c index dcf373a51..001fd37cd 100644 --- a/flight/targets/boards/revonano/firmware/pios_board.c +++ b/flight/targets/boards/revonano/firmware/pios_board.c @@ -38,6 +38,7 @@ #include #include #include +#include #ifdef PIOS_INCLUDE_INSTRUMENTATION #include @@ -92,9 +93,26 @@ void PIOS_ADC_DMC_irq_handler(void) /* Call into the generic code to handle the IRQ for this specific device */ PIOS_ADC_DMA_Handler(); } - #endif /* if defined(PIOS_INCLUDE_ADC) */ +#if defined(PIOS_INCLUDE_HMC5X83) +#include "pios_hmc5x83.h" +pios_hmc5x83_dev_t external_mag = 0; + +static const struct pios_hmc5x83_cfg pios_hmc5x83_external_cfg = { +#ifdef PIOS_HMC5X83_HAS_GPIOS + .exti_cfg = NULL, +#endif + .M_ODR = PIOS_HMC5x83_ODR_75, // if you change this for auxmag, change AUX_MAG_SKIP in sensors.c + .Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL, + .Gain = PIOS_HMC5x83_GAIN_1_9, + .Mode = PIOS_HMC5x83_MODE_CONTINUOUS, + .TempCompensation = false, + .Driver = &PIOS_HMC5x83_I2C_DRIVER, + .Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP, +}; +#endif /* PIOS_INCLUDE_HMC5X83 */ + /** * Configuration for the MS5611 chip */ @@ -360,8 +378,15 @@ void PIOS_Board_Init(void) PIOS_IAP_WriteBootCmd(1, 0); PIOS_IAP_WriteBootCmd(2, 0); } + #ifdef PIOS_INCLUDE_WDG - PIOS_WDG_Init(); + /* From TauLabs + * Initialize watchdog as early as possible to catch faults during init + * but do it only if there is no debugger connected + */ + if ((CoreDebug->DHCSR & CoreDebug_DHCSR_C_DEBUGEN_Msk) == 0) { + PIOS_WDG_Init(); + } #endif /* Initialize the task monitor */ @@ -617,11 +642,25 @@ void PIOS_Board_Init(void) break; case HWSETTINGS_RM_FLEXIPORT_I2C: #if defined(PIOS_INCLUDE_I2C) - { - if (PIOS_I2C_Init(&pios_i2c_flexiport_adapter_id, &pios_i2c_flexiport_adapter_cfg)) { - PIOS_Assert(0); - } + if (PIOS_I2C_Init(&pios_i2c_flexiport_adapter_id, &pios_i2c_flexiport_adapter_cfg)) { + PIOS_Assert(0); } + PIOS_DELAY_WaitmS(50); // this was after the other PIOS_I2C_Init(), so I copied it here too +#if defined(PIOS_INCLUDE_HMC5X83) + // get auxmag type + AuxMagSettingsTypeOptions option; + AuxMagSettingsInitialize(); + AuxMagSettingsTypeGet(&option); + // if the aux mag type is FlexiPort then set it up + if (option == AUXMAGSETTINGS_TYPE_FLEXI) { + // attach the 5x83 mag to the previously inited I2C2 + external_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_external_cfg, pios_i2c_flexiport_adapter_id, 0); + // add this sensor to the sensor task's list + PIOS_HMC5x83_Register(external_mag, PIOS_SENSORS_TYPE_3AXIS_AUXMAG); + // mag alarm is cleared later, so use I2C + AlarmsSet(SYSTEMALARMS_ALARM_I2C, (external_mag)?SYSTEMALARMS_ALARM_OK:SYSTEMALARMS_ALARM_WARNING); + } +#endif /* PIOS_INCLUDE_HMC5X83 */ #endif /* PIOS_INCLUDE_I2C */ break; case HWSETTINGS_RM_FLEXIPORT_GPS: diff --git a/flight/targets/boards/revoproto/firmware/pios_board.c b/flight/targets/boards/revoproto/firmware/pios_board.c index b0c9030ec..297cacefc 100644 --- a/flight/targets/boards/revoproto/firmware/pios_board.c +++ b/flight/targets/boards/revoproto/firmware/pios_board.c @@ -31,6 +31,7 @@ #include #include #include +#include /* * Pull in the board-specific static HW definitions. @@ -85,7 +86,9 @@ void PIOS_ADC_DMC_irq_handler(void) #include "pios_hmc5x83.h" pios_hmc5x83_dev_t onboard_mag = 0; +pios_hmc5x83_dev_t external_mag = 0; +#ifdef PIOS_HMC5X83_HAS_GPIOS bool pios_board_internal_mag_handler() { return PIOS_HMC5x83_IRQHandler(onboard_mag); @@ -120,13 +123,30 @@ static const struct pios_exti_cfg pios_exti_hmc5x83_cfg __exti_config = { }, }, }; +#endif /* PIOS_HMC5X83_HAS_GPIOS */ -static const struct pios_hmc5x83_cfg pios_hmc5x83_cfg = { +static const struct pios_hmc5x83_cfg pios_hmc5x83_internal_cfg = { +#ifdef PIOS_HMC5X83_HAS_GPIOS .exti_cfg = &pios_exti_hmc5x83_cfg, +#endif .M_ODR = PIOS_HMC5x83_ODR_75, .Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL, .Gain = PIOS_HMC5x83_GAIN_1_9, .Mode = PIOS_HMC5x83_MODE_CONTINUOUS, + .TempCompensation = false, + .Driver = &PIOS_HMC5x83_I2C_DRIVER, + .Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP, +}; + +static const struct pios_hmc5x83_cfg pios_hmc5x83_external_cfg = { +#ifdef PIOS_HMC5X83_HAS_GPIOS + .exti_cfg = NULL, +#endif + .M_ODR = PIOS_HMC5x83_ODR_75, // if you change this for auxmag, change AUX_MAG_SKIP in sensors.c + .Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL, + .Gain = PIOS_HMC5x83_GAIN_1_9, + .Mode = PIOS_HMC5x83_MODE_CONTINUOUS, + .TempCompensation = false, .Driver = &PIOS_HMC5x83_I2C_DRIVER, .Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP, }; @@ -753,14 +773,27 @@ void PIOS_Board_Init(void) break; case HWSETTINGS_RV_FLEXIPORT_I2C: #if defined(PIOS_INCLUDE_I2C) - { - if (PIOS_I2C_Init(&pios_i2c_flexiport_adapter_id, &pios_i2c_flexiport_adapter_cfg)) { - PIOS_Assert(0); - } + if (PIOS_I2C_Init(&pios_i2c_flexiport_adapter_id, &pios_i2c_flexiport_adapter_cfg)) { + PIOS_Assert(0); } + PIOS_DELAY_WaitmS(50); // this was after the other PIOS_I2C_Init(), so I copied it here too +#if defined(PIOS_INCLUDE_HMC5X83) + // get auxmag type + AuxMagSettingsTypeOptions option; + AuxMagSettingsInitialize(); + AuxMagSettingsTypeGet(&option); + // if the aux mag type is FlexiPort then set it up + if (option == AUXMAGSETTINGS_TYPE_FLEXI) { + // attach the 5x83 mag to the previously inited I2C2 + external_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_external_cfg, pios_i2c_flexiport_adapter_id, 0); + // add this sensor to the sensor task's list + PIOS_HMC5x83_Register(external_mag, PIOS_SENSORS_TYPE_3AXIS_AUXMAG); + // mag alarm is cleared later, so use I2C + AlarmsSet(SYSTEMALARMS_ALARM_I2C, (external_mag)?SYSTEMALARMS_ALARM_OK:SYSTEMALARMS_ALARM_WARNING); + } +#endif /* PIOS_INCLUDE_HMC5X83 */ #endif /* PIOS_INCLUDE_I2C */ break; - case HWSETTINGS_RV_FLEXIPORT_DSM: // TODO: Define the various Channelgroup for Revo dsm inputs and handle here PIOS_Board_configure_dsm(&pios_usart_dsm_flexi_cfg, &pios_dsm_flexi_cfg, @@ -879,10 +912,12 @@ void PIOS_Board_Init(void) PIOS_DEBUG_Init(pios_tim_servoport_all_pins, NELEMENTS(pios_tim_servoport_all_pins)); #endif + // init I2C1 for use with the internal mag if (PIOS_I2C_Init(&pios_i2c_mag_adapter_id, &pios_i2c_mag_adapter_cfg)) { PIOS_DEBUG_Assert(0); } + // init I2C1 for use with the internal baro if (PIOS_I2C_Init(&pios_i2c_pressure_adapter_id, &pios_i2c_pressure_adapter_cfg)) { PIOS_DEBUG_Assert(0); } @@ -894,8 +929,10 @@ void PIOS_Board_Init(void) #endif #if defined(PIOS_INCLUDE_HMC5X83) - onboard_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, pios_i2c_mag_adapter_id, 0); - PIOS_HMC5x83_Register(onboard_mag); + // attach the 5x83 mag to the previously inited I2C1 + onboard_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_internal_cfg, pios_i2c_mag_adapter_id, 0); + // add this sensor to the sensor task's list + PIOS_HMC5x83_Register(onboard_mag, PIOS_SENSORS_TYPE_3AXIS_MAG); #endif #if defined(PIOS_INCLUDE_MS5611)