From b086d1a021a5952689f76c3ed90b0d4d19477c72 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Sun, 13 Sep 2015 06:47:42 -0400 Subject: [PATCH] LP-73 ExtMag fix some more watchdog issues --- flight/modules/Sensors/sensors.c | 33 ++- flight/pios/common/pios_hmc5x83.c | 16 +- flight/pios/inc/pios_initcall.h | 20 ++ .../boards/revolution/firmware/pios_board.c | 203 +----------------- .../boards/revolution/firmware/revolution.cpp | 9 + shared/uavobjectdefinition/auxmagsettings.xml | 2 +- 6 files changed, 74 insertions(+), 209 deletions(-) diff --git a/flight/modules/Sensors/sensors.c b/flight/modules/Sensors/sensors.c index d1d271d17..5b4b1f1cc 100644 --- a/flight/modules/Sensors/sensors.c +++ b/flight/modules/Sensors/sensors.c @@ -59,6 +59,7 @@ #include #include #include +#include #include #include @@ -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)samples[1] - mag_bias[1], - (float)samples[2] - mag_bias[2] }; + 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); @@ -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); diff --git a/flight/pios/common/pios_hmc5x83.c b/flight/pios/common/pios_hmc5x83.c index 68d4013f0..92b3d7e47 100644 --- a/flight/pios/common/pios_hmc5x83.c +++ b/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. diff --git a/flight/pios/inc/pios_initcall.h b/flight/pios/inc/pios_initcall.h index 55b3e15f9..52669e658 100644 --- a/flight/pios/inc/pios_initcall.h +++ b/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 */ diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index 0c0c67bfe..2d339d223 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/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 -*/ diff --git a/flight/targets/boards/revolution/firmware/revolution.cpp b/flight/targets/boards/revolution/firmware/revolution.cpp index 268325b26..be85f08c4 100644 --- a/flight/targets/boards/revolution/firmware/revolution.cpp +++ b/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); diff --git a/shared/uavobjectdefinition/auxmagsettings.xml b/shared/uavobjectdefinition/auxmagsettings.xml index d2ea3dcb2..f02264df9 100644 --- a/shared/uavobjectdefinition/auxmagsettings.xml +++ b/shared/uavobjectdefinition/auxmagsettings.xml @@ -7,7 +7,7 @@ + defaultvalue="NORTH_EAST_DOWN"/>