Merged in f5soh/librepilot/LP-482_Mag_Kp (pull request #439)

f5soh · filnet · commit 319a7d1d38ca · 2017-06-26T19:16:49.000Z
LP-482 Mag Kp + LP-534 Mag source EKF

Approved-by: Alessio Morale &lt;alessiomorale@gmail.com&gt;
Approved-by: Philippe Renon &lt;philippe_renon@yahoo.fr&gt;
Approved-by: Lalanne Laurent &lt;f5soh@free.fr&gt;
Approved-by: Brian Webb &lt;webbbn@gmail.com&gt;
diff --git 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,14 @@
 
 #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;
@@ -68,6 +76,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];
@@ -248,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
@@ -288,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]);
@@ -304,36 +313,40 @@ 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 ? 1.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;
         this->attitudeSettings.AccelKi     = 0.0f;
         this->attitudeSettings.YawBiasRate = 0.23f;
         this->accel_filter_enabled   = false;
         this->rollPitchBiasRate      = 0.01f;
-        this->attitudeSettings.MagKp = this->magCalibrated ? 1.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) {
-        // Reload settings (all the rates)
-        AttitudeSettingsGet(&this->attitudeSettings);
+    } else if (this->init == UNDONE) {
         this->rollPitchBiasRate = 0.0f;
         if (this->accel_alpha > 0.0f) {
             this->accel_filter_enabled = true;
         }
-        this->init = 1;
+        this->inittime = xTaskGetTickCount();
+        this->init     = DONE;
+        // Allow running filter with custom MagKp for some time before reload settings
+    } 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 = RUN;
     }
 
     // Compute the dT using the cpu clock
@@ -423,7 +436,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;
     }
@@ -473,7 +486,7 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float
         return FILTERRESULT_WARNING;
     }
 
-    if (this->init) {
+    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
diff --git 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;
diff --git a/shared/uavobjectdefinition/attitudesettings.xml b/shared/uavobjectdefinition/attitudesettings.xml
@@ -5,8 +5,8 @@
         <field name="BoardLevelTrim" units="deg" type="float" elementnames="Roll,Pitch" defaultvalue="0,0"/>
         <field name="AccelKp" units="channel" type="float" elements="1" defaultvalue="0.05"/>
         <field name="AccelKi" units="channel" type="float" elements="1" defaultvalue="0.0001"/>
-        <field name="MagKi" units="" type="float" elements="1" defaultvalue="0.000001"/>
-        <field name="MagKp" units="" type="float" elements="1" defaultvalue="0.01"/>
+        <field name="MagKi" units="" type="float" elements="1" defaultvalue="0.00005"/>
+        <field name="MagKp" units="" type="float" elements="1" defaultvalue="1.0"/>
         <field name="AccelTau" units="" type="float" elements="1" defaultvalue="0.05"/>
         <field name="YawBiasRate" units="channel" type="float" elements="1" defaultvalue="0.000001"/>
         <field name="ZeroDuringArming" units="channel" type="enum" elements="1" options="False,True" defaultvalue="True"/>
