Merge branch 'next' into corvuscorax/OP-1259_Cruise_Control_Tweaks

flight/libraries/inc/insgps.h

@@ -67,6 +67,7 @@ void INSSetAccelVar(float accel_var[3]);
 void INSSetGyroVar(float gyro_var[3]);
 void INSSetGyroBiasVar(float gyro_bias_var[3]);
 void INSSetMagNorth(float B[3]);
+void INSSetG(float g_e);
 void INSSetMagVar(float scaled_mag_var[3]);
 void INSSetBaroVar(float baro_var);
 void INSPosVelReset(float pos[3], float vel[3]);

flight/libraries/insgps13state.c

@@ -91,6 +91,8 @@ static struct EKFData {
     float H[NUMV][NUMX];
     // local magnetic unit vector in NED frame
     float Be[3];
+    // local gravity vector
+    float g_e;
     // covariance matrix and state vector
     float P[NUMX][NUMX];
     float X[NUMX];
@@ -286,6 +288,11 @@ void INSSetMagNorth(float B[3])
     ekf.Be[2] = B[2] / mag;
 }
 
+void INSSetG(float g_e)
+{
+    ekf.g_e = g_e;
+}
+
 void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT)
 {
     float U[6];
@@ -641,7 +648,7 @@ void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX])
         az;
     Xdot[5] =
         2.0f * (q1 * q3 - q0 * q2) * ax + 2 * (q2 * q3 + q0 * q1) * ay +
-        (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * az + 9.81f;
+        (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * az + ekf.g_e;
 
     // qdot = Q*w
     Xdot[6]  = (-q1 * wx - q2 * wy - q3 * wz) / 2.0f;

flight/modules/StateEstimation/filteraltitude.c

@@ -33,6 +33,7 @@
 #include "inc/stateestimation.h"
 #include <attitudestate.h>
 #include <altitudefiltersettings.h>
+#include <homelocation.h>
 
 #include <CoordinateConversions.h>
 
@@ -65,6 +66,7 @@ struct data {
     bool  first_run;
     portTickType initTimer;
     AltitudeFilterSettingsData settings;
+    float gravity;
 };
 
 // Private variables
@@ -81,6 +83,7 @@ int32_t filterAltitudeInitialize(stateFilter *handle)
     handle->init      = &init;
     handle->filter    = &filter;
     handle->localdata = pvPortMalloc(sizeof(struct data));
+    HomeLocationInitialize();
     AttitudeStateInitialize();
     AltitudeFilterSettingsInitialize();
     AltitudeFilterSettingsConnectCallback(&settingsUpdatedCb);
@@ -107,6 +110,7 @@ static int32_t init(stateFilter *self)
     this->baroLast  = 0.0f;
     this->accelLast = 0.0f;
     this->first_run = 1;
+    HomeLocationg_eGet(&this->gravity);
     return 0;
 }
 
@@ -145,7 +149,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
             AttitudeStateGet(&att);
             float Rbe[3][3];
             Quaternion2R(&att.q1, Rbe);
-            float current = -(Rbe[0][2] * state->accel[0] + Rbe[1][2] * state->accel[1] + Rbe[2][2] * state->accel[2] + 9.81f);
+            float current = -(Rbe[0][2] * state->accel[0] + Rbe[1][2] * state->accel[1] + Rbe[2][2] * state->accel[2] + this->gravity);
 
             // low pass filter accelerometers
             this->accelState = (1.0f - this->settings.AccelLowPassKp) * this->accelState + this->settings.AccelLowPassKp * current;

flight/modules/StateEstimation/filterekf.c

@@ -356,6 +356,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
     }
 
     INSSetMagNorth(this->homeLocation.Be);
+    INSSetG(this->homeLocation.g_e);
 
     if (!this->usePos) {
         // position and velocity variance used in indoor mode

flight/modules/VtolPathFollower/vtolpathfollower.c

@@ -139,6 +139,7 @@ int32_t VtolPathFollowerInitialize()
         AccessoryDesiredInitialize();
         PoiLearnSettingsInitialize();
         PoiLocationInitialize();
+        HomeLocationInitialize();
         vtolpathfollower_enabled = true;
     } else {
         vtolpathfollower_enabled = false;
@@ -158,6 +159,7 @@ static float eastPosIntegral  = 0;
 static float downPosIntegral  = 0;
 
 static float thrustOffset     = 0;
+static float gravity;
 /**
  * Module thread, should not return.
  */
@@ -181,6 +183,7 @@ static void vtolPathFollowerTask(__attribute__((unused)) void *parameters)
         // 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint  OR
         // FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path
 
+        HomeLocationg_eGet(&gravity);
         SystemSettingsGet(&systemSettings);
         if ((systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP)
             && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADX)
@@ -683,7 +686,7 @@ static void updateNedAccel()
             accel_ned[i] += Rbe[j][i] * accel[j];
         }
     }
-    accel_ned[2] += 9.81f;
+    accel_ned[2] += gravity;
 
     NedAccelData accelData;
     NedAccelGet(&accelData);