From ee7d630e3e6fcc38ad2653741e992806baae2f37 Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Sat, 17 Jun 2017 16:49:40 +0200 Subject: [PATCH 1/6] LP-482 Fix Complementary Mag integral sign --- flight/modules/StateEstimation/filtercf.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/modules/StateEstimation/filtercf.c b/flight/modules/StateEstimation/filtercf.c index fae8d1f8c..714f5d938 100644 --- a/flight/modules/StateEstimation/filtercf.c +++ b/flight/modules/StateEstimation/filtercf.c @@ -423,7 +423,7 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float this->gyroBias[0] -= accel_err[0] * this->attitudeSettings.AccelKi - gyro[0] * this->rollPitchBiasRate; this->gyroBias[1] -= accel_err[1] * this->attitudeSettings.AccelKi - gyro[1] * this->rollPitchBiasRate; if (this->useMag) { - this->gyroBias[2] -= -mag_err[2] * this->attitudeSettings.MagKi - gyro[2] * this->rollPitchBiasRate; + this->gyroBias[2] -= mag_err[2] * this->attitudeSettings.MagKi - gyro[2] * this->rollPitchBiasRate; } else { this->gyroBias[2] -= -gyro[2] * this->rollPitchBiasRate; } From 684e0dc14f1e517fe88bce5c2213e270a3a03f15 Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Sat, 17 Jun 2017 16:50:58 +0200 Subject: [PATCH 2/6] LP-482 Increase temporary MagKp for initialisation --- flight/modules/StateEstimation/filtercf.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/flight/modules/StateEstimation/filtercf.c b/flight/modules/StateEstimation/filtercf.c index 714f5d938..62c15adcf 100644 --- a/flight/modules/StateEstimation/filtercf.c +++ b/flight/modules/StateEstimation/filtercf.c @@ -315,7 +315,7 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float this->attitudeSettings.YawBiasRate = 0.23f; this->accel_filter_enabled = false; this->rollPitchBiasRate = 0.01f; - this->attitudeSettings.MagKp = this->magCalibrated ? 1.0f : 0.0f; + this->attitudeSettings.MagKp = this->magCalibrated ? 20.0f : 0.0f; PIOS_NOTIFY_StartNotification(NOTIFY_DRAW_ATTENTION, NOTIFY_PRIORITY_REGULAR); } else if ((this->attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) { this->attitudeSettings.AccelKp = 1.0f; @@ -323,7 +323,7 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float this->attitudeSettings.YawBiasRate = 0.23f; this->accel_filter_enabled = false; this->rollPitchBiasRate = 0.01f; - this->attitudeSettings.MagKp = this->magCalibrated ? 1.0f : 0.0f; + this->attitudeSettings.MagKp = this->magCalibrated ? 20.0f : 0.0f; this->init = 0; PIOS_NOTIFY_StartNotification(NOTIFY_DRAW_ATTENTION, NOTIFY_PRIORITY_REGULAR); } else if (this->init == 0) { From ad23d188b5eb1241fe605221c851f380ddd74e2d Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Sat, 17 Jun 2017 17:03:09 +0200 Subject: [PATCH 3/6] LP-482 Complementary+Mag : Increase MagKp and MagKi values --- shared/uavobjectdefinition/attitudesettings.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/shared/uavobjectdefinition/attitudesettings.xml b/shared/uavobjectdefinition/attitudesettings.xml index e45ec747d..82540609a 100644 --- a/shared/uavobjectdefinition/attitudesettings.xml +++ b/shared/uavobjectdefinition/attitudesettings.xml @@ -5,8 +5,8 @@ - - + + From ed28311841cf1600db5b125434d7f9229a374d79 Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Sat, 17 Jun 2017 19:21:55 +0200 Subject: [PATCH 4/6] LP-482 Complementary+Mag : Allow fast convergence for mag --- flight/modules/StateEstimation/filtercf.c | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/flight/modules/StateEstimation/filtercf.c b/flight/modules/StateEstimation/filtercf.c index 62c15adcf..8850880ef 100644 --- a/flight/modules/StateEstimation/filtercf.c +++ b/flight/modules/StateEstimation/filtercf.c @@ -7,7 +7,8 @@ * @{ * * @file filtercf.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013. * @brief Complementary filter to calculate Attitude from Accels and Gyros * and optionally magnetometers: * WARNING: Will drift if the mean acceleration force doesn't point @@ -49,7 +50,9 @@ #define CALIBRATION_DELAY_MS 4000 #define CALIBRATION_DURATION_MS 6000 +#define RELOADSETTINGS_DELAY_MS 1000 #define VARIANCE_WINDOW_SIZE 40 + // Private types struct data { AttitudeSettingsData attitudeSettings; @@ -68,6 +71,7 @@ struct data { float rollPitchBiasRate; int32_t timeval; int32_t starttime; + int32_t inittime; uint8_t init; bool magCalibrated; pw_variance_t gyro_var[3]; @@ -327,13 +331,17 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float this->init = 0; PIOS_NOTIFY_StartNotification(NOTIFY_DRAW_ATTENTION, NOTIFY_PRIORITY_REGULAR); } else if (this->init == 0) { - // Reload settings (all the rates) - AttitudeSettingsGet(&this->attitudeSettings); this->rollPitchBiasRate = 0.0f; if (this->accel_alpha > 0.0f) { this->accel_filter_enabled = true; } - this->init = 1; + this->inittime = xTaskGetTickCount(); + this->init = 1; + // Allow running filter with custom MagKp for some time before reload settings + } else if (this->init == 1 && (!this->magCalibrated || (xTaskGetTickCount() - this->inittime > RELOADSETTINGS_DELAY_MS / portTICK_RATE_MS))) { + // Reload settings (all the rates) + AttitudeSettingsGet(&this->attitudeSettings); + this->init = 2; } // Compute the dT using the cpu clock @@ -473,7 +481,7 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float return FILTERRESULT_WARNING; } - if (this->init) { + if (this->init > 0) { return FILTERRESULT_OK; } else { return FILTERRESULT_CRITICAL; // return "critical" for now, so users can see the zeroing period, switch to more graceful notification later From c2a7582bd9a40c6de69322ac2e3ce52ba64207c8 Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Mon, 19 Jun 2017 09:38:05 +0200 Subject: [PATCH 5/6] LP-482 Changes from review, add defines --- flight/modules/StateEstimation/filtercf.c | 29 +++++++++++++---------- 1 file changed, 17 insertions(+), 12 deletions(-) diff --git a/flight/modules/StateEstimation/filtercf.c b/flight/modules/StateEstimation/filtercf.c index 8850880ef..e72243762 100644 --- a/flight/modules/StateEstimation/filtercf.c +++ b/flight/modules/StateEstimation/filtercf.c @@ -51,8 +51,13 @@ #define CALIBRATION_DELAY_MS 4000 #define CALIBRATION_DURATION_MS 6000 #define RELOADSETTINGS_DELAY_MS 1000 +#define CONVERGENCE_MAGKP 20.0f #define VARIANCE_WINDOW_SIZE 40 +#define UNDONE 0 +#define DONE 1 +#define RUN 2 + // Private types struct data { AttitudeSettingsData attitudeSettings; @@ -252,7 +257,7 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float AttitudeStateData attitudeState; // base on previous state AttitudeStateGet(&attitudeState); - this->init = 0; + this->init = UNDONE; // Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly, // so pseudo "north" vector can be estimated even if the board is not level @@ -292,7 +297,7 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float return FILTERRESULT_OK; // must return OK on initial initialization, so attitude will init with a valid quaternion } // check whether copter is steady - if (this->init == 0 && this->attitudeSettings.InitialZeroWhenBoardSteady == ATTITUDESETTINGS_INITIALZEROWHENBOARDSTEADY_TRUE) { + if (this->init == UNDONE && this->attitudeSettings.InitialZeroWhenBoardSteady == ATTITUDESETTINGS_INITIALZEROWHENBOARDSTEADY_TRUE) { pseudo_windowed_variance_push_sample(&this->gyro_var[0], gyro[0]); pseudo_windowed_variance_push_sample(&this->gyro_var[1], gyro[1]); pseudo_windowed_variance_push_sample(&this->gyro_var[2], gyro[2]); @@ -308,18 +313,18 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float } - if (this->init == 0 && xTaskGetTickCount() - this->starttime < CALIBRATION_DELAY_MS / portTICK_RATE_MS) { + if (this->init == UNDONE && xTaskGetTickCount() - this->starttime < CALIBRATION_DELAY_MS / portTICK_RATE_MS) { // wait 4 seconds for the user to get his hands off in case the board was just powered this->timeval = PIOS_DELAY_GetRaw(); return FILTERRESULT_ERROR; - } else if (this->init == 0 && xTaskGetTickCount() - this->starttime < (CALIBRATION_DELAY_MS + CALIBRATION_DURATION_MS) / portTICK_RATE_MS) { + } else if (this->init == UNDONE && xTaskGetTickCount() - this->starttime < (CALIBRATION_DELAY_MS + CALIBRATION_DURATION_MS) / portTICK_RATE_MS) { // For first 6 seconds use accels to get gyro bias this->attitudeSettings.AccelKp = 1.0f; this->attitudeSettings.AccelKi = 0.0f; this->attitudeSettings.YawBiasRate = 0.23f; this->accel_filter_enabled = false; this->rollPitchBiasRate = 0.01f; - this->attitudeSettings.MagKp = this->magCalibrated ? 20.0f : 0.0f; + this->attitudeSettings.MagKp = this->magCalibrated ? CONVERGENCE_MAGKP : 0.0f; PIOS_NOTIFY_StartNotification(NOTIFY_DRAW_ATTENTION, NOTIFY_PRIORITY_REGULAR); } else if ((this->attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) { this->attitudeSettings.AccelKp = 1.0f; @@ -327,21 +332,21 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float this->attitudeSettings.YawBiasRate = 0.23f; this->accel_filter_enabled = false; this->rollPitchBiasRate = 0.01f; - this->attitudeSettings.MagKp = this->magCalibrated ? 20.0f : 0.0f; - this->init = 0; + this->attitudeSettings.MagKp = this->magCalibrated ? CONVERGENCE_MAGKP : 0.0f; + this->init = UNDONE; PIOS_NOTIFY_StartNotification(NOTIFY_DRAW_ATTENTION, NOTIFY_PRIORITY_REGULAR); - } else if (this->init == 0) { + } else if (this->init == UNDONE) { this->rollPitchBiasRate = 0.0f; if (this->accel_alpha > 0.0f) { this->accel_filter_enabled = true; } this->inittime = xTaskGetTickCount(); - this->init = 1; + this->init = DONE; // Allow running filter with custom MagKp for some time before reload settings - } else if (this->init == 1 && (!this->magCalibrated || (xTaskGetTickCount() - this->inittime > RELOADSETTINGS_DELAY_MS / portTICK_RATE_MS))) { + } else if (this->init == DONE && (!this->magCalibrated || (xTaskGetTickCount() - this->inittime > RELOADSETTINGS_DELAY_MS / portTICK_RATE_MS))) { // Reload settings (all the rates) AttitudeSettingsGet(&this->attitudeSettings); - this->init = 2; + this->init = RUN; } // Compute the dT using the cpu clock @@ -481,7 +486,7 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float return FILTERRESULT_WARNING; } - if (this->init > 0) { + if (this->init != UNDONE) { return FILTERRESULT_OK; } else { return FILTERRESULT_CRITICAL; // return "critical" for now, so users can see the zeroing period, switch to more graceful notification later From 1b624d7de3fb36293e171cdcb01155f22219a29b Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Mon, 19 Jun 2017 12:10:06 +0200 Subject: [PATCH 6/6] LP-534 MagStatus trouble: disable the corrected mag debug from EKF --- flight/modules/StateEstimation/filterekf.c | 20 +++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/flight/modules/StateEstimation/filterekf.c b/flight/modules/StateEstimation/filterekf.c index 88943a2f4..25d463013 100644 --- a/flight/modules/StateEstimation/filterekf.c +++ b/flight/modules/StateEstimation/filterekf.c @@ -223,7 +223,6 @@ static filterResult filter(stateFilter *self, stateEstimation *state) UNSET_MASK(state->updated, SENSORUPDATES_vel); UNSET_MASK(state->updated, SENSORUPDATES_attitude); UNSET_MASK(state->updated, SENSORUPDATES_gyro); - UNSET_MASK(state->updated, SENSORUPDATES_mag); return FILTERRESULT_OK; } @@ -387,15 +386,18 @@ static filterResult filter(stateFilter *self, stateEstimation *state) local_down[2] *= MagStrength; rot_mult(R, local_down, this->work.mag); } + // From Eric: "exporting it in MagState was meant for debugging, but I think it makes a + // lot of sense to have a "corrected" magnetometer reading available in the system." + // TODO: Should move above calc to filtermag, updating from here cause trouble with the state->MagStatus (LP-534) // debug rotated mags - state->mag[0] = this->work.mag[0]; - state->mag[1] = this->work.mag[1]; - state->mag[2] = this->work.mag[2]; - state->updated |= SENSORUPDATES_mag; - } else { - // mag state is delayed until EKF processed it, allows overriding/debugging magnetometer estimate - UNSET_MASK(state->updated, SENSORUPDATES_mag); - } + // state->mag[0] = this->work.mag[0]; + // state->mag[1] = this->work.mag[1]; + // state->mag[2] = this->work.mag[2]; + // state->updated |= SENSORUPDATES_mag; + } // else { + // mag state is delayed until EKF processed it, allows overriding/debugging magnetometer estimate + // UNSET_MASK(state->updated, SENSORUPDATES_mag); + // } if (IS_SET(this->work.updated, SENSORUPDATES_baro)) { sensors |= BARO_SENSOR;