Converge IMU faster at higher groundspeed during GPS Rescue (#12738)

* dynamic dcm_kp depending on groundspeed vs velocity to home

* reduce max pitch angle default to 40

* Only 10% of set velocity to home is enough, angle to 45

to reduce chance of sanity check against headwind

* fixes, thanks @KarateBrot

* Feedback from Ledvinap, ensure full pitch and yaw authority in descend

* pitch limit max of 1/3 and dcm_kp gain to 6x max

* apply IMU kP factor to yaw only via ez_ef

* More from @Ledvinap, constrain pitch in descent also

* Fix the velocity error gain, refactoring thanks to Ledvinap

* thanks, Mark!

* fixes, must be fabsf

* make IMU gain twice as sensitive to groundspeed error

* useCOG as float

* Fix unit test

* ensure cogYawGain is zero, except during gps rescue

* update comment after suggestion from ledvinap

* keep roll angle limit consistently at 50% of pitch limit when active

* changes to comments, thanks petr

src/main/flight/gps_rescue.c

@@ -112,6 +112,8 @@ typedef struct {
     float alitutudeStepCm;
     float maxPitchStep;
     float absErrorAngle;
+    float groundspeedPitchAttenuator;
+    float imuYawCogGain;
 } rescueSensorData_s;
 
 typedef struct {
@@ -233,7 +235,8 @@ static void rescueAttainPosition(void)
         throttleI = 0.0f;
         previousAltitudeError = 0.0f;
         throttleAdjustment = 0;
-        rescueState.intent.disarmThreshold = gpsRescueConfig()->disarmThreshold / 10.0f;
+        rescueState.intent.disarmThreshold = gpsRescueConfig()->disarmThreshold * 0.1f;
+        rescueState.sensor.imuYawCogGain = 1.0f;
         return;
     case RESCUE_DO_NOTHING:
         // 20s of slow descent for switch induced sanity failures to allow time to recover
@@ -453,7 +456,7 @@ static void performSanityChecks(void)
     if (rescueState.phase == RESCUE_FLY_HOME) {
         const float velocityToHomeCmS = previousDistanceToHomeCm - rescueState.sensor.distanceToHomeCm; // cm/s
         previousDistanceToHomeCm = rescueState.sensor.distanceToHomeCm;
-        rescueState.intent.secondsFailing += (velocityToHomeCmS < 0.5f * rescueState.intent.targetVelocityCmS) ? 1 : -1;
+        rescueState.intent.secondsFailing += (velocityToHomeCmS < 0.1f * rescueState.intent.targetVelocityCmS) ? 1 : -1;
         rescueState.intent.secondsFailing = constrain(rescueState.intent.secondsFailing, 0, 15);
         if (rescueState.intent.secondsFailing == 15) {
 #ifdef USE_MAG
@@ -477,7 +480,6 @@ static void performSanityChecks(void)
         rescueState.failure = RESCUE_LOWSATS;
     }
 
-
     // These conditions ignore sanity mode settings, and apply in all rescues, to handle getting stuck in a climb or descend
 
     const float actualAltitudeChange = rescueState.sensor.currentAltitudeCm - prevAltitudeCm;
@@ -567,6 +569,25 @@ static void sensorUpdate(void)
     rescueState.sensor.distanceToHomeM = rescueState.sensor.distanceToHomeCm / 100.0f;
     rescueState.sensor.groundSpeedCmS = gpsSol.groundSpeed; // cm/s
 
+    // when there is significant velocity error, increase IMU COG Gain for yaw to a higher value, and reduce max pitch angle
+    if (gpsRescueConfig()->rescueGroundspeed) {
+        const float rescueGroundspeed = 1000.0f; // in cm/s, fixed 10 m/s groundspeed
+        // sets the slope of the increase in IMU COG Gain as velocity error increases; at 10, we get max CoG gain around 25m/s drift, approx the limit of what we can fix
+        const float groundspeedErrorRatio = fabsf(rescueState.sensor.groundSpeedCmS - rescueState.sensor.velocityToHomeCmS) / rescueGroundspeed;
+        // 0 if groundspeed = velocity to home,
+        // 1 if moving sideways at 10m/s but zero home velocity,
+        // 2 if moving backwards (away from home) at 10 m/s
+        // 4 if moving backwards (away from home) at 20 m/s
+        if ((rescueState.phase == RESCUE_ATTAIN_ALT) || (rescueState.phase == RESCUE_ROTATE)) {
+            // zero IMU CoG yaw during climb or rotate, to stop IMU error accumulation arising from drift during long climbs
+            rescueState.sensor.imuYawCogGain = 0;
+        } else {
+            // up to 6x increase in CoG IMU Yaw gain
+            rescueState.sensor.imuYawCogGain =  constrainf(1.0f + (2.0f * groundspeedErrorRatio), 1.0f, 6.0f);
+        }
+        rescueState.sensor.groundspeedPitchAttenuator = 1.0f / constrainf(1.0f + groundspeedErrorRatio, 1.0f, 3.0f); // cut pitch angle by up to one third
+    }
+
     rescueState.sensor.gpsDataIntervalSeconds = getGpsDataIntervalSeconds();
     // Range from 10ms (100hz) to 1000ms (1Hz). Intended to cover common GPS data rates and exclude unusual values.
 
@@ -648,10 +669,13 @@ void descend(void)
         rescueState.intent.targetVelocityCmS = gpsRescueConfig()->rescueGroundspeed * rescueState.intent.proximityToLandingArea;
         // reduce target velocity as we get closer to home. Zero within 2m of home, reducing risk of overshooting.
         // if quad drifts further than 2m away from home, should by then have rotated towards home, so pitch is allowed
-        rescueState.intent.rollAngleLimitDeg = gpsRescueConfig()->maxRescueAngle * rescueState.intent.proximityToLandingArea;
+        rescueState.intent.rollAngleLimitDeg = 0.5f * gpsRescueConfig()->maxRescueAngle * rescueState.intent.proximityToLandingArea;
         // reduce roll capability when closer to home, none within final 2m
+        rescueState.intent.pitchAngleLimitDeg = gpsRescueConfig()->maxRescueAngle * rescueState.sensor.groundspeedPitchAttenuator;
     }
 
+    rescueState.intent.yawAttenuator = 1.0f; // just in case entered descend phase before yaw authority was complete
+
     // configure altitude step for descent, considering interval between altitude readings
     rescueState.intent.altitudeStep = -1.0f * rescueState.sensor.altitudeDataIntervalSeconds * gpsRescueConfig()->descendRate;
 
@@ -758,7 +782,8 @@ void gpsRescueUpdate(void)
             rescueState.intent.yawAttenuator += rescueState.sensor.gpsRescueTaskIntervalSeconds;
         }
         if (rescueState.sensor.absErrorAngle < 30.0f) {
-            rescueState.intent.pitchAngleLimitDeg = gpsRescueConfig()->maxRescueAngle; // allow pitch
+            // allow pitch, limiting allowed angle if we are drifting away from home
+            rescueState.intent.pitchAngleLimitDeg = gpsRescueConfig()->maxRescueAngle * rescueState.sensor.groundspeedPitchAttenuator;
             rescueState.phase = RESCUE_FLY_HOME; // enter fly home phase
             rescueState.intent.secondsFailing = 0; // reset sanity timer for flight home
             rescueState.intent.proximityToLandingArea = 1.0f; // velocity iTerm activated, initialise proximity for descent phase at 1.0
@@ -771,7 +796,6 @@ void gpsRescueUpdate(void)
         if (rescueState.intent.yawAttenuator < 1.0f) { // be sure to accumulate full yaw authority
             rescueState.intent.yawAttenuator += rescueState.sensor.gpsRescueTaskIntervalSeconds;
         }
-
         // velocity PIDs are now active
         // update target velocity gradually, aiming for rescueGroundspeed with a time constant of 1.0s
         const float targetVelocityError = gpsRescueConfig()->rescueGroundspeed - rescueState.intent.targetVelocityCmS;
@@ -794,6 +818,8 @@ void gpsRescueUpdate(void)
         // gradually gain roll capability to max of half of max pitch angle
 
         if (newGPSData) {
+            // cut back on allowed angle if there is a high groundspeed error
+            rescueState.intent.pitchAngleLimitDeg = gpsRescueConfig()->maxRescueAngle * rescueState.sensor.groundspeedPitchAttenuator;
             if (rescueState.sensor.distanceToHomeM <= rescueState.intent.descentDistanceM) {
                 rescueState.phase = RESCUE_DESCENT;
                 rescueState.intent.secondsFailing = 0; // reset sanity timer for descent
@@ -853,6 +879,11 @@ float gpsRescueGetYawRate(void)
     return rescueYaw;
 }
 
+float gpsRescueGetImuYawCogGain(void)
+{
+    return rescueState.sensor.imuYawCogGain;
+}
+
 float gpsRescueGetThrottle(void)
 {
     // Calculated a desired commanded throttle scaled from 0.0 to 1.0 for use in the mixer.

src/main/flight/gps_rescue.h

@@ -57,3 +57,4 @@ bool gpsRescueIsConfigured(void);
 bool gpsRescueIsAvailable(void);
 bool gpsRescueIsDisabled(void);
 bool gpsRescueDisableMag(void);
+float gpsRescueGetImuYawCogGain(void);

src/main/flight/imu.c

@@ -201,7 +201,7 @@ static float invSqrt(float x)
 static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
                                 bool useAcc, float ax, float ay, float az,
                                 bool useMag,
-                                bool useCOG, float courseOverGround, const float dcmKpGain)
+                                float cogYawGain, float courseOverGround, const float dcmKpGain)
 {
     static float integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f;    // integral error terms scaled by Ki
 
@@ -210,17 +210,15 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
 
     // Use raw heading error (from GPS or whatever else)
     float ex = 0, ey = 0, ez = 0;
-    if (useCOG) {
+    if (cogYawGain != 0.0f) {
+        // Used in a GPS Rescue to boost IMU yaw gain when course over ground and velocity to home differ significantly
         while (courseOverGround >  M_PIf) {
             courseOverGround -= (2.0f * M_PIf);
         }
-
         while (courseOverGround < -M_PIf) {
             courseOverGround += (2.0f * M_PIf);
         }
-
+        const float ez_ef = cogYawGain * (- sin_approx(courseOverGround) * rMat[0][0] - cos_approx(courseOverGround) * rMat[1][0]);
-        const float ez_ef = (- sin_approx(courseOverGround) * rMat[0][0] - cos_approx(courseOverGround) * rMat[1][0]);
-
         ex = rMat[2][0] * ez_ef;
         ey = rMat[2][1] * ez_ef;
         ez = rMat[2][2] * ez_ef;
@@ -263,7 +261,7 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
     // Use measured acceleration vector
     float recipAccNorm = sq(ax) + sq(ay) + sq(az);
     if (useAcc && recipAccNorm > 0.01f) {
-        // Normalise accelerometer measurement
+        // Normalise accelerometer measurement; useAcc is true when all smoothed acc axes are within 20% of 1G
         recipAccNorm = invSqrt(recipAccNorm);
         ax *= recipAccNorm;
         ay *= recipAccNorm;
@@ -415,10 +413,10 @@ static float imuCalcKpGain(timeUs_t currentTimeUs, bool useAcc, float *gyroAvera
     if (attitudeResetActive) {
         ret = ATTITUDE_RESET_KP_GAIN;
     } else {
-       ret = imuRuntimeConfig.dcm_kp;
+        ret = imuRuntimeConfig.dcm_kp;
-       if (!armState) {
+        if (!armState) {
-          ret = ret * 10.0f; // Scale the kP to generally converge faster when disarmed.
+            ret *= 10.0f; // Scale the kP to generally converge faster when disarmed.
-       }
+        }
     }
 
     return ret;
@@ -476,8 +474,8 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
     static timeUs_t previousIMUUpdateTime;
     bool useAcc = false;
     bool useMag = false;
-    bool useCOG = false; // Whether or not correct yaw via imuMahonyAHRSupdate from our ground course
+    float cogYawGain = 0.0f; // IMU yaw gain to be applied in imuMahonyAHRSupdate from ground course, default to no correction from CoG
-    float courseOverGround = 0; // To be used when useCOG is true.  Stored in Radians
+    float courseOverGround = 0; // To be used when cogYawGain is non-zero, in radians
 
     const timeDelta_t deltaT = currentTimeUs - previousIMUUpdateTime;
     previousIMUUpdateTime = currentTimeUs;
@@ -495,14 +493,15 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
     if (!useMag && sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat > GPS_MIN_SAT_COUNT && gpsSol.groundSpeed >= GPS_COG_MIN_GROUNDSPEED) {
         // Use GPS course over ground to correct attitude.values.yaw
         courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
-        useCOG = true;
+        if (FLIGHT_MODE(GPS_RESCUE_MODE)) {
-
+            cogYawGain = gpsRescueGetImuYawCogGain(); // do not modify IMU yaw gain unless in a rescue
+        }
         if (shouldInitializeGPSHeading()) {
-            // Reset our reference and reinitialize quaternion.  This will likely ideally happen more than once per flight, but for now,
+            // Reset our reference and reinitialize quaternion.
             // shouldInitializeGPSHeading() returns true only once.
             imuComputeQuaternionFromRPY(&qP, attitude.values.roll, attitude.values.pitch, gpsSol.groundCourse);
 
-            useCOG = false; // Don't use the COG when we first reinitialize.  Next time around though, yes.
+            cogYawGain = 0.0f; // Don't use the COG when we first initialize
         }
     }
 #endif
@@ -512,7 +511,7 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
     UNUSED(imuIsAccelerometerHealthy);
     UNUSED(useAcc);
     UNUSED(useMag);
-    UNUSED(useCOG);
+    UNUSED(cogYawGain);
     UNUSED(canUseGPSHeading);
     UNUSED(courseOverGround);
     UNUSED(deltaT);
@@ -528,13 +527,13 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
         gyroAverage[axis] = gyroGetFilteredDownsampled(axis);
     }
 
-    useAcc = imuIsAccelerometerHealthy(acc.accADC);
+    useAcc = imuIsAccelerometerHealthy(acc.accADC); // all smoothed accADC values are within 20% of 1G
 
     imuMahonyAHRSupdate(deltaT * 1e-6f,
                         DEGREES_TO_RADIANS(gyroAverage[X]), DEGREES_TO_RADIANS(gyroAverage[Y]), DEGREES_TO_RADIANS(gyroAverage[Z]),
                         useAcc, acc.accADC[X], acc.accADC[Y], acc.accADC[Z],
                         useMag,
-                        useCOG, courseOverGround,  imuCalcKpGain(currentTimeUs, useAcc, gyroAverage));
+                        cogYawGain, courseOverGround,  imuCalcKpGain(currentTimeUs, useAcc, gyroAverage));
 
     imuUpdateEulerAngles();
 #endif

src/main/pg/gps_rescue.c

@@ -40,7 +40,7 @@ PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
 
     .initialAltitudeM = 30,
     .rescueGroundspeed = 750,
-    .maxRescueAngle = 70,
+    .maxRescueAngle = 45,
     .rollMix = 150,
     .pitchCutoffHz = 75,
 

src/test/unit/flight_imu_unittest.cc

@@ -256,6 +256,7 @@ extern "C" {
     float baroUpsampleAltitude()  { return 0.0f; }
     float pt2FilterGain(float, float)  { return 0.0f; }
     float getBaroAltitude(void) { return 3000.0f; }
+    float gpsRescueGetImuYawCogGain(void) { return 1.0f; }
 
     void pt2FilterInit(pt2Filter_t *baroDerivativeLpf, float) {
         UNUSED(baroDerivativeLpf);