From 3d1cc31c3b91971cf1765ec6dc3746f54b23775f Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Tue, 14 Jan 2014 12:22:01 +0100 Subject: [PATCH 01/10] OP-1161 Add alarm for magnetometer plausibility check --- flight/modules/Sensors/sensors.c | 65 +++++++++++++++++++++ shared/uavobjectdefinition/revosettings.xml | 3 + shared/uavobjectdefinition/systemalarms.xml | 1 + 3 files changed, 69 insertions(+) diff --git a/flight/modules/Sensors/sensors.c b/flight/modules/Sensors/sensors.c index cc99a0e76..b0ac93b0c 100644 --- a/flight/modules/Sensors/sensors.c +++ b/flight/modules/Sensors/sensors.c @@ -55,6 +55,8 @@ #include #include #include +#include +#include #include #include @@ -73,6 +75,7 @@ // Private functions static void SensorsTask(void *parameters); static void settingsUpdatedCb(UAVObjEvent *objEv); +static void checkMagValidity(MagSensorData *mag); // static void magOffsetEstimation(MagSensorData *mag); // Private variables @@ -92,6 +95,8 @@ static float R[3][3] = { { 0 } }; static int8_t rotate = 0; +static RevoSettingsMagnetometerMaxDeviationData magMaxDev; +static float Be[3] = { 0 }; /** * API for sensor fusion algorithms: @@ -112,12 +117,17 @@ int32_t SensorsInitialize(void) AccelSensorInitialize(); MagSensorInitialize(); RevoCalibrationInitialize(); + RevoSettingsInitialize(); AttitudeSettingsInitialize(); + AttitudeStateInitialize(); + HomeLocationInitialize(); rotate = 0; RevoCalibrationConnectCallback(&settingsUpdatedCb); AttitudeSettingsConnectCallback(&settingsUpdatedCb); + RevoSettingsConnectCallback(&settingsUpdatedCb); + HomeLocationConnectCallback(&settingsUpdatedCb); return 0; } @@ -404,6 +414,7 @@ static void SensorsTask(__attribute__((unused)) void *parameters) MagSensorSet(&mag); mag_update_time = PIOS_DELAY_GetRaw(); + checkMagValidity(&mag); } #endif /* if defined(PIOS_INCLUDE_HMC5883) */ @@ -415,6 +426,57 @@ static void SensorsTask(__attribute__((unused)) void *parameters) } } +/** + * check validity of magnetometers + */ +static void checkMagValidity(MagSensorData *mag) +{ + #define lowPassAlpha 0.01f + #define idleCount 100 + static float average[3] = { 0 }; + static uint8_t noteverytime = idleCount; + + // low pass filter sensor to not give warnings due to noise + average[0] = (1.0f - lowPassAlpha) * average[0] + lowPassAlpha * mag->x; + average[1] = (1.0f - lowPassAlpha) * average[1] + lowPassAlpha * mag->y; + average[2] = (1.0f - lowPassAlpha) * average[2] + lowPassAlpha * mag->z; + + // throttle this check, thanks to low pass filter it is not necessary every iteration + if (!noteverytime--) { + noteverytime = idleCount; + + // calculate expected Be vector + AttitudeStateData attitudeState; + AttitudeStateGet(&attitudeState); + float Rot[3][3]; + float expected[3]; + Quaternion2R(&attitudeState.q1, Rot); + rot_mult(Rot, Be, expected); + + // calculate maximum allowed deviation + float warning = expected[0] * expected[0] + expected[1] * expected[1] + expected[2] * expected[2]; + float error = magMaxDev.Error * magMaxDev.Error * warning; + warning = magMaxDev.Warning * magMaxDev.Warning * warning; + + + // calculate difference + expected[0] = expected[0] - average[0]; + expected[1] = expected[1] - average[1]; + expected[2] = expected[2] - average[2]; + float deviation = expected[0] * expected[0] + expected[1] * expected[1] + expected[2] * expected[2]; + + // set errors + if (deviation < warning) { + AlarmsClear(SYSTEMALARMS_ALARM_MAGNETOMETER); + } else if (deviation < error) { + AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_WARNING); + } else { + AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_ERROR); + } + } +} + + /** * Locally cache some variables from the AtttitudeSettings object */ @@ -458,6 +520,9 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv) Quaternion2R(rotationQuat, R); rotate = 1; } + + RevoSettingsMagnetometerMaxDeviationGet(&magMaxDev); + HomeLocationBeGet(Be); } /** * @} diff --git a/shared/uavobjectdefinition/revosettings.xml b/shared/uavobjectdefinition/revosettings.xml index 0cf9a4561..3fc3554e8 100644 --- a/shared/uavobjectdefinition/revosettings.xml +++ b/shared/uavobjectdefinition/revosettings.xml @@ -8,6 +8,9 @@ Set BaroGPSOffsetCorrectionAlpha = 1.0 to completely disable baro offset updates. --> + + + diff --git a/shared/uavobjectdefinition/systemalarms.xml b/shared/uavobjectdefinition/systemalarms.xml index 34c353d64..05b717cc3 100644 --- a/shared/uavobjectdefinition/systemalarms.xml +++ b/shared/uavobjectdefinition/systemalarms.xml @@ -14,6 +14,7 @@ Actuator Attitude Sensors + Magnetometer Stabilization Guidance Battery From c180846ca9ccb9ac688af92d5a7d0350f6cab97a Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Tue, 14 Jan 2014 15:42:45 +0100 Subject: [PATCH 02/10] OP-1161 discard magnetometer in filters if plausibility alarm is set --- flight/modules/StateEstimation/filtermag.c | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/flight/modules/StateEstimation/filtermag.c b/flight/modules/StateEstimation/filtermag.c index 7997016e0..87506cbb8 100644 --- a/flight/modules/StateEstimation/filtermag.c +++ b/flight/modules/StateEstimation/filtermag.c @@ -33,6 +33,7 @@ #include "inc/stateestimation.h" #include #include +#include #include #include @@ -81,8 +82,14 @@ static int32_t filter(stateFilter *self, stateEstimation *state) struct data *this = (struct data *)self->localdata; if (IS_SET(state->updated, SENSORUPDATES_mag)) { - if (this->revoCalibration.MagBiasNullingRate > 0) { - magOffsetEstimation(this, state->mag); + SystemAlarmsAlarmData alarms; + SystemAlarmsAlarGet(&alarms); + if (alarms.Magnetometer != SYSTEMALARMS_ALARM_OK) { + UNSET_MASK(state->updated, SENSORUPDATES_mag); + } else { + if (this->revoCalibration.MagBiasNullingRate > 0) { + magOffsetEstimation(this, state->mag); + } } } From 75b1a7ff5f4537f8cde094785d4230ec3b115d1d Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Tue, 14 Jan 2014 16:38:16 +0100 Subject: [PATCH 03/10] OP-1161 moved mag validity check function from sensors to state estimation --- flight/modules/Sensors/sensors.c | 65 -------------------- flight/modules/StateEstimation/filtermag.c | 70 ++++++++++++++++++++-- 2 files changed, 65 insertions(+), 70 deletions(-) diff --git a/flight/modules/Sensors/sensors.c b/flight/modules/Sensors/sensors.c index b0ac93b0c..cc99a0e76 100644 --- a/flight/modules/Sensors/sensors.c +++ b/flight/modules/Sensors/sensors.c @@ -55,8 +55,6 @@ #include #include #include -#include -#include #include #include @@ -75,7 +73,6 @@ // Private functions static void SensorsTask(void *parameters); static void settingsUpdatedCb(UAVObjEvent *objEv); -static void checkMagValidity(MagSensorData *mag); // static void magOffsetEstimation(MagSensorData *mag); // Private variables @@ -95,8 +92,6 @@ static float R[3][3] = { { 0 } }; static int8_t rotate = 0; -static RevoSettingsMagnetometerMaxDeviationData magMaxDev; -static float Be[3] = { 0 }; /** * API for sensor fusion algorithms: @@ -117,17 +112,12 @@ int32_t SensorsInitialize(void) AccelSensorInitialize(); MagSensorInitialize(); RevoCalibrationInitialize(); - RevoSettingsInitialize(); AttitudeSettingsInitialize(); - AttitudeStateInitialize(); - HomeLocationInitialize(); rotate = 0; RevoCalibrationConnectCallback(&settingsUpdatedCb); AttitudeSettingsConnectCallback(&settingsUpdatedCb); - RevoSettingsConnectCallback(&settingsUpdatedCb); - HomeLocationConnectCallback(&settingsUpdatedCb); return 0; } @@ -414,7 +404,6 @@ static void SensorsTask(__attribute__((unused)) void *parameters) MagSensorSet(&mag); mag_update_time = PIOS_DELAY_GetRaw(); - checkMagValidity(&mag); } #endif /* if defined(PIOS_INCLUDE_HMC5883) */ @@ -426,57 +415,6 @@ static void SensorsTask(__attribute__((unused)) void *parameters) } } -/** - * check validity of magnetometers - */ -static void checkMagValidity(MagSensorData *mag) -{ - #define lowPassAlpha 0.01f - #define idleCount 100 - static float average[3] = { 0 }; - static uint8_t noteverytime = idleCount; - - // low pass filter sensor to not give warnings due to noise - average[0] = (1.0f - lowPassAlpha) * average[0] + lowPassAlpha * mag->x; - average[1] = (1.0f - lowPassAlpha) * average[1] + lowPassAlpha * mag->y; - average[2] = (1.0f - lowPassAlpha) * average[2] + lowPassAlpha * mag->z; - - // throttle this check, thanks to low pass filter it is not necessary every iteration - if (!noteverytime--) { - noteverytime = idleCount; - - // calculate expected Be vector - AttitudeStateData attitudeState; - AttitudeStateGet(&attitudeState); - float Rot[3][3]; - float expected[3]; - Quaternion2R(&attitudeState.q1, Rot); - rot_mult(Rot, Be, expected); - - // calculate maximum allowed deviation - float warning = expected[0] * expected[0] + expected[1] * expected[1] + expected[2] * expected[2]; - float error = magMaxDev.Error * magMaxDev.Error * warning; - warning = magMaxDev.Warning * magMaxDev.Warning * warning; - - - // calculate difference - expected[0] = expected[0] - average[0]; - expected[1] = expected[1] - average[1]; - expected[2] = expected[2] - average[2]; - float deviation = expected[0] * expected[0] + expected[1] * expected[1] + expected[2] * expected[2]; - - // set errors - if (deviation < warning) { - AlarmsClear(SYSTEMALARMS_ALARM_MAGNETOMETER); - } else if (deviation < error) { - AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_WARNING); - } else { - AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_ERROR); - } - } -} - - /** * Locally cache some variables from the AtttitudeSettings object */ @@ -520,9 +458,6 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv) Quaternion2R(rotationQuat, R); rotate = 1; } - - RevoSettingsMagnetometerMaxDeviationGet(&magMaxDev); - HomeLocationBeGet(Be); } /** * @} diff --git a/flight/modules/StateEstimation/filtermag.c b/flight/modules/StateEstimation/filtermag.c index 87506cbb8..8882ceee4 100644 --- a/flight/modules/StateEstimation/filtermag.c +++ b/flight/modules/StateEstimation/filtermag.c @@ -33,6 +33,7 @@ #include "inc/stateestimation.h" #include #include +#include #include #include @@ -46,7 +47,11 @@ struct data { HomeLocationData homeLocation; RevoCalibrationData revoCalibration; - float magBias[3]; + RevoSettingsData revoSettings; + uint16_t counter; + bool validity; + float magAverage[3]; + float magBias[3]; }; // Private variables @@ -55,6 +60,7 @@ struct data { static int32_t init(stateFilter *self); static int32_t filter(stateFilter *self, stateEstimation *state); +static bool checkMagValidity(struct data *this, float mag[3]); static void magOffsetEstimation(struct data *this, float mag[3]); @@ -71,9 +77,13 @@ static int32_t init(stateFilter *self) { struct data *this = (struct data *)self->localdata; - this->magBias[0] = this->magBias[1] = this->magBias[2] = 0.0f; + this->magBias[0] = this->magBias[1] = this->magBias[2] = 0.0f; + this->magAverage[0] = this->magAverage[1] = this->magAverage[2] = 0.0f; + this->counter = 0; + this->validity = true; HomeLocationGet(&this->homeLocation); RevoCalibrationGet(&this->revoCalibration); + RevoSettingsGet(&this->revoSettings); return 0; } @@ -82,9 +92,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state) struct data *this = (struct data *)self->localdata; if (IS_SET(state->updated, SENSORUPDATES_mag)) { - SystemAlarmsAlarmData alarms; - SystemAlarmsAlarGet(&alarms); - if (alarms.Magnetometer != SYSTEMALARMS_ALARM_OK) { + if (!checkMagValidity(this, state->mag)) { UNSET_MASK(state->updated, SENSORUPDATES_mag); } else { if (this->revoCalibration.MagBiasNullingRate > 0) { @@ -96,6 +104,58 @@ static int32_t filter(stateFilter *self, stateEstimation *state) return 0; } +/** + * check validity of magnetometers + */ +static bool checkMagValidity(struct data *this, float mag[3]) +{ + #define MAG_LOW_PASS_ALPHA 0.05f + #define IDLE_COUNT 20 + + // low pass filter sensor to not give warnings due to noise + this->magAverage[0] = (1.0f - MAG_LOW_PASS_ALPHA) * this->magAverage[0] + MAG_LOW_PASS_ALPHA * mag[0]; + this->magAverage[1] = (1.0f - MAG_LOW_PASS_ALPHA) * this->magAverage[1] + MAG_LOW_PASS_ALPHA * mag[0]; + this->magAverage[2] = (1.0f - MAG_LOW_PASS_ALPHA) * this->magAverage[2] + MAG_LOW_PASS_ALPHA * mag[0]; + + // throttle this check, thanks to low pass filter it is not necessary every iteration + if (!this->counter--) { + this->counter = IDLE_COUNT; + + // calculate expected Be vector + AttitudeStateData attitudeState; + AttitudeStateGet(&attitudeState); + float Rot[3][3]; + float expected[3]; + Quaternion2R(&attitudeState.q1, Rot); + rot_mult(Rot, this->homeLocation.Be, expected); + + // calculate maximum allowed deviation + float warning2 = expected[0] * expected[0] + expected[1] * expected[1] + expected[2] * expected[2]; + float error2 = this->revoSettings.MagnetometerMaxDeviation.Error * this->revoSettings.MagnetometerMaxDeviation.Error * warning2; + warning2 = this->revoSettings.MagnetometerMaxDeviation.Warning * this->revoSettings.MagnetometerMaxDeviation.Warning * warning2; + + // calculate difference + expected[0] = expected[0] - this->magAverage[0]; + expected[1] = expected[1] - this->magAverage[1]; + expected[2] = expected[2] - this->magAverage[2]; + float deviation2 = expected[0] * expected[0] + expected[1] * expected[1] + expected[2] * expected[2]; + + // set errors + if (deviation2 < warning2) { + AlarmsClear(SYSTEMALARMS_ALARM_MAGNETOMETER); + this->validity = true; + } else if (deviation2 < error2) { + AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_WARNING); + this->validity = false; + } else { + AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_ERROR); + this->validity = false; + } + } + return this->validity; +} + + /** * Perform an update of the @ref MagBias based on * Magmeter Offset Cancellation: Theory and Implementation, From 3b635693a5810d7727af7364779b3a3b4ca90f38 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Tue, 14 Jan 2014 21:09:14 +0100 Subject: [PATCH 04/10] OP-1161 changed the way magnetometer-errors are handled - and defaults * ekf will ignore magnetometer errors for initialization (errors might be caused by not yet existing attitude estimation) * defaults changed for filtering * magnetometer sensor will be available for all filters, alarms need to be checked separately --- flight/modules/StateEstimation/filterekf.c | 7 ++- flight/modules/StateEstimation/filtermag.c | 53 +++++++++++---------- shared/uavobjectdefinition/revosettings.xml | 2 +- 3 files changed, 36 insertions(+), 26 deletions(-) diff --git a/flight/modules/StateEstimation/filterekf.c b/flight/modules/StateEstimation/filterekf.c index a141e4d8f..d527e776a 100644 --- a/flight/modules/StateEstimation/filterekf.c +++ b/flight/modules/StateEstimation/filterekf.c @@ -36,6 +36,7 @@ #include #include #include +#include #include #include @@ -358,7 +359,11 @@ static int32_t filter(stateFilter *self, stateEstimation *state) INSCovariancePrediction(this->dTa); if (IS_SET(this->work.updated, SENSORUPDATES_mag)) { - sensors |= MAG_SENSORS; + SystemAlarmsAlarmData alarms; + SystemAlarmsAlarmGet(&alarms); + if (alarms.Magnetometer == SYSTEMALARMS_ALARM_OK) { + sensors |= MAG_SENSORS; + } } if (IS_SET(this->work.updated, SENSORUPDATES_baro)) { diff --git a/flight/modules/StateEstimation/filtermag.c b/flight/modules/StateEstimation/filtermag.c index 8882ceee4..887888e39 100644 --- a/flight/modules/StateEstimation/filtermag.c +++ b/flight/modules/StateEstimation/filtermag.c @@ -48,8 +48,9 @@ struct data { HomeLocationData homeLocation; RevoCalibrationData revoCalibration; RevoSettingsData revoSettings; - uint16_t counter; - bool validity; + uint16_t idlecounter; + uint8_t warningcount; + uint8_t errorcount; float magAverage[3]; float magBias[3]; }; @@ -60,7 +61,7 @@ struct data { static int32_t init(stateFilter *self); static int32_t filter(stateFilter *self, stateEstimation *state); -static bool checkMagValidity(struct data *this, float mag[3]); +static void checkMagValidity(struct data *this, float mag[3]); static void magOffsetEstimation(struct data *this, float mag[3]); @@ -79,8 +80,7 @@ static int32_t init(stateFilter *self) this->magBias[0] = this->magBias[1] = this->magBias[2] = 0.0f; this->magAverage[0] = this->magAverage[1] = this->magAverage[2] = 0.0f; - this->counter = 0; - this->validity = true; + this->idlecounter = this->warningcount = this->errorcount = 0; HomeLocationGet(&this->homeLocation); RevoCalibrationGet(&this->revoCalibration); RevoSettingsGet(&this->revoSettings); @@ -92,12 +92,9 @@ static int32_t filter(stateFilter *self, stateEstimation *state) struct data *this = (struct data *)self->localdata; if (IS_SET(state->updated, SENSORUPDATES_mag)) { - if (!checkMagValidity(this, state->mag)) { - UNSET_MASK(state->updated, SENSORUPDATES_mag); - } else { - if (this->revoCalibration.MagBiasNullingRate > 0) { - magOffsetEstimation(this, state->mag); - } + checkMagValidity(this, state->mag); + if (this->revoCalibration.MagBiasNullingRate > 0) { + magOffsetEstimation(this, state->mag); } } @@ -107,19 +104,20 @@ static int32_t filter(stateFilter *self, stateEstimation *state) /** * check validity of magnetometers */ -static bool checkMagValidity(struct data *this, float mag[3]) +static void checkMagValidity(struct data *this, float mag[3]) { - #define MAG_LOW_PASS_ALPHA 0.05f - #define IDLE_COUNT 20 + #define MAG_LOW_PASS_ALPHA 0.2f + #define IDLE_COUNT 10 + #define ALARM_THRESHOLD 3 // low pass filter sensor to not give warnings due to noise this->magAverage[0] = (1.0f - MAG_LOW_PASS_ALPHA) * this->magAverage[0] + MAG_LOW_PASS_ALPHA * mag[0]; - this->magAverage[1] = (1.0f - MAG_LOW_PASS_ALPHA) * this->magAverage[1] + MAG_LOW_PASS_ALPHA * mag[0]; - this->magAverage[2] = (1.0f - MAG_LOW_PASS_ALPHA) * this->magAverage[2] + MAG_LOW_PASS_ALPHA * mag[0]; + this->magAverage[1] = (1.0f - MAG_LOW_PASS_ALPHA) * this->magAverage[1] + MAG_LOW_PASS_ALPHA * mag[1]; + this->magAverage[2] = (1.0f - MAG_LOW_PASS_ALPHA) * this->magAverage[2] + MAG_LOW_PASS_ALPHA * mag[2]; // throttle this check, thanks to low pass filter it is not necessary every iteration - if (!this->counter--) { - this->counter = IDLE_COUNT; + if (!this->idlecounter--) { + this->idlecounter = IDLE_COUNT; // calculate expected Be vector AttitudeStateData attitudeState; @@ -142,17 +140,24 @@ static bool checkMagValidity(struct data *this, float mag[3]) // set errors if (deviation2 < warning2) { + this->warningcount = 0; + this->errorcount = 0; AlarmsClear(SYSTEMALARMS_ALARM_MAGNETOMETER); - this->validity = true; } else if (deviation2 < error2) { - AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_WARNING); - this->validity = false; + this->errorcount = 0; + if (this->warningcount > ALARM_THRESHOLD) { + AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_WARNING); + } else { + this->warningcount++; + } } else { - AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_ERROR); - this->validity = false; + if (this->errorcount > ALARM_THRESHOLD) { + AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_ERROR); + } else { + this->errorcount++; + } } } - return this->validity; } diff --git a/shared/uavobjectdefinition/revosettings.xml b/shared/uavobjectdefinition/revosettings.xml index 3fc3554e8..12fa9171b 100644 --- a/shared/uavobjectdefinition/revosettings.xml +++ b/shared/uavobjectdefinition/revosettings.xml @@ -9,7 +9,7 @@ - + From 5c1435bef10c8cf221259d64721e07371c4a4053 Mon Sep 17 00:00:00 2001 From: Bertrand Oresve Date: Thu, 1 May 2014 12:45:34 +0200 Subject: [PATCH 05/10] OP-1161 Add Mag alarm in System-Health --- .../diagrams/default/system-health.svg | 5050 ++++++++--------- 1 file changed, 2286 insertions(+), 2764 deletions(-) diff --git a/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg b/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg index 914709810..884c78717 100644 --- a/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg +++ b/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg @@ -15,7 +15,7 @@ id="svg3604" version="1.1" inkscape:version="0.48.4 r9939" - sodipodi:docname="system-health-mod.svg" + sodipodi:docname="system-health.svg" inkscape:export-filename="C:\NoBackup\OpenPilot\mainboard-health.png" inkscape:export-xdpi="269.53" inkscape:export-ydpi="269.53" @@ -27,19 +27,20 @@ borderopacity="1.0" inkscape:pageopacity="0.0" inkscape:pageshadow="2" - inkscape:zoom="10.554213" - inkscape:cx="47.1292" - inkscape:cy="39.787519" - inkscape:current-layer="svg3604" + inkscape:zoom="29.851822" + inkscape:cx="82.604436" + inkscape:cy="54.252252" + inkscape:current-layer="background" id="namedview3608" - showgrid="false" - inkscape:window-width="1920" - inkscape:window-height="1025" - inkscape:window-x="-2" - inkscape:window-y="-3" + showgrid="true" + inkscape:window-width="1366" + inkscape:window-height="712" + inkscape:window-x="-4" + inkscape:window-y="-4" inkscape:window-maximized="1" showguides="true" - inkscape:guide-bbox="true"> + inkscape:guide-bbox="true" + inkscape:snap-global="false"> + + @@ -521,17 +529,6 @@ inkscape:vp_z="1 : 0.5 : 1" inkscape:persp3d-origin="0.5 : 0.33333333 : 1" id="perspective3185" /> - - - - + + + + + + + + @@ -643,2845 +693,2317 @@ inkscape:groupmode="layer" inkscape:label="Background"> - - - - - - - - - - - + style="fill:#483737;fill-opacity:1;fill-rule:evenodd;stroke:#000000;stroke-width:1.00617588;stroke-linecap:butt;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none" /> + style="fill:#241c1c;fill-opacity:1;stroke:#ffffff;stroke-width:0.45410183;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + id="rect4550-8-7-82" + width="10.470912" + height="8.5405388" + x="547.97754" + y="411.38937" + ry="1" /> + + width="19.129522" + height="10.639811" + x="504.99179" + y="387.5629" + ry="0.24124773" + inkscape:label="#rect4550" /> + style="fill:#6c5d53;fill-opacity:1;stroke:none;display:inline" + id="rect4358-7" + width="37.799622" + height="13.255064" + x="553.24451" + y="395.2457" + ry="0" /> + style="fill:#6c5d53;fill-opacity:1;stroke:none;display:inline" + id="rect4358-4" + width="63.468098" + height="12.839675" + x="527.57617" + y="381.38202" + ry="0" /> + style="fill:#6c5d53;fill-opacity:1;stroke:none;display:inline" + id="rect4358-0" + width="91.066727" + height="13.183137" + x="499.96637" + y="346.05209" + ry="0" /> + style="fill:#6c5d53;fill-opacity:1;stroke:none;display:inline" + id="rect4358-0-9" + width="91.019348" + height="13.183137" + x="500.06686" + y="360.27713" + ry="0" /> + style="fill:#6c5d53;fill-opacity:1;stroke:none;display:inline" + id="rect4358-7-4" + width="24.646557" + height="13.221018" + x="527.58087" + y="395.29047" + ry="0" /> - - - - - - + width="10.470912" + height="8.5405388" + x="532.78839" + y="411.4689" + ry="1" + inkscape:label="#rect4550-8" /> - + width="10.470912" + height="8.5405388" + x="544.5553" + y="411.4689" + ry="1" + inkscape:label="#rect4550-8-7" /> + + width="10.470912" + height="8.5405388" + x="568.08905" + y="411.4689" + ry="1" + inkscape:label="#rect4550-8-15" /> + style="fill:#241c1c;fill-opacity:1;stroke:none;display:inline" + id="I2C" + width="10.470912" + height="8.5405388" + x="579.8559" + y="411.4689" + ry="1" + inkscape:label="#rect4550-8-2" /> + SYSTEM HEALTH + Sensors + Auto + Misc. + Link + Power + Syst. - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + inkscape:label="#g3426" + transform="translate(14,0)"> + id="path6233-9-9-86-0" + d="m 46.502131,52.910887 14.610607,9.14342" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.09097731;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" /> + id="path6233-9-9-4-5-6" + d="M 46.442675,61.953487 61.145921,52.934252" + style="fill:#ff0000;stroke:#ff0000;stroke-width:1.08697307;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" /> - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + style="fill:none;stroke:#000000;stroke-width:1.00617588;stroke-linecap:butt;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" + id="rect4795" + width="95.066467" + height="79.056633" + x="0.25920519" + y="0.25921404" + ry="1" /> + transform="scale(0.83127393,1.2029729)">PATH Boot Fault + id="tspan4291" + x="93.860985" + y="8.3444462">PLAN Battery + id="tspan3458" + x="10.759087" + y="8.3200321">ATTITUDE Sensors + id="tspan4287" + x="41.377663" + y="8.3434696">STAB Airspeed + id="tspan5866" + x="16.329399" + y="20.115072">GPS + SENSORS + AIRSPEED + MAG + CPU + EVENT + MEM. + STACK + I2C + CONF. + BOOT + TELEM. + BATT. + TIME + + + + + + + + + + + + + + + + + + + + + + + + INPUT + + + OUTPUT From 14652440a2e6c055ea5a8525c3fbe7e507940668 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Tue, 6 May 2014 18:46:58 +0200 Subject: [PATCH 06/10] OP-1161 skip magnetometer entirelly if complementary (no mag) mode, thus not triggering magnetometer alarms --- flight/modules/StateEstimation/stateestimation.c | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index 3f77a1d8f..3b5f4b9b3 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -161,17 +161,14 @@ static float gyroDelta[3]; // preconfigured filter chains selectable via revoSettings.FusionAlgorithm static const filterPipeline *cfQueue = &(filterPipeline) { - .filter = &magFilter, + .filter = &airFilter, .next = &(filterPipeline) { - .filter = &airFilter, + .filter = &baroiFilter, .next = &(filterPipeline) { - .filter = &baroiFilter, + .filter = &altitudeFilter, .next = &(filterPipeline) { - .filter = &altitudeFilter, - .next = &(filterPipeline) { - .filter = &cfFilter, - .next = NULL, - } + .filter = &cfFilter, + .next = NULL, } } } From 56b5747b4e0eee93724a4c2b1f031eeff8062da8 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Tue, 6 May 2014 18:53:57 +0200 Subject: [PATCH 07/10] uncrustify --- flight/modules/Sensors/sensors.c | 20 ++++---- .../calibration/sixpointcalibrationmodel.cpp | 49 +++++++++---------- .../calibration/sixpointcalibrationmodel.h | 10 ++-- 3 files changed, 40 insertions(+), 39 deletions(-) diff --git a/flight/modules/Sensors/sensors.c b/flight/modules/Sensors/sensors.c index b21b67fd4..c1c76ee8f 100644 --- a/flight/modules/Sensors/sensors.c +++ b/flight/modules/Sensors/sensors.c @@ -84,7 +84,9 @@ AccelGyroSettingsData agcal; // These values are initialized by settings but can be updated by the attitude algorithm static float mag_bias[3] = { 0, 0, 0 }; -static float mag_transform[3][3]={{ 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1}}; +static float mag_transform[3][3] = { + { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } +}; // temp coefficient to calculate gyro bias static volatile bool gyro_temp_calibrated = false; static volatile bool accel_temp_calibrated = false; @@ -442,9 +444,9 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv) { RevoCalibrationGet(&cal); AccelGyroSettingsGet(&agcal); - mag_bias[0] = cal.mag_bias.X; - mag_bias[1] = cal.mag_bias.Y; - mag_bias[2] = cal.mag_bias.Z; + mag_bias[0] = cal.mag_bias.X; + mag_bias[1] = cal.mag_bias.Y; + mag_bias[2] = cal.mag_bias.Z; 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); @@ -459,16 +461,16 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv) // Indicates not to expend cycles on rotation if (fabsf(attitudeSettings.BoardRotation.Roll) < 0.00001f - && fabsf(attitudeSettings.BoardRotation.Pitch) < 0.00001f && - fabsf(attitudeSettings.BoardRotation.Yaw) <0.00001f ) { + && fabsf(attitudeSettings.BoardRotation.Pitch) < 0.00001f && + fabsf(attitudeSettings.BoardRotation.Yaw) < 0.00001f) { rotate = 0; } else { rotate = 1; } float rotationQuat[4]; - const float rpy[3] = { attitudeSettings.BoardRotation.Roll, - attitudeSettings.BoardRotation.Pitch, - attitudeSettings.BoardRotation.Yaw }; + const float rpy[3] = { attitudeSettings.BoardRotation.Roll, + attitudeSettings.BoardRotation.Pitch, + attitudeSettings.BoardRotation.Yaw }; RPY2Quaternion(rpy, rotationQuat); Quaternion2R(rotationQuat, R); diff --git a/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp b/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp index 5f9961e43..becec9b3e 100644 --- a/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp +++ b/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp @@ -47,36 +47,35 @@ SixPointCalibrationModel::SixPointCalibrationModel(QObject *parent) : calibratingMag(false), calibratingAccel(false), collectingData(false) - { calibrationStepsMag.clear(); calibrationStepsMag - << CalibrationStep(CALIBRATION_HELPER_IMAGE_NED, - tr("Place horizontally, nose pointing north and click save position...")) - << CalibrationStep(CALIBRATION_HELPER_IMAGE_DWN, - tr("Place with nose down, right side west and click save position...")) - << CalibrationStep(CALIBRATION_HELPER_IMAGE_WDS, - tr("Place right side down, nose west and click save position...")) - << CalibrationStep(CALIBRATION_HELPER_IMAGE_ENU, - tr("Place upside down, nose east and click save position...")) - << CalibrationStep(CALIBRATION_HELPER_IMAGE_USE, - tr("Place with nose up, left side north and click save position...")) - << CalibrationStep(CALIBRATION_HELPER_IMAGE_SUW, - tr("Place with left side down, nose south and click save position...")); + << CalibrationStep(CALIBRATION_HELPER_IMAGE_NED, + tr("Place horizontally, nose pointing north and click save position...")) + << CalibrationStep(CALIBRATION_HELPER_IMAGE_DWN, + tr("Place with nose down, right side west and click save position...")) + << CalibrationStep(CALIBRATION_HELPER_IMAGE_WDS, + tr("Place right side down, nose west and click save position...")) + << CalibrationStep(CALIBRATION_HELPER_IMAGE_ENU, + tr("Place upside down, nose east and click save position...")) + << CalibrationStep(CALIBRATION_HELPER_IMAGE_USE, + tr("Place with nose up, left side north and click save position...")) + << CalibrationStep(CALIBRATION_HELPER_IMAGE_SUW, + tr("Place with left side down, nose south and click save position...")); calibrationStepsAccelOnly.clear(); calibrationStepsAccelOnly << CalibrationStep(CALIBRATION_HELPER_IMAGE_NED, - tr("Place horizontally and click save position...")) - << CalibrationStep(CALIBRATION_HELPER_IMAGE_DWN, - tr("Place with nose down and click save position...")) - << CalibrationStep(CALIBRATION_HELPER_IMAGE_WDS, - tr("Place right side down and click save position...")) - << CalibrationStep(CALIBRATION_HELPER_IMAGE_ENU, - tr("Place upside down and click save position...")) - << CalibrationStep(CALIBRATION_HELPER_IMAGE_USE, - tr("Place with nose up and click save position...")) - << CalibrationStep(CALIBRATION_HELPER_IMAGE_SUW, - tr("Place with left side down and click save position...")); + tr("Place horizontally and click save position...")) + << CalibrationStep(CALIBRATION_HELPER_IMAGE_DWN, + tr("Place with nose down and click save position...")) + << CalibrationStep(CALIBRATION_HELPER_IMAGE_WDS, + tr("Place right side down and click save position...")) + << CalibrationStep(CALIBRATION_HELPER_IMAGE_ENU, + tr("Place upside down and click save position...")) + << CalibrationStep(CALIBRATION_HELPER_IMAGE_USE, + tr("Place with nose up and click save position...")) + << CalibrationStep(CALIBRATION_HELPER_IMAGE_SUW, + tr("Place with left side down and click save position...")); } /********** Six point calibration **************/ @@ -103,7 +102,7 @@ void SixPointCalibrationModel::start(bool calibrateAccel, bool calibrateMag) // Store and reset board rotation before calibration starts storeAndClearBoardRotation(); - if(calibrateMag){ + if (calibrateMag) { currentSteps = &calibrationStepsMag; } else { currentSteps = &calibrationStepsAccelOnly; diff --git a/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.h b/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.h index cf453af33..128ff884a 100644 --- a/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.h +++ b/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.h @@ -41,15 +41,15 @@ namespace OpenPilot { class SixPointCalibrationModel : public QObject { Q_OBJECT - class CalibrationStep{ - public: - CalibrationStep(QString newVisualHelp, QString newInstructions){ - visualHelp = newVisualHelp; + class CalibrationStep { +public: + CalibrationStep(QString newVisualHelp, QString newInstructions) + { + visualHelp = newVisualHelp; instructions = newInstructions; } QString visualHelp; QString instructions; - }; typedef struct { From 1add404d8002dd3c481a685efacbc6927ad6155f Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Wed, 7 May 2014 23:47:14 +0200 Subject: [PATCH 08/10] OP-1161 reimplemented magnetometer plausibikity check using vector length only --- flight/modules/StateEstimation/filtermag.c | 101 +++++++++------------ 1 file changed, 44 insertions(+), 57 deletions(-) diff --git a/flight/modules/StateEstimation/filtermag.c b/flight/modules/StateEstimation/filtermag.c index 308d0b286..8adaee6a1 100644 --- a/flight/modules/StateEstimation/filtermag.c +++ b/flight/modules/StateEstimation/filtermag.c @@ -45,14 +45,13 @@ // Private types struct data { - HomeLocationData homeLocation; RevoCalibrationData revoCalibration; RevoSettingsData revoSettings; - uint16_t idlecounter; - uint8_t warningcount; - uint8_t errorcount; - float magAverage[3]; - float magBias[3]; + uint8_t warningcount; + uint8_t errorcount; + float homeLocationBe[3]; + float magBe2; + float magBias[3]; }; // Private variables @@ -78,10 +77,11 @@ static int32_t init(stateFilter *self) { struct data *this = (struct data *)self->localdata; - this->magBias[0] = this->magBias[1] = this->magBias[2] = 0.0f; - this->magAverage[0] = this->magAverage[1] = this->magAverage[2] = 0.0f; - this->idlecounter = this->warningcount = this->errorcount = 0; - HomeLocationGet(&this->homeLocation); + this->magBias[0] = this->magBias[1] = this->magBias[2] = 0.0f; + this->warningcount = this->errorcount = 0; + HomeLocationBeGet(this->homeLocationBe); + // magBe2 holds the squared magnetic vector length (extpected) + this->magBe2 = this->homeLocationBe[0] * this->homeLocationBe[0] + this->homeLocationBe[1] * this->homeLocationBe[1] + this->homeLocationBe[2] * this->homeLocationBe[2]; RevoCalibrationGet(&this->revoCalibration); RevoSettingsGet(&this->revoSettings); return 0; @@ -106,56 +106,43 @@ static int32_t filter(stateFilter *self, stateEstimation *state) */ static void checkMagValidity(struct data *this, float mag[3]) { - #define MAG_LOW_PASS_ALPHA 0.2f - #define IDLE_COUNT 10 - #define ALARM_THRESHOLD 3 + #define ALARM_THRESHOLD 5 - // low pass filter sensor to not give warnings due to noise - this->magAverage[0] = (1.0f - MAG_LOW_PASS_ALPHA) * this->magAverage[0] + MAG_LOW_PASS_ALPHA * mag[0]; - this->magAverage[1] = (1.0f - MAG_LOW_PASS_ALPHA) * this->magAverage[1] + MAG_LOW_PASS_ALPHA * mag[1]; - this->magAverage[2] = (1.0f - MAG_LOW_PASS_ALPHA) * this->magAverage[2] + MAG_LOW_PASS_ALPHA * mag[2]; + // mag2 holds the actual magnetic vector length (squared) + float mag2 = mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2]; - // throttle this check, thanks to low pass filter it is not necessary every iteration - if (!this->idlecounter--) { - this->idlecounter = IDLE_COUNT; + // warning and error thresholds + // avoud sqrt() : minlimithomeLocation.Be, expected); + float minWarning2 = this->magBe2 * (1.0f - 2.0f * this->revoSettings.MagnetometerMaxDeviation.Warning + this->revoSettings.MagnetometerMaxDeviation.Warning * this->revoSettings.MagnetometerMaxDeviation.Warning); + float maxWarning2 = this->magBe2 * (1.0f + 2.0f * this->revoSettings.MagnetometerMaxDeviation.Warning + this->revoSettings.MagnetometerMaxDeviation.Warning * this->revoSettings.MagnetometerMaxDeviation.Warning); + float minError2 = this->magBe2 * (1.0f - 2.0f * this->revoSettings.MagnetometerMaxDeviation.Error + this->revoSettings.MagnetometerMaxDeviation.Error * this->revoSettings.MagnetometerMaxDeviation.Error); + float maxError2 = this->magBe2 * (1.0f + 2.0f * this->revoSettings.MagnetometerMaxDeviation.Error + this->revoSettings.MagnetometerMaxDeviation.Error * this->revoSettings.MagnetometerMaxDeviation.Error); - // calculate maximum allowed deviation - float warning2 = expected[0] * expected[0] + expected[1] * expected[1] + expected[2] * expected[2]; - float error2 = this->revoSettings.MagnetometerMaxDeviation.Error * this->revoSettings.MagnetometerMaxDeviation.Error * warning2; - warning2 = this->revoSettings.MagnetometerMaxDeviation.Warning * this->revoSettings.MagnetometerMaxDeviation.Warning * warning2; - - // calculate difference - expected[0] = expected[0] - this->magAverage[0]; - expected[1] = expected[1] - this->magAverage[1]; - expected[2] = expected[2] - this->magAverage[2]; - float deviation2 = expected[0] * expected[0] + expected[1] * expected[1] + expected[2] * expected[2]; - - // set errors - if (deviation2 < warning2) { - this->warningcount = 0; - this->errorcount = 0; - AlarmsClear(SYSTEMALARMS_ALARM_MAGNETOMETER); - } else if (deviation2 < error2) { - this->errorcount = 0; - if (this->warningcount > ALARM_THRESHOLD) { - AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_WARNING); - } else { - this->warningcount++; - } + // set errors + if (minWarning2 < mag2 && mag2 < maxWarning2) { + this->warningcount = 0; + this->errorcount = 0; + AlarmsClear(SYSTEMALARMS_ALARM_MAGNETOMETER); + } else if (minError2 < mag2 && mag2 < maxError2) { + this->errorcount = 0; + if (this->warningcount > ALARM_THRESHOLD) { + AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_WARNING); } else { - if (this->errorcount > ALARM_THRESHOLD) { - AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_CRITICAL); - } else { - this->errorcount++; - } + this->warningcount++; + } + } else { + if (this->errorcount > ALARM_THRESHOLD) { + AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_CRITICAL); + } else { + this->errorcount++; } } } @@ -209,8 +196,8 @@ static void magOffsetEstimation(struct data *this, float mag[3]) } #else // if 0 - const float Rxy = sqrtf(this->homeLocation.Be[0] * this->homeLocation.Be[0] + this->homeLocation.Be[1] * this->homeLocation.Be[1]); - const float Rz = this->homeLocation.Be[2]; + const float Rxy = sqrtf(this->homeLocationBe[0] * this->homeLocationBe[0] + this->homeLocationBe[1] * this->homeLocationBe[1]); + const float Rz = this->homeLocationBe[2]; const float rate = this->revoCalibration.MagBiasNullingRate; float Rot[3][3]; From 4cf667123cf34ce4cde758ee158846741ea923cf Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Thu, 8 May 2014 00:20:00 +0200 Subject: [PATCH 09/10] OP-1161 changed default maximum warning and critical deviations for magnetometer --- shared/uavobjectdefinition/revosettings.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/shared/uavobjectdefinition/revosettings.xml b/shared/uavobjectdefinition/revosettings.xml index 9ad0f12d2..c1883ee73 100644 --- a/shared/uavobjectdefinition/revosettings.xml +++ b/shared/uavobjectdefinition/revosettings.xml @@ -9,7 +9,7 @@ - + From 48b9edc1e0701eb161ddc41eb849eace57bd739e Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Thu, 8 May 2014 21:21:50 +0200 Subject: [PATCH 10/10] uncrustify --- flight/modules/Sensors/sensors.c | 20 +++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/flight/modules/Sensors/sensors.c b/flight/modules/Sensors/sensors.c index b21b67fd4..c1c76ee8f 100644 --- a/flight/modules/Sensors/sensors.c +++ b/flight/modules/Sensors/sensors.c @@ -84,7 +84,9 @@ AccelGyroSettingsData agcal; // These values are initialized by settings but can be updated by the attitude algorithm static float mag_bias[3] = { 0, 0, 0 }; -static float mag_transform[3][3]={{ 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1}}; +static float mag_transform[3][3] = { + { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } +}; // temp coefficient to calculate gyro bias static volatile bool gyro_temp_calibrated = false; static volatile bool accel_temp_calibrated = false; @@ -442,9 +444,9 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv) { RevoCalibrationGet(&cal); AccelGyroSettingsGet(&agcal); - mag_bias[0] = cal.mag_bias.X; - mag_bias[1] = cal.mag_bias.Y; - mag_bias[2] = cal.mag_bias.Z; + mag_bias[0] = cal.mag_bias.X; + mag_bias[1] = cal.mag_bias.Y; + mag_bias[2] = cal.mag_bias.Z; 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); @@ -459,16 +461,16 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv) // Indicates not to expend cycles on rotation if (fabsf(attitudeSettings.BoardRotation.Roll) < 0.00001f - && fabsf(attitudeSettings.BoardRotation.Pitch) < 0.00001f && - fabsf(attitudeSettings.BoardRotation.Yaw) <0.00001f ) { + && fabsf(attitudeSettings.BoardRotation.Pitch) < 0.00001f && + fabsf(attitudeSettings.BoardRotation.Yaw) < 0.00001f) { rotate = 0; } else { rotate = 1; } float rotationQuat[4]; - const float rpy[3] = { attitudeSettings.BoardRotation.Roll, - attitudeSettings.BoardRotation.Pitch, - attitudeSettings.BoardRotation.Yaw }; + const float rpy[3] = { attitudeSettings.BoardRotation.Roll, + attitudeSettings.BoardRotation.Pitch, + attitudeSettings.BoardRotation.Yaw }; RPY2Quaternion(rpy, rotationQuat); Quaternion2R(rotationQuat, R);