Merge remote-tracking branch 'origin/next' into brian/GPIO_update

flight/modules/StateEstimation/filterbaro.c

@@ -32,19 +32,16 @@
 
 #include "inc/stateestimation.h"
 
+#include <revosettings.h>
+
 // Private constants
 
 #define STACK_REQUIRED 64
 
-// low pass filter configuration to calculate offset
-// of barometric altitude sensor
-// reasoning: updates at: 10 Hz, tau= 300 s settle time
-// exp(-(1/f) / tau ) ~=~ 0.9997
-#define BARO_OFFSET_LOWPASS_ALPHA 0.9997f
-
 // Private types
 struct data {
     float   baroOffset;
+    float   baroGPSOffsetCorrectionAlpha;
     float   baroAlt;
     int16_t first_run;
 };
@@ -71,6 +68,10 @@ static int32_t init(stateFilter *self)
 
     this->baroOffset = 0.0f;
     this->first_run  = 100;
+
+    RevoSettingsInitialize();
+    RevoSettingsBaroGPSOffsetCorrectionAlphaGet(&this->baroGPSOffsetCorrectionAlpha);
+
     return 0;
 }
 
@@ -90,9 +91,8 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
         // Track barometric altitude offset with a low pass filter
         // based on GPS altitude if available
         if (IS_SET(state->updated, SENSORUPDATES_pos)) {
-            this->baroOffset = BARO_OFFSET_LOWPASS_ALPHA * this->baroOffset +
-                               (1.0f - BARO_OFFSET_LOWPASS_ALPHA)
-                               * (this->baroAlt + state->pos[2]);
+            this->baroOffset = this->baroOffset * this->baroGPSOffsetCorrectionAlpha +
+                               (1.0f - this->baroGPSOffsetCorrectionAlpha) * (this->baroAlt + state->pos[2]);
         }
         // calculate bias corrected altitude
         if (IS_SET(state->updated, SENSORUPDATES_baro)) {

flight/modules/StateEstimation/filtercf.c

@@ -55,6 +55,8 @@ struct data {
     bool    useMag;
     float   currentAccel[3];
     float   currentMag[3];
+    float   accels_filtered[3];
+    float   grot_filtered[3];
     float   gyroBias[3];
     bool    accelUpdated;
     bool    magUpdated;
@@ -266,7 +268,12 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel
         RPY2Quaternion(&attitudeState.Roll, attitude);
 
         this->first_run = 0;
-
+        this->accels_filtered[0] = 0.0f;
+        this->accels_filtered[1] = 0.0f;
+        this->accels_filtered[2] = 0.0f;
+        this->grot_filtered[0] = 0.0f;
+        this->grot_filtered[1] = 0.0f;
+        this->grot_filtered[2] = 0.0f;
         this->timeval   = PIOS_DELAY_GetRaw();
         this->starttime = this->timeval;
 
@@ -316,9 +323,8 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel
     // Get the current attitude estimate
     quat_copy(&attitudeState.q1, attitude);
 
-    float accels_filtered[3];
     // Apply smoothing to accel values, to reduce vibration noise before main calculations.
-    apply_accel_filter(this, accel, accels_filtered);
+    apply_accel_filter(this, accel, this->accels_filtered);
 
     // Rotate gravity to body frame and cross with accels
     float grot[3];
@@ -326,13 +332,13 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel
     grot[1] = -(2.0f * (attitude[2] * attitude[3] + attitude[0] * attitude[1]));
     grot[2] = -(attitude[0] * attitude[0] - attitude[1] * attitude[1] - attitude[2] * attitude[2] + attitude[3] * attitude[3]);
 
-    float grot_filtered[3];
     float accel_err[3];
-    apply_accel_filter(this, grot, grot_filtered);
-    CrossProduct((const float *)accels_filtered, (const float *)grot_filtered, accel_err);
+    apply_accel_filter(this, grot, this->grot_filtered);
+
+    CrossProduct((const float *)this->accels_filtered, (const float *)this->grot_filtered, accel_err);
 
     // Account for accel magnitude
-    float accel_mag = sqrtf(accels_filtered[0] * accels_filtered[0] + accels_filtered[1] * accels_filtered[1] + accels_filtered[2] * accels_filtered[2]);
+    float accel_mag = sqrtf(this->accels_filtered[0] * this->accels_filtered[0] + this->accels_filtered[1] * this->accels_filtered[1] + this->accels_filtered[2] * this->accels_filtered[2]);
     if (accel_mag < 1.0e-3f) {
         return 2; // safety feature copied from CC
     }
@@ -340,7 +346,7 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel
     // Account for filtered gravity vector magnitude
     float grot_mag;
     if (this->accel_filter_enabled) {
-        grot_mag = sqrtf(grot_filtered[0] * grot_filtered[0] + grot_filtered[1] * grot_filtered[1] + grot_filtered[2] * grot_filtered[2]);
+        grot_mag = sqrtf(this->grot_filtered[0] * this->grot_filtered[0] + this->grot_filtered[1] * this->grot_filtered[1] + this->grot_filtered[2] * this->grot_filtered[2]);
     } else {
         grot_mag = 1.0f;
     }

ground/openpilotgcs/src/plugins/systemhealth/html/Attitude-Critical.html

@@ -1,4 +1,4 @@
-html>
+<html>
   <head>
     <title></title>
     <meta content="">

shared/uavobjectdefinition/revosettings.xml

@@ -2,6 +2,12 @@
     <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,Complementary,Complementary+Mag,INS13Indoor,INS13Outdoor" defaultvalue="Complementary"/>
+
+        <!-- Low pass filter configuration to calculate offset of barometric altitude sensor.
+        Defaults: updates at 5 Hz, tau = 300s settle time, exp(-(1/f)/tau) ~= 0.9993335555062
+        Set BaroGPSOffsetCorrectionAlpha = 1.0 to completely disable baro offset updates. -->
+        <field name="BaroGPSOffsetCorrectionAlpha" units="" type="float" elements="1" defaultvalue="0.9993335555062"/>
+
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="true" updatemode="onchange" period="0"/>
         <telemetryflight acked="true" updatemode="onchange" period="0"/>