LP-73 ExtMag fix some more watchdog issues

flight/modules/Sensors/sensors.c

@@ -59,6 +59,7 @@
 #include <revocalibration.h>
 #include <auxmagsettings.h>
 #include <auxmagsensor.h>
+#include <auxmagsupport.h>
 #include <accelgyrosettings.h>
 #include <revosettings.h>
 
@@ -86,6 +87,8 @@
 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
 
@@ -152,7 +155,9 @@ 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
@@ -160,9 +165,12 @@ 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;
@@ -204,7 +212,9 @@ int32_t SensorsInitialize(void)
     MagSensorInitialize();
     BaroSensorInitialize();
     RevoCalibrationInitialize();
+#ifdef AUX_MAG_LOCAL_SUPPORT
     AuxMagSettingsInitialize();
+#endif
     RevoSettingsInitialize();
     AttitudeSettingsInitialize();
     AccelGyroSettingsInitialize();
@@ -213,7 +223,9 @@ int32_t SensorsInitialize(void)
 
     RevoSettingsConnectCallback(&settingsUpdatedCb);
     RevoCalibrationConnectCallback(&settingsUpdatedCb);
+#ifdef AUX_MAG_LOCAL_SUPPORT
     AuxMagSettingsConnectCallback(&settingsUpdatedCb);
+#endif
     AttitudeSettingsConnectCallback(&settingsUpdatedCb);
     AccelGyroSettingsConnectCallback(&settingsUpdatedCb);
 
@@ -485,10 +497,11 @@ 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] - mag_bias[0],
+    float mags[3] = { (float)samples[0] - auxmag_bias[0],
-                      (float)samples[1] - mag_bias[1],
+                      (float)samples[1] - auxmag_bias[1],
-                      (float)samples[2] - mag_bias[2] };
+                      (float)samples[2] - auxmag_bias[2] };
 
     rot_mult(auxmag_transform, mags, samples);
 
@@ -498,6 +511,9 @@ static void handleAuxMag(float *samples)
     mag.Status = AUXMAGSENSOR_STATUS_OK;
 
     AuxMagSensorSet(&mag);
+#else
+    auxmagsupport_publish_samples(samples, AUXMAGSENSOR_STATUS_OK);
+#endif
 }
 
 static void handleBaro(float sample, float temperature)
@@ -584,11 +600,16 @@ static void updateBaroTempBias(float temperature)
 static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
 {
     RevoCalibrationGet(&cal);
-    AuxMagSettingsGet(&auxmagcal);
-    AccelGyroSettingsGet(&agcal);
     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);
 
     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);
@@ -632,7 +653,9 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
         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

@@ -114,8 +114,14 @@ 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;
@@ -123,7 +129,9 @@ pios_hmc5x83_dev_t PIOS_HMC5x83_Init(const struct pios_hmc5x83_cfg *cfg, uint32_
 
 void PIOS_HMC5x83_Register(pios_hmc5x83_dev_t handler, PIOS_SENSORS_TYPE sensortype)
 {
-    PIOS_SENSORS_Register(&PIOS_HMC5x83_Driver, sensortype, handler);
+    if (handler) {
+        PIOS_SENSORS_Register(&PIOS_HMC5x83_Driver, sensortype, handler);
+    }
 }
 
 /**
@@ -360,7 +368,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_POLLED_STATUS_RETURNS_TRUE
+#define HMC5X83_STATUS_POLL_RETURNS_TRUE
 
 /**
  * @brief Tells whether new magnetometer readings are available
@@ -369,7 +377,7 @@ uint8_t PIOS_HMC5x83_ReadID(pios_hmc5x83_dev_t handler, uint8_t out[4])
  */
 bool PIOS_HMC5x83_NewDataAvailable(__attribute__((unused)) pios_hmc5x83_dev_t handler)
 {
-#if ( defined(PIOS_HMC5X83_HAS_GPIOS) || !defined(HMC5X83_POLLED_STATUS_RETURNS_TRUE) )
+#if ( defined(PIOS_HMC5X83_HAS_GPIOS) || !defined(HMC5X83_STATUS_POLL_RETURNS_TRUE) )
     pios_hmc5x83_dev_data_t *dev = dev_validate(handler);
 #endif
 
@@ -380,7 +388,7 @@ bool PIOS_HMC5x83_NewDataAvailable(__attribute__((unused)) pios_hmc5x83_dev_t ha
     else
 #endif /* PIOS_HMC5X83_HAS_GPIOS */
     {                           // else poll to see if data is ready or just say "true" and set polling interval elsewhere
-#ifdef HMC5X83_POLLED_STATUS_RETURNS_TRUE
+#ifdef HMC5X83_STATUS_POLL_RETURNS_TRUE
         return true;
 #else
         // poll SR0 (RDY) here.  1 -> data ready.

flight/pios/inc/pios_initcall.h

@@ -92,6 +92,7 @@ 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) { \
@@ -99,13 +100,32 @@ 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) {    \
+              (fn->fn_tinit)();  \
+              PIOS_WDG_Clear();  \
+          } \
+    } }
+#endif
 
 #endif /* USE_SIM_POSIX */
 

flight/targets/boards/revolution/firmware/pios_board.c

@@ -508,8 +508,12 @@ void PIOS_Board_Init(void)
         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 */
@@ -1021,202 +1025,3 @@ void PIOS_Board_Init(void)
  * @}
  */
 
-
-
-/*
-things to try:
---------------
-tau uses single mode on external
-REMEMBER that to change this, we need to change the hack that always sets it continuous after each read?/write?
-static const struct pios_hmc5883_cfg pios_hmc5883_internal_cfg = {
-	.exti_cfg = &pios_exti_hmc5883_internal_cfg,
-	.M_ODR = PIOS_HMC5883_ODR_75,
-	.Meas_Conf = PIOS_HMC5883_MEASCONF_NORMAL,
-	.Gain = PIOS_HMC5883_GAIN_1_9,
-	.Mode = PIOS_HMC5883_MODE_CONTINUOUS,
-	.Default_Orientation = PIOS_HMC5883_TOP_90DEG,
-};
-
-static const struct pios_hmc5883_cfg pios_hmc5883_external_cfg = {
-	.M_ODR = PIOS_HMC5883_ODR_75,
-	.Meas_Conf = PIOS_HMC5883_MEASCONF_NORMAL,
-	.Gain = PIOS_HMC5883_GAIN_1_9,
-	.Mode = PIOS_HMC5883_MODE_SINGLE,
-	.Default_Orientation = PIOS_HMC5883_TOP_0DEG,
-};
-
-tau does as many i2c_init()s as are enabled
-but only does PIOS_HMC5883_Init() for the one mag that is in use
-
-try these in this order:
-------------------------
-current setup with tau fixes
-single mode
-only PIOS_HMC5883_Init() and register one mag
-increase several stacks
- sensors.c sensor task
- callback
- hardware interrupt stack
-research tau's PIOS_I2C_Init() and PIOS_HMC5883_Init()
-
-RELOAD_WDG();
-
-things to do:
--------------
-make an ASSERT() for enum PIOS_HMC5X83_ORIENTATION must match AuxMagSettingsOrientationOptions
-fix i2c reset in all processors, not just F4
-move all I2C stuff to a lower priority sensor task
-
-Things to do now:
------------------
-fix AUX_MAG_SKIP
-set AMS lower and see some duplicates
-set AMS to exactly what it needs to be for highest rate with no dups
-figure out mag orientation and set correct default and document how to determine what to use
-  I think that chip upside down and facing forward is the best default.
-back up into a tar
-clean up code 90% at least
-examine git diff
-back up into a tar
-merge latest next into it
-test calibration
-test fly
-push
-announce
-code and TL fixes on all other boards
-push
-
-for a test I moved PIOS_WDG_Init() to the end of PIOS_Board_Init()
-four work arounds:
-------------------
-disable ext mag with either UAVO setting or code
-disable int mag with code
-  remove just registration
-    PIOS_HMC5x83_Register(onboard_mag, PIOS_SENSORS_TYPE_3AXIS_MAG);
-disable watchdog
-  either comment out #define PIOS_INCLUDE_WDG or comment out PIOS_WDG_Init();
-sometimes: pause in PIOS_Board_Init() with the debugger, then some next/step then cont, can't reproduce
-
-does not fix it:
-----------------
-disable baro registration
-some RELOAD_WDG(); in sensors.c that I tried
-
-neo6 gps/mag:
--------------
-east south down is right side up
-south west down locks up the GCS, no it just reds the mag and reinits and so atti and stab are offline
-
-mag facts:
-Revo front pointed north gives high positive X
-Revo right (servo pins) pointed north gives high Y
-Revo bottom pointed north gives high positive Z
-
-neo6 bottom pointed north gives high
-neo6 bottom pointed north gives high
-neo6 bottom pointed north gives high
-
-steps:
-send UNCHANGED
-point front north (and very importantly down 64 degrees for me)
-flip 180 degrees
-which axis moved the most?  was it positive (then negative)?  X yes
-that is N channel
-
-point bottom north
-flip 180 degrees
-which axis moved the most?  was it positive (then negative)?  Y yes
-that is D channel
-
-point right side north
-flip 180 degrees
-which axis moved the most?  was it positive (then negative)?  Z yes
-that is E channel
-
-Revo does X yes, Z yes, Y yes
-so to get that from this mag we need all positive
-NED
-maybe ask which axis moves Z
-
-which one makes X move, that is north (south if negative)
-which one makes Y move, that is east   (west if negative)
-which one makes Z move, that is down     (up if negative)
-find that exact combination in some order
-
-a test with upside down battery the desired orientation
-N channel Z neg is S
-D channel Y neg is U
-E channel X neg is W
-arrange in XZY order and that is WSU
-
-should be WSU
-that is correct
-What must we do to get NDE XZY
-
-=================================
-rephrase question
-what gives us XZY
-NED
-WSU
-
-=================================
-send UNCHANGED
-point front north (and very importantly down 64 degrees for me)
-flip 180 degrees
-which axis moved the most?  was it positive (then negative)?  X yes
-that is N channel
-
-point bottom north
-flip 180 degrees
-which axis moved the most?  was it positive (then negative)?  Y yes
-that is D channel
-
-point right side north
-flip 180 degrees
-which axis moved the most?  was it positive (then negative)?  Z yes
-that is E channel
-
-negative N is S and comes from Z
-negative D is U and comes from Y
-negative E is W and comes from X
-where did XZY come from?  WSU
-
-
-wires forward and top side up
-Note that it is the largest difference.
-  -100 to -900 is larger than 200 to -200
-  -100 to -900 started more positive
-you can find a more accurate north after you know which axis is pointing that way
-  make small rotations in yaw and pitch to change where it is pointing a little and watch for the strongest signal ON THAT AXIS
-send UNCHANGED
-point front north (and very importantly down 64 degrees for me)
-flip 180 degrees to point the back north
-which axis moved the most?  did it start positive (then negative)?  X neg
-that is N channel
-
-point bottom north
-flip 180 degrees to point the top north
-which axis moved the most?  did it start positive (then negative)?  Y pos
-that is D channel
-
-point right side north
-flip 180 degrees to point the left north
-which axis moved the most?  did it start positive (then negative)?  Z neg
-that is E channel
-
-X-N = S
-Y+D = D
-Z-E = W
-where did XZY come from?  SWD
-looks pointed south, not west after hours
-
-
---- or? ---
-debug and hit continue when it traps
-
-It appears that the 0x08000bcc address is the boot code where it goes after watchdog
-
-
-Neo6
-7-8 sats upside down, indoors
-*/

flight/targets/boards/revolution/firmware/revolution.cpp

@@ -128,7 +128,16 @@ 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);

shared/uavobjectdefinition/auxmagsettings.xml

@@ -7,7 +7,7 @@
         <field name="MagBiasNullingRate" units="" type="float" elements="1" defaultvalue="0"/>
         <field name="Orientation" units="" type="enum" elements="1"
                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="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"/>