LP-73 changes for pull request comments

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)

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->driver->is_polled) {
-            if (sensor->type != PIOS_SENSORS_TYPE_3AXIS_AUXMAG || aux_mag_skip == 0) {
+                const QueueHandle_t queue = PIOS_SENSORS_GetQueue(sensor);
-                if (!sensor->driver->is_polled) {
+                while (xQueueReceive(queue,
-                    const QueueHandle_t queue = PIOS_SENSORS_GetQueue(sensor);
+                                     (void *)source_data,
-                    while (xQueueReceive(queue,
+                                     (is_primary && !sensor_context.count) ? sensor_period_ticks : 0) == pdTRUE) {
-                                         (void *)source_data,
+                    accumulateSamples(&sensor_context, source_data);
-                                         (is_primary && !sensor_context.count) ? sensor_period_ticks : 0) == pdTRUE) {
+                }
+                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 {
-                    } else if (is_primary) {
+                        processSamples1d(&source_data->sensorSample1Axis, sensor);
-                        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);
                     }
+                    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 ||
+    // convert BoardLevelTrim ("board level calibration") into a quaternion
-        fabsf(attitudeSettings.BoardLevelTrim.Pitch) > ZERO_ROT_ANGLE) {
+    float trimQuat[4];
-        float trimQuat[4];
+    float sumQuat[4];
-        float sumQuat[4];
+    const float trimRpy[3] = { attitudeSettings.BoardLevelTrim.Roll, attitudeSettings.BoardLevelTrim.Pitch, 0.0f };
-        rotate = 1;
+    // 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 };
+    // add the boardrotation and boardtrim rotations and put them into a rotation matrix
-        RPY2Quaternion(trimRpy, trimQuat);
+    quat_mult(rotationQuat, trimQuat, sumQuat);
+    Quaternion2R(sumQuat, R);
 
-        quat_mult(rotationQuat, trimQuat, sumQuat);
+    // mag_transform is only a scaling
-        Quaternion2R(sumQuat, R);
+    // so add the scaling, and store the result in mag_transform for run time use
-    } else {
-        Quaternion2R(rotationQuat, R);
-    }
     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);

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)
+static void PIOS_HMC5x83_Orient(enum PIOS_HMC5X83_ORIENTATION orientation, int16_t in[3], int16_t out[3])
-{
-    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])
 {
     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

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]);

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
+    PIOS_WDG_Init();
-     * 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 */
@@ -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);

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
+    PIOS_WDG_Init();
-     * 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 */
@@ -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);

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
+    PIOS_WDG_Init();
-     * 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 */
@@ -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);

shared/uavobjectdefinition/auxmagsettings.xml

@@ -5,9 +5,7 @@
         <field name="mag_transform" units="gain" type="float" elementnames="r0c0,r0c1,r0c2,r1c0,r1c1,r1c2,r2c0,r2c1,r2c2"
          defaultvalue="1,0,0,0,1,0,0,0,1"/>
         <field name="MagBiasNullingRate" units="" type="float" elements="1" defaultvalue="0"/>
-        <field name="Orientation" units="" type="enum" elements="1"
+        <field name="BoardRotation" units="deg" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="0,0,0"/>
-               options="EAST_NORTH_UP,SOUTH_EAST_UP,WEST_SOUTH_UP,NORTH_WEST_UP,EAST_SOUTH_DOWN,SOUTH_WEST_DOWN,WEST_NORTH_DOWN,NORTH_EAST_DOWN,UNCHANGED"
-               defaultvalue="NORTH_EAST_DOWN"/>
         <field name="Type" units="" type="enum" elements="1" options="GPSV9,Ext,Flexi" defaultvalue="GPSV9"/>
         <field name="Usage" units="" type="enum" elements="1" options="Both,OnboardOnly,AuxOnly" defaultvalue="Both"/>
         <access gcs="readwrite" flight="readwrite"/>