dynamic dcm_kp depending on groundspeed and velocity to home

src/main/flight/gps_rescue.c

@@ -112,6 +112,7 @@ typedef struct {
     float alitutudeStepCm;
     float maxPitchStep;
     float absErrorAngle;
+    float dcm_kpModifier;
 } rescueSensorData_s;
 
 typedef struct {
@@ -233,7 +234,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.dcm_kpModifier = 1.0f;
         return;
     case RESCUE_DO_NOTHING:
         // 20s of slow descent for switch induced sanity failures to allow time to recover
@@ -567,6 +569,18 @@ static void sensorUpdate(void)
     rescueState.sensor.distanceToHomeM = rescueState.sensor.distanceToHomeCm / 100.0f;
     rescueState.sensor.groundSpeedCmS = gpsSol.groundSpeed; // cm/s
 
+    // values to modify dcm_kp to a low value when climbing, and a higher value when there is a velocity error
+    if (gpsRescueConfig()->rescueGroundspeed) {
+        const float setGroundspeedInv = 1.0f / gpsRescueConfig()->rescueGroundspeed;
+        const float targetVelocityRatio = rescueState.intent.targetVelocityCmS * setGroundspeedInv;
+        // 0 when climbing or rotating, 1 when target velocity is fully set
+        const float groundspeedErrorRatio = fabsf(rescueState.sensor.groundSpeedCmS - rescueState.sensor.velocityToHomeCmS) * setGroundspeedInv;
+        // this is zero if groundspeed = velocity to home; 1 if flying sideways at target dspeed with zero home velocity, 2 if flying backwards at target speed
+        rescueState.sensor.dcm_kpModifier = MIN(10.0f, 2.0f * groundspeedErrorRatio * targetVelocityRatio);
+        // mulitply by 2 means if the groundspeed errorRatio is 1, dcm_kp will be 2x normal; if error is 2, then 4x dcm_kp, max 10x
+    }
+
+
     rescueState.sensor.gpsDataIntervalSeconds = getGpsDataIntervalSeconds();
     // Range from 10ms (100hz) to 1000ms (1Hz). Intended to cover common GPS data rates and exclude unusual values.
 
@@ -853,6 +867,11 @@ float gpsRescueGetYawRate(void)
     return rescueYaw;
 }
 
+float gpsRescueGetDcmKpModifier(void)
+{
+    return rescueState.sensor.dcm_kpModifier;
+}
+
 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 gpsRescueGetDcmKpModifier(void);

src/main/flight/imu.c

@@ -416,6 +416,12 @@ static float imuCalcKpGain(timeUs_t currentTimeUs, bool useAcc, float *gyroAvera
         ret = ATTITUDE_RESET_KP_GAIN;
     } else {
        ret = imuRuntimeConfig.dcm_kp;
+       if (FLIGHT_MODE(GPS_RESCUE_MODE)) {
+            ret *= gpsRescueGetDcmKpModifier();
+            // converge faster if groundspeed differs significantly from velocity to home during fly home phase
+            // don't converge while climbing or rotating, keep the original IMU information, which in most cases should be OK
+            // note that if there is significant drift away from nose orientation at the start of the rescue, this always gets locked in
+        }
        if (!armState) {
           ret = ret * 10.0f; // Scale the kP to generally converge faster when disarmed.
        }