From c2a7582bd9a40c6de69322ac2e3ce52ba64207c8 Mon Sep 17 00:00:00 2001
From: Laurent Lalanne <f5soh@free.fr>
Date: Mon, 19 Jun 2017 09:38:05 +0200
Subject: [PATCH] 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