Merged in f5soh/librepilot/LP-613_cfGPS_algo (pull request #533)

LP-613 cfGPS algo

Approved-by: Lalanne Laurent <f5soh@free.fr>
Approved-by: Vladimir Zidar <mr_w@mindnever.org>

flight/libraries/sanitycheck.c

@@ -77,6 +77,9 @@ int32_t configuration_check()
     RevoSettingsFusionAlgorithmGet(&revoFusion);
     bool navCapableFusion;
     switch (revoFusion) {
+    case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYGPSOUTDOOR:
+        navCapableFusion = (GetCurrentFrameType() == FRAME_TYPE_FIXED_WING) ? true : false;
+        break;
     case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAGGPSOUTDOOR:
     case REVOSETTINGS_FUSIONALGORITHM_GPSNAVIGATIONINS13:
     case REVOSETTINGS_FUSIONALGORITHM_GPSNAVIGATIONINS13CF:

flight/modules/StateEstimation/filtercf.c

@@ -64,13 +64,16 @@ struct data {
     HomeLocationData     homeLocation;
     bool    first_run;
     bool    useMag;
+    bool    useVelHeading;
     float   currentAccel[3];
     float   currentMag[3];
+    float   currentVel[3];
     float   accels_filtered[3];
     float   grot_filtered[3];
     float   gyroBias[3];
     bool    accelUpdated;
     bool    magUpdated;
+    bool    velUpdated;
     float   accel_alpha;
     bool    accel_filter_enabled;
     float   rollPitchBiasRate;
@@ -90,9 +93,10 @@ static FlightStatusData flightStatus;
 
 static int32_t initwithmag(stateFilter *self);
 static int32_t initwithoutmag(stateFilter *self);
+static int32_t initwithvelheading(stateFilter *self);
 static int32_t maininit(stateFilter *self);
 static filterResult filter(stateFilter *self, stateEstimation *state);
-static filterResult complementaryFilter(struct data *this, float gyro[3], float accel[3], float mag[3], float attitude[4]);
+static filterResult complementaryFilter(struct data *this, float gyro[3], float accel[3], float mag[3], float velheading[2], float attitude[4]);
 
 static void flightStatusUpdatedCb(UAVObjEvent *ev);
 
@@ -118,6 +122,15 @@ int32_t filterCFInitialize(stateFilter *handle)
     return STACK_REQUIRED;
 }
 
+int32_t filterCFHInitialize(stateFilter *handle)
+{
+    globalInit();
+    handle->init      = &initwithvelheading;
+    handle->filter    = &filter;
+    handle->localdata = pios_malloc(sizeof(struct data));
+    return STACK_REQUIRED;
+}
+
 int32_t filterCFMInitialize(stateFilter *handle)
 {
     globalInit();
@@ -132,6 +145,16 @@ static int32_t initwithmag(stateFilter *self)
     struct data *this = (struct data *)self->localdata;
 
     this->useMag = 1;
+    this->useVelHeading = 0;
+    return maininit(self);
+}
+
+static int32_t initwithvelheading(stateFilter *self)
+{
+    struct data *this = (struct data *)self->localdata;
+
+    this->useMag = 0;
+    this->useVelHeading = 1;
     return maininit(self);
 }
 
@@ -140,6 +163,7 @@ static int32_t initwithoutmag(stateFilter *self)
     struct data *this = (struct data *)self->localdata;
 
     this->useMag = 0;
+    this->useVelHeading = 0;
     return maininit(self);
 }
 
@@ -184,6 +208,12 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
         this->currentMag[1] = state->mag[1];
         this->currentMag[2] = state->mag[2];
     }
+    if (IS_SET(state->updated, SENSORUPDATES_vel)) {
+        this->velUpdated    = 1;
+        this->currentVel[0] = state->vel[0];
+        this->currentVel[1] = state->vel[1];
+        this->currentVel[2] = state->vel[2];
+    }
     if (IS_SET(state->updated, SENSORUPDATES_accel)) {
         this->accelUpdated    = 1;
         this->currentAccel[0] = state->accel[0];
@@ -193,7 +223,7 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
     if (IS_SET(state->updated, SENSORUPDATES_gyro)) {
         if (this->accelUpdated) {
             float attitude[4];
-            result = complementaryFilter(this, state->gyro, this->currentAccel, this->currentMag, attitude);
+            result = complementaryFilter(this, state->gyro, this->currentAccel, this->currentMag, this->currentVel, attitude);
             if (result == FILTERRESULT_OK) {
                 state->attitude[0] = attitude[0];
                 state->attitude[1] = attitude[1];
@@ -203,6 +233,7 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
             }
             this->accelUpdated = 0;
             this->magUpdated   = 0;
+            this->velUpdated   = 0;
         }
     }
     return result;
@@ -222,7 +253,7 @@ static inline void apply_accel_filter(const struct data *this, const float *raw,
     }
 }
 
-static filterResult complementaryFilter(struct data *this, float gyro[3], float accel[3], float mag[3], float attitude[4])
+static filterResult complementaryFilter(struct data *this, float gyro[3], float accel[3], float mag[3], float vel[3], float attitude[4])
 {
     float dT;
 
@@ -396,7 +427,7 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float
     accel_err[2] /= (accel_mag * grot_mag);
 
     float mag_err[3] = { 0.0f };
-    if (this->magUpdated && this->useMag) {
+    if (this->magUpdated && this->useMag && !this->useVelHeading) {
         // Rotate gravity to body frame and cross with accels
         float brot[3];
         float Rbe[3][3];
@@ -421,6 +452,31 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float
         } else {
             CrossProduct((const float *)mag, (const float *)brot, mag_err);
         }
+    } else if (this->velUpdated && this->useVelHeading) {
+        float speed = sqrtf(vel[0] * vel[0] + vel[1] * vel[1]);
+        if (speed > 3.0f) { // ~11kmh
+            float rpy[3];
+
+            Quaternion2RPY(attitude, rpy);
+            float heading = RAD2DEG(atan2f(-vel[1], -vel[0])) + 180.0f;
+
+            // find shortest way
+            float modulo  = fmodf((heading - rpy[2]) + 180.0f, 360.0f);
+            if (modulo < 0) {
+                modulo += 180.0f;
+            } else {
+                modulo -= 180.0f;
+            }
+            /* From dRonin: The 0.008 is chosen to put things in a somewhat similar scale
+             * to the cross product.  A 45 degree heading error will become
+             * a 7 deg/sec correction, so it neither takes forever to
+             * correct nor does a second of bad heading data screw us up
+             * too badly.
+             */
+            mag_err[2] = modulo * 0.008f;
+        } else {
+            mag_err[0] = mag_err[1] = mag_err[2] = 0.0f;
+        }
     } else {
         mag_err[0] = mag_err[1] = mag_err[2] = 0.0f;
     }
@@ -433,7 +489,7 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float
     // Accumulate integral of error.  Scale here so that units are (deg/s) but Ki has units of s
     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) {
+    if (this->useMag || this->useVelHeading) {
         this->gyroBias[2] -= mag_err[2] * this->attitudeSettings.MagKi - gyro[2] * this->rollPitchBiasRate;
     } else {
         this->gyroBias[2] -= -gyro[2] * this->rollPitchBiasRate;
@@ -443,7 +499,7 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float
     // Correct rates based on proportional coefficient
     gyrotmp[0] += accel_err[0] * this->attitudeSettings.AccelKp / dT;
     gyrotmp[1] += accel_err[1] * this->attitudeSettings.AccelKp / dT;
-    if (this->useMag) {
+    if (this->useMag || this->useVelHeading) {
         gyrotmp[2] += accel_err[2] * this->attitudeSettings.AccelKp / dT + mag_err[2] * this->attitudeSettings.MagKp / dT;
     } else {
         gyrotmp[2] += accel_err[2] * this->attitudeSettings.AccelKp / dT;

flight/modules/StateEstimation/inc/stateestimation.h

@@ -95,6 +95,7 @@ int32_t filterAirInitialize(stateFilter *handle);
 int32_t filterStationaryInitialize(stateFilter *handle);
 int32_t filterLLAInitialize(stateFilter *handle);
 int32_t filterCFInitialize(stateFilter *handle);
+int32_t filterCFHInitialize(stateFilter *handle);
 int32_t filterCFMInitialize(stateFilter *handle);
 int32_t filterEKF13iInitialize(stateFilter *handle);
 int32_t filterEKF13Initialize(stateFilter *handle);

flight/modules/StateEstimation/stateestimation.c

@@ -154,6 +154,7 @@ static stateFilter airFilter;
 static stateFilter stationaryFilter;
 static stateFilter llaFilter;
 static stateFilter cfFilter;
+static stateFilter cfhFilter;
 static stateFilter cfmFilter;
 static stateFilter ekf13iFilter;
 static stateFilter ekf13Filter;
@@ -184,6 +185,22 @@ static const filterPipeline *cfQueue = &(filterPipeline) {
         }
     }
 };
+static const filterPipeline *cfgpsQueue = &(filterPipeline) {
+    .filter = &airFilter,
+    .next   = &(filterPipeline) {
+        .filter = &llaFilter,
+        .next   = &(filterPipeline) {
+            .filter = &baroiFilter,
+            .next   = &(filterPipeline) {
+                .filter = &altitudeFilter,
+                .next   = &(filterPipeline) {
+                    .filter = &cfhFilter,
+                    .next   = NULL,
+                }
+            }
+        }
+    }
+};
 static const filterPipeline *cfmiQueue = &(filterPipeline) {
     .filter = &magFilter,
     .next   = &(filterPipeline) {
@@ -366,6 +383,7 @@ int32_t StateEstimationInitialize(void)
     stack_required = maxint32_t(stack_required, filterStationaryInitialize(&stationaryFilter));
     stack_required = maxint32_t(stack_required, filterLLAInitialize(&llaFilter));
     stack_required = maxint32_t(stack_required, filterCFInitialize(&cfFilter));
+    stack_required = maxint32_t(stack_required, filterCFHInitialize(&cfhFilter));
     stack_required = maxint32_t(stack_required, filterCFMInitialize(&cfmFilter));
     stack_required = maxint32_t(stack_required, filterEKF13iInitialize(&ekf13iFilter));
     stack_required = maxint32_t(stack_required, filterEKF13Initialize(&ekf13Filter));
@@ -443,6 +461,11 @@ static void StateEstimationCb(void)
                 // reinit Mag alarm
                 AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_UNINITIALISED);
                 break;
+            case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYGPSOUTDOOR:
+                newFilterChain = cfgpsQueue;
+                // reinit Mag alarm
+                AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_UNINITIALISED);
+                break;
             case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAG:
                 newFilterChain = cfmiQueue;
                 break;

ground/gcs/src/share/qml/js/uav.js

@@ -287,7 +287,7 @@ function gpsStatus() {
 }
 
 function fusionAlgorithm() {
-    var fusionAlgorithmText = ["None", "Basic (No Nav)", "CompMag", "Comp+Mag+GPS", "EKFIndoor", "GPSNav (INS)", "GPSNav (INS+CF)", "Testing (INS Indoor+CF)"];
+    var fusionAlgorithmText = ["None", "Basic (No Nav)", "Comp+GPS", "CompMag", "Comp+Mag+GPS", "EKFIndoor", "GPSNav (INS)", "GPSNav (INS+CF)", "Testing (INS Indoor+CF)", "Acro"];
 
     if (fusionAlgorithmText.length != RevoSettings.RevoSettingsConstants.FusionAlgorithmCount) {
         console.log("uav.js: fusionAlgorithm() do not match revoSettings.fusionAlgorithm uavo");

shared/uavobjectdefinition/revosettings.xml

@@ -2,7 +2,7 @@
     <object name="RevoSettings" singleinstance="true" settings="true" category="State">
         <description>Settings for the revo to control the algorithm and what is updated</description>
         <field name="FusionAlgorithm" units="" type="enum" elements="1" 
-        options="None,Basic (Complementary),Complementary+Mag,Complementary+Mag+GPSOutdoor,INS13Indoor,GPS Navigation (INS13),GPS Navigation (INS13+CF),Testing (INS Indoor+CF),Acro (No Sensors)" 
+        options="None,Basic (Complementary),Complementary+GPSOutdoor,Complementary+Mag,Complementary+Mag+GPSOutdoor,INS13Indoor,GPS Navigation (INS13),GPS Navigation (INS13+CF),Testing (INS Indoor+CF),Acro (No Sensors)" 
         defaultvalue="Basic (Complementary)"/>
 
         <!-- Low pass filter configuration to calculate offset of barometric altitude sensor.