From 7de55812dd1636dd84a88baef18956d5d8ded1df Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Sat, 19 Sep 2015 01:04:12 -0400 Subject: [PATCH] LP-73 changes for pull request comments --- flight/libraries/auxmagsupport.c | 46 +++--- flight/modules/Sensors/sensors.c | 144 +++++------------- flight/pios/common/pios_hmc5x83.c | 27 +--- flight/pios/inc/pios_hmc5x83.h | 4 - .../discoveryf4bare/firmware/pios_board.c | 13 +- .../boards/revolution/firmware/pios_board.c | 13 +- .../boards/revonano/firmware/pios_board.c | 13 +- shared/uavobjectdefinition/auxmagsettings.xml | 4 +- 8 files changed, 80 insertions(+), 184 deletions(-) diff --git a/flight/libraries/auxmagsupport.c b/flight/libraries/auxmagsupport.c index 1c6239e50..44f5bdb65 100644 --- a/flight/libraries/auxmagsupport.c +++ b/flight/libraries/auxmagsupport.c @@ -29,27 +29,6 @@ #include "CoordinateConversions.h" #include "pios_hmc5x83.h" // this is needed for mag orientation even for other mag types -#define assumptions \ - ( \ - ((int) PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP == \ - (int) AUXMAGSETTINGS_ORIENTATION_EAST_NORTH_UP) && \ - ((int) PIOS_HMC5X83_ORIENTATION_SOUTH_EAST_UP == \ - (int) AUXMAGSETTINGS_ORIENTATION_SOUTH_EAST_UP) && \ - ((int) PIOS_HMC5X83_ORIENTATION_WEST_SOUTH_UP == \ - (int) AUXMAGSETTINGS_ORIENTATION_WEST_SOUTH_UP) && \ - ((int) PIOS_HMC5X83_ORIENTATION_NORTH_WEST_UP == \ - (int) AUXMAGSETTINGS_ORIENTATION_NORTH_WEST_UP) && \ - ((int) PIOS_HMC5X83_ORIENTATION_EAST_SOUTH_DOWN == \ - (int) AUXMAGSETTINGS_ORIENTATION_EAST_SOUTH_DOWN) && \ - ((int) PIOS_HMC5X83_ORIENTATION_SOUTH_WEST_DOWN == \ - (int) AUXMAGSETTINGS_ORIENTATION_SOUTH_WEST_DOWN) && \ - ((int) PIOS_HMC5X83_ORIENTATION_WEST_NORTH_DOWN == \ - (int) AUXMAGSETTINGS_ORIENTATION_WEST_NORTH_DOWN) && \ - ((int) PIOS_HMC5X83_ORIENTATION_NORTH_EAST_DOWN == \ - (int) AUXMAGSETTINGS_ORIENTATION_NORTH_EAST_DOWN) && \ - ((int) PIOS_HMC5X83_ORIENTATION_UNCHANGED == \ - (int) AUXMAGSETTINGS_ORIENTATION_UNCHANGED) ) - static float mag_bias[3] = { 0, 0, 0 }; static float mag_transform[3][3] = { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } @@ -59,13 +38,26 @@ AuxMagSettingsTypeOptions option; void auxmagsupport_reload_settings() { + AuxMagSettingsData cal; + float magQuat[4]; + float R[3][3]; + + AuxMagSettingsGet(&cal); + mag_bias[0] = cal.mag_bias.X; + mag_bias[1] = cal.mag_bias.Y; + mag_bias[2] = cal.mag_bias.Z; + + // convert the RPY mag board rotation to into a rotation matrix + // rotate the vector into the level hover frame (the attitude frame) + const float magRpy[3] = { cal.BoardRotation.Roll, cal.BoardRotation.Pitch, cal.BoardRotation.Yaw }; + RPY2Quaternion(magRpy, magQuat); + Quaternion2R(magQuat, R); + + // the mag transform only scales the raw mag values + matrix_mult_3x3f((float(*)[3])AuxMagSettingsmag_transformToArray(cal.mag_transform), R, mag_transform); + + // GPSV9, Ext (unused), and Flexi AuxMagSettingsTypeGet(&option); - AuxMagSettingsmag_transformArrayGet((float *)mag_transform); - AuxMagSettingsOrientationOptions orientation; - AuxMagSettingsOrientationGet(&orientation); - PIOS_STATIC_ASSERT(assumptions); - PIOS_HMC5x83_Ext_Orientation_Set((enum PIOS_HMC5X83_ORIENTATION) orientation); - AuxMagSettingsmag_biasArrayGet(mag_bias); } void auxmagsupport_publish_samples(float mags[3], uint8_t status) diff --git a/flight/modules/Sensors/sensors.c b/flight/modules/Sensors/sensors.c index 5b4b1f1cc..b5f73c21f 100644 --- a/flight/modules/Sensors/sensors.c +++ b/flight/modules/Sensors/sensors.c @@ -85,9 +85,6 @@ #endif static const TickType_t sensor_period_ticks = ((uint32_t) (1000.0f / PIOS_SENSOR_RATE / (float) portTICK_RATE_MS)); -#define AUX_MAG_SKIP ((int) ((((PIOS_SENSOR_RATE < 76) ? 76 : PIOS_SENSOR_RATE) + 74) / 75)) /* (AMS at least 2) 75 is mag ODR output data rate in pios_board.c */ - -#define AUX_MAG_LOCAL_SUPPORT // Interval in number of sample to recalculate temp bias #define TEMP_CALIB_INTERVAL 30 @@ -155,9 +152,6 @@ static void updateBaroTempBias(float temperature); static sensor_data *source_data; static xTaskHandle sensorsTaskHandle; RevoCalibrationData cal; -#ifdef AUX_MAG_LOCAL_SUPPORT -AuxMagSettingsData auxmagcal; -#endif AccelGyroSettingsData agcal; // These values are initialized by settings but can be updated by the attitude algorithm @@ -165,12 +159,6 @@ static float mag_bias[3] = { 0, 0, 0 }; static float mag_transform[3][3] = { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } }; -#ifdef AUX_MAG_LOCAL_SUPPORT -static float auxmag_bias[3] = { 0, 0, 0 }; -static float auxmag_transform[3][3] = { - { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } -}; -#endif // Variables used to handle accel/gyro temperature bias static volatile bool gyro_temp_calibrated = false; @@ -196,10 +184,6 @@ static float baro_temp_bias = 0; static float baro_temperature = NAN; static uint8_t baro_temp_calibration_count = 0; -// this is set, but not used -// it was intended to be a flag to avoid rotation calculation if the rotation was zero -static int8_t rotate = 0; - /** * Initialise the module. Called before the start function * \returns 0 on success or -1 if initialisation failed @@ -212,20 +196,16 @@ int32_t SensorsInitialize(void) MagSensorInitialize(); BaroSensorInitialize(); RevoCalibrationInitialize(); -#ifdef AUX_MAG_LOCAL_SUPPORT - AuxMagSettingsInitialize(); -#endif RevoSettingsInitialize(); AttitudeSettingsInitialize(); AccelGyroSettingsInitialize(); - rotate = 0; + // for auxmagsupport.c helpers + AuxMagSettingsInitialize(); + AuxMagSensorInitialize(); RevoSettingsConnectCallback(&settingsUpdatedCb); RevoCalibrationConnectCallback(&settingsUpdatedCb); -#ifdef AUX_MAG_LOCAL_SUPPORT - AuxMagSettingsConnectCallback(&settingsUpdatedCb); -#endif AttitudeSettingsConnectCallback(&settingsUpdatedCb); AccelGyroSettingsConnectCallback(&settingsUpdatedCb); @@ -267,7 +247,6 @@ static void SensorsTask(__attribute__((unused)) void *parameters) bool error = false; const PIOS_SENSORS_Instance *sensors_list = PIOS_SENSORS_GetList(); PIOS_SENSORS_Instance *sensor; - uint8_t aux_mag_skip = 0; AlarmsClear(SYSTEMALARMS_ALARM_SENSORS); settingsUpdatedCb(NULL); @@ -317,39 +296,35 @@ static void SensorsTask(__attribute__((unused)) void *parameters) // reset the fetch context clearContext(&sensor_context); - aux_mag_skip = (aux_mag_skip + 1) % AUX_MAG_SKIP; LL_FOREACH((PIOS_SENSORS_Instance *)sensors_list, sensor) { // we will wait on the sensor that's marked as primary( that means the sensor with higher sample rate) bool is_primary = (sensor->type & PIOS_SENSORS_TYPE_3AXIS_ACCEL); - - if (sensor->type != PIOS_SENSORS_TYPE_3AXIS_AUXMAG || aux_mag_skip == 0) { - if (!sensor->driver->is_polled) { - const QueueHandle_t queue = PIOS_SENSORS_GetQueue(sensor); - while (xQueueReceive(queue, - (void *)source_data, - (is_primary && !sensor_context.count) ? sensor_period_ticks : 0) == pdTRUE) { + if (!sensor->driver->is_polled) { + const QueueHandle_t queue = PIOS_SENSORS_GetQueue(sensor); + while (xQueueReceive(queue, + (void *)source_data, + (is_primary && !sensor_context.count) ? sensor_period_ticks : 0) == pdTRUE) { + accumulateSamples(&sensor_context, source_data); + } + if (sensor_context.count) { + processSamples3d(&sensor_context, sensor); + clearContext(&sensor_context); + } else if (is_primary) { + PIOS_SENSOR_Reset(sensor); + reset_counter++; + PERF_TRACK_VALUE(counterSensorResets, reset_counter); + error = true; + } + } else { + if (PIOS_SENSORS_Poll(sensor)) { + PIOS_SENSOR_Fetch(sensor, (void *)source_data, MAX_SENSORS_PER_INSTANCE); + if (sensor->type & PIOS_SENSORS_TYPE_3D) { accumulateSamples(&sensor_context, source_data); - } - if (sensor_context.count) { processSamples3d(&sensor_context, sensor); - clearContext(&sensor_context); - } else if (is_primary) { - PIOS_SENSOR_Reset(sensor); - reset_counter++; - PERF_TRACK_VALUE(counterSensorResets, reset_counter); - error = true; - } - } else { - if (PIOS_SENSORS_Poll(sensor)) { - PIOS_SENSOR_Fetch(sensor, (void *)source_data, MAX_SENSORS_PER_INSTANCE); - if (sensor->type & PIOS_SENSORS_TYPE_3D) { - accumulateSamples(&sensor_context, source_data); - processSamples3d(&sensor_context, sensor); - } else { - processSamples1d(&source_data->sensorSample1Axis, sensor); - } - clearContext(&sensor_context); + } else { + processSamples1d(&source_data->sensorSample1Axis, sensor); } + clearContext(&sensor_context); } } } @@ -497,23 +472,7 @@ static void handleMag(float *samples, float temperature) static void handleAuxMag(float *samples) { -#ifdef AUX_MAG_LOCAL_SUPPORT - AuxMagSensorData mag; - float mags[3] = { (float)samples[0] - auxmag_bias[0], - (float)samples[1] - auxmag_bias[1], - (float)samples[2] - auxmag_bias[2] }; - - rot_mult(auxmag_transform, mags, samples); - - mag.x = samples[0]; - mag.y = samples[1]; - mag.z = samples[2]; - mag.Status = AUXMAGSENSOR_STATUS_OK; - - AuxMagSensorSet(&mag); -#else auxmagsupport_publish_samples(samples, AUXMAGSENSOR_STATUS_OK); -#endif } static void handleBaro(float sample, float temperature) @@ -595,7 +554,7 @@ static void updateBaroTempBias(float temperature) } /** - * Locally cache some variables from the AtttitudeSettings object + * Locally cache some variables from the AttitudeSettings object */ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv) { @@ -603,59 +562,38 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv) mag_bias[0] = cal.mag_bias.X; mag_bias[1] = cal.mag_bias.Y; mag_bias[2] = cal.mag_bias.Z; -#ifdef AUX_MAG_LOCAL_SUPPORT - AuxMagSettingsGet(&auxmagcal); - auxmag_bias[0] = auxmagcal.mag_bias.X; - auxmag_bias[1] = auxmagcal.mag_bias.Y; - auxmag_bias[2] = auxmagcal.mag_bias.Z; -#endif - AccelGyroSettingsGet(&agcal); + AccelGyroSettingsGet(&agcal); accel_temp_calibrated = (agcal.temp_calibrated_extent.max - agcal.temp_calibrated_extent.min > .1f) && (fabsf(agcal.accel_temp_coeff.X) > 1e-9f || fabsf(agcal.accel_temp_coeff.Y) > 1e-9f || fabsf(agcal.accel_temp_coeff.Z) > 1e-9f); - gyro_temp_calibrated = (agcal.temp_calibrated_extent.max - agcal.temp_calibrated_extent.min > .1f) && (fabsf(agcal.gyro_temp_coeff.X) > 1e-9f || fabsf(agcal.gyro_temp_coeff.Y) > 1e-9f || fabsf(agcal.gyro_temp_coeff.Z) > 1e-9f || fabsf(agcal.gyro_temp_coeff.Z2) > 1e-9f); - + // convert BoardRotation ("rotate virtual") into a quaternion AttitudeSettingsData attitudeSettings; AttitudeSettingsGet(&attitudeSettings); - - // Indicates not to expend cycles on rotation - if (fabsf(attitudeSettings.BoardRotation.Roll) < ZERO_ROT_ANGLE - && fabsf(attitudeSettings.BoardRotation.Pitch) < ZERO_ROT_ANGLE && - fabsf(attitudeSettings.BoardRotation.Yaw) < ZERO_ROT_ANGLE) { - rotate = 0; - } else { - rotate = 1; - } - const float rpy[3] = { attitudeSettings.BoardRotation.Roll, attitudeSettings.BoardRotation.Pitch, attitudeSettings.BoardRotation.Yaw }; - float rotationQuat[4]; RPY2Quaternion(rpy, rotationQuat); - if (fabsf(attitudeSettings.BoardLevelTrim.Roll) > ZERO_ROT_ANGLE || - fabsf(attitudeSettings.BoardLevelTrim.Pitch) > ZERO_ROT_ANGLE) { - float trimQuat[4]; - float sumQuat[4]; - rotate = 1; + // convert BoardLevelTrim ("board level calibration") into a quaternion + float trimQuat[4]; + float sumQuat[4]; + const float trimRpy[3] = { attitudeSettings.BoardLevelTrim.Roll, attitudeSettings.BoardLevelTrim.Pitch, 0.0f }; + // do we actually want to include BoardLevelTrim in the mag rotation? BoardRotation yes, but BoardLevelTrim? + // and is BoardLevelTrim done at the correct point in the sequence of rotations? + RPY2Quaternion(trimRpy, trimQuat); - const float trimRpy[3] = { attitudeSettings.BoardLevelTrim.Roll, attitudeSettings.BoardLevelTrim.Pitch, 0.0f }; - RPY2Quaternion(trimRpy, trimQuat); + // add the boardrotation and boardtrim rotations and put them into a rotation matrix + quat_mult(rotationQuat, trimQuat, sumQuat); + Quaternion2R(sumQuat, R); - quat_mult(rotationQuat, trimQuat, sumQuat); - Quaternion2R(sumQuat, R); - } else { - Quaternion2R(rotationQuat, R); - } + // mag_transform is only a scaling + // so add the scaling, and store the result in mag_transform for run time use matrix_mult_3x3f((float(*)[3])RevoCalibrationmag_transformToArray(cal.mag_transform), R, mag_transform); -#ifdef AUX_MAG_LOCAL_SUPPORT - matrix_mult_3x3f((float(*)[3])AuxMagSettingsmag_transformToArray(auxmagcal.mag_transform), R, auxmag_transform); -#endif RevoSettingsBaroTempCorrectionPolynomialGet(&baroCorrection); RevoSettingsBaroTempCorrectionExtentGet(&baroCorrectionExtent); diff --git a/flight/pios/common/pios_hmc5x83.c b/flight/pios/common/pios_hmc5x83.c index 92b3d7e47..34460ab2e 100644 --- a/flight/pios/common/pios_hmc5x83.c +++ b/flight/pios/common/pios_hmc5x83.c @@ -42,7 +42,6 @@ typedef struct { uint32_t magic; const struct pios_hmc5x83_cfg *cfg; - enum PIOS_HMC5X83_ORIENTATION Orientation; uint32_t port_id; uint8_t slave_num; uint8_t CTRLB; @@ -103,10 +102,9 @@ pios_hmc5x83_dev_t PIOS_HMC5x83_Init(const struct pios_hmc5x83_cfg *cfg, uint32_ { pios_hmc5x83_dev_data_t *dev = dev_alloc(); - dev->cfg = cfg; // store config before enabling interrupt + dev->cfg = cfg; // store config before enabling interrupt dev->port_id = port_id; dev->slave_num = slave_num; - dev->Orientation = cfg->Orientation; // make a read/write copy so we can update it at run time. #ifdef PIOS_HMC5X83_HAS_GPIOS if (cfg->exti_cfg) { @@ -114,14 +112,9 @@ pios_hmc5x83_dev_t PIOS_HMC5x83_Init(const struct pios_hmc5x83_cfg *cfg, uint32_ } #endif -#if 0 - int32_t val = PIOS_HMC5x83_Config(dev); - PIOS_Assert(val == 0); -#else if (PIOS_HMC5x83_Config(dev) != 0) { return ((pios_hmc5x83_dev_t) NULL); } -#endif dev->data_ready = false; return (pios_hmc5x83_dev_t)dev; @@ -226,14 +219,7 @@ static int32_t PIOS_HMC5x83_Config(pios_hmc5x83_dev_data_t *dev) return 0; } -void PIOS_HMC5x83_Ext_Orientation_Set(enum PIOS_HMC5X83_ORIENTATION orientation) -{ - if (external_mag) { - ((pios_hmc5x83_dev_data_t *) external_mag)->Orientation = orientation; - } -} - -void PIOS_HMC5x83_Orient(enum PIOS_HMC5X83_ORIENTATION orientation, int16_t in[3], int16_t out[3]) +static void PIOS_HMC5x83_Orient(enum PIOS_HMC5X83_ORIENTATION orientation, int16_t in[3], int16_t out[3]) { switch (orientation) { case PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP: @@ -276,11 +262,6 @@ void PIOS_HMC5x83_Orient(enum PIOS_HMC5X83_ORIENTATION orientation, int16_t in[3 out[1] = in[2]; out[2] = in[1]; break; - case PIOS_HMC5X83_ORIENTATION_UNCHANGED: - out[0] = in[0]; // N - out[1] = in[1]; // D - out[2] = in[2]; // E - break; } } @@ -337,7 +318,7 @@ int32_t PIOS_HMC5x83_ReadMag(pios_hmc5x83_dev_t handler, int16_t out[3]) temp[i] = v; } - PIOS_HMC5x83_Orient(dev->Orientation, temp, out); + PIOS_HMC5x83_Orient(dev->cfg->Orientation, temp, out); // "This should not be necessary but for some reason it is coming out of continuous conversion mode" // @@ -368,7 +349,7 @@ uint8_t PIOS_HMC5x83_ReadID(pios_hmc5x83_dev_t handler, uint8_t out[4]) // define this to simply return true when asking if data is available on non-GPIO devices // we just set the polling rate elsewhere // this is more efficient, but has more data time lag -#define HMC5X83_STATUS_POLL_RETURNS_TRUE +//#define HMC5X83_STATUS_POLL_RETURNS_TRUE /** * @brief Tells whether new magnetometer readings are available diff --git a/flight/pios/inc/pios_hmc5x83.h b/flight/pios/inc/pios_hmc5x83.h index 92070d4c6..02206ed66 100644 --- a/flight/pios/inc/pios_hmc5x83.h +++ b/flight/pios/inc/pios_hmc5x83.h @@ -99,7 +99,6 @@ #define PIOS_HMC5x83_DATAOUT_STATUS_RDY 0x01 typedef uintptr_t pios_hmc5x83_dev_t; -extern pios_hmc5x83_dev_t external_mag; struct pios_hmc5x83_io_driver { int32_t (*Write)(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t buffer); @@ -123,7 +122,6 @@ enum PIOS_HMC5X83_ORIENTATION { PIOS_HMC5X83_ORIENTATION_SOUTH_WEST_DOWN, PIOS_HMC5X83_ORIENTATION_WEST_NORTH_DOWN, PIOS_HMC5X83_ORIENTATION_NORTH_EAST_DOWN, - PIOS_HMC5X83_ORIENTATION_UNCHANGED, }; @@ -144,8 +142,6 @@ struct pios_hmc5x83_cfg { extern pios_hmc5x83_dev_t PIOS_HMC5x83_Init(const struct pios_hmc5x83_cfg *cfg, uint32_t port_id, uint8_t device_num); extern void PIOS_HMC5x83_Register(pios_hmc5x83_dev_t handler, PIOS_SENSORS_TYPE sensortype); -extern void PIOS_HMC5x83_Ext_Orientation_Set(enum PIOS_HMC5X83_ORIENTATION orientation); -extern void PIOS_HMC5x83_Orient(enum PIOS_HMC5X83_ORIENTATION orientation, int16_t in[3], int16_t out[3]); extern bool PIOS_HMC5x83_NewDataAvailable(pios_hmc5x83_dev_t handler); extern int32_t PIOS_HMC5x83_ReadMag(pios_hmc5x83_dev_t handler, int16_t out[3]); extern uint8_t PIOS_HMC5x83_ReadID(pios_hmc5x83_dev_t handler, uint8_t out[4]); diff --git a/flight/targets/boards/discoveryf4bare/firmware/pios_board.c b/flight/targets/boards/discoveryf4bare/firmware/pios_board.c index 108fb340e..1264cd96b 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/pios_board.c +++ b/flight/targets/boards/discoveryf4bare/firmware/pios_board.c @@ -155,7 +155,7 @@ static const struct pios_hmc5x83_cfg pios_hmc5x83_external_cfg = { .Mode = PIOS_HMC5x83_MODE_CONTINUOUS, .TempCompensation = false, .Driver = &PIOS_HMC5x83_I2C_DRIVER, - .Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP, + .Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP, // ENU for GPSV9, WND for typical I2C mag }; #endif /* PIOS_INCLUDE_HMC5X83 */ @@ -422,13 +422,7 @@ void PIOS_Board_Init(void) PIOS_IAP_WriteBootCmd(2, 0); } #ifdef PIOS_INCLUDE_WDG - /* 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(); - } + PIOS_WDG_Init(); #endif /* Initialize the task monitor */ @@ -699,6 +693,9 @@ void PIOS_Board_Init(void) // 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 + // be careful that you don't register a slow, unimportant sensor after registering the fastest sensor + // and before registering some other fast and important sensor + // as that would cause delay and time jitter for the second fast sensor 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); diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index 660e0d194..e63121dc5 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -157,7 +157,7 @@ static const struct pios_hmc5x83_cfg pios_hmc5x83_external_cfg = { .Mode = PIOS_HMC5x83_MODE_CONTINUOUS, .TempCompensation = false, .Driver = &PIOS_HMC5x83_I2C_DRIVER, - .Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP, + .Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP, // ENU for GPSV9, WND for typical I2C mag }; #endif /* PIOS_INCLUDE_HMC5X83 */ @@ -431,13 +431,7 @@ void PIOS_Board_Init(void) } #ifdef PIOS_INCLUDE_WDG - /* 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(); - } + PIOS_WDG_Init(); #endif /* Initialize the task monitor */ @@ -510,6 +504,9 @@ void PIOS_Board_Init(void) // 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 + // be careful that you don't register a slow, unimportant sensor after registering the fastest sensor + // and before registering some other fast and important sensor + // as that would cause delay and time jitter for the second fast sensor 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); diff --git a/flight/targets/boards/revonano/firmware/pios_board.c b/flight/targets/boards/revonano/firmware/pios_board.c index 001fd37cd..8f7d25193 100644 --- a/flight/targets/boards/revonano/firmware/pios_board.c +++ b/flight/targets/boards/revonano/firmware/pios_board.c @@ -109,7 +109,7 @@ static const struct pios_hmc5x83_cfg pios_hmc5x83_external_cfg = { .Mode = PIOS_HMC5x83_MODE_CONTINUOUS, .TempCompensation = false, .Driver = &PIOS_HMC5x83_I2C_DRIVER, - .Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP, + .Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP, // ENU for GPSV9, WND for typical I2C mag }; #endif /* PIOS_INCLUDE_HMC5X83 */ @@ -380,13 +380,7 @@ void PIOS_Board_Init(void) } #ifdef PIOS_INCLUDE_WDG - /* 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(); - } + PIOS_WDG_Init(); #endif /* Initialize the task monitor */ @@ -656,6 +650,9 @@ void PIOS_Board_Init(void) // 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 + // be careful that you don't register a slow, unimportant sensor after registering the fastest sensor + // and before registering some other fast and important sensor + // as that would cause delay and time jitter for the second fast sensor 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); diff --git a/shared/uavobjectdefinition/auxmagsettings.xml b/shared/uavobjectdefinition/auxmagsettings.xml index f02264df9..3ebc0545a 100644 --- a/shared/uavobjectdefinition/auxmagsettings.xml +++ b/shared/uavobjectdefinition/auxmagsettings.xml @@ -5,9 +5,7 @@ - +