Improve GPS Rescue Pitch smoothing and disarming (#12343)

src/main/blackbox/blackbox.c

@@ -1532,12 +1532,14 @@ static bool blackboxWriteSysinfo(void)
 
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_RETURN_ALT, "%d",      gpsRescueConfig()->initialAltitudeM)
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_RETURN_SPEED, "%d",    gpsRescueConfig()->rescueGroundspeed)
-        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_PITCH_ANGLE_MAX, "%d", gpsRescueConfig()->angle)
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_MAX_RESCUE_ANGLE, "%d", gpsRescueConfig()->maxRescueAngle)
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ROLL_MIX, "%d",        gpsRescueConfig()->rollMix)
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_PITCH_CUTOFF, "%d",    gpsRescueConfig()->pitchCutoffHz)
 
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_DESCENT_DIST, "%d",    gpsRescueConfig()->descentDistanceM)
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_DESCEND_RATE, "%d",    gpsRescueConfig()->descendRate)
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_LANDING_ALT, "%d",     gpsRescueConfig()->targetLandingAltitudeM)
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_DISARM_THRESHOLD, "%d", gpsRescueConfig()->disarmThreshold)
 
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_MIN, "%d",    gpsRescueConfig()->throttleMin)
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_MAX, "%d",    gpsRescueConfig()->throttleMax)

src/main/cli/settings.c

@@ -1017,12 +1017,14 @@ const clivalue_t valueTable[] = {
 
     { PARAM_NAME_GPS_RESCUE_RETURN_ALT,      VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 2, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, initialAltitudeM) },
     { PARAM_NAME_GPS_RESCUE_RETURN_SPEED,    VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 3000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, rescueGroundspeed) },
-    { PARAM_NAME_GPS_RESCUE_PITCH_ANGLE_MAX, VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 60 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, angle) },
+    { PARAM_NAME_GPS_RESCUE_MAX_RESCUE_ANGLE, VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 80 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, maxRescueAngle) },
     { PARAM_NAME_GPS_RESCUE_ROLL_MIX,        VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, rollMix) },
+    { PARAM_NAME_GPS_RESCUE_PITCH_CUTOFF,    VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 10, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, pitchCutoffHz) },
 
     { PARAM_NAME_GPS_RESCUE_DESCENT_DIST,    VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 5, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, descentDistanceM) },
     { PARAM_NAME_GPS_RESCUE_DESCEND_RATE,    VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 25, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, descendRate) },
     { PARAM_NAME_GPS_RESCUE_LANDING_ALT,     VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 1, 15 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, targetLandingAltitudeM) },
+    { PARAM_NAME_GPS_RESCUE_DISARM_THRESHOLD, VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 1, 250 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, disarmThreshold) },
 
     { PARAM_NAME_GPS_RESCUE_THROTTLE_MIN,    VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleMin) },
     { PARAM_NAME_GPS_RESCUE_THROTTLE_MAX,    VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleMax) },

src/main/cms/cms_menu_gps_rescue.c

@@ -63,6 +63,7 @@ static uint8_t gpsRescueConfig_throttleP, gpsRescueConfig_throttleI, gpsRescueCo
 static uint8_t gpsRescueConfig_yawP;
 static uint8_t gpsRescueConfig_velP, gpsRescueConfig_velI, gpsRescueConfig_velD;
 
+static uint8_t gpsRescueConfig_pitchCutoffHz;
 
 static const void *cms_menuGpsRescuePidOnEnter(displayPort_t *pDisp)
 {
@@ -78,6 +79,7 @@ static const void *cms_menuGpsRescuePidOnEnter(displayPort_t *pDisp)
     gpsRescueConfig_velI = gpsRescueConfig()->velI;
     gpsRescueConfig_velD = gpsRescueConfig()->velD;
 
+    gpsRescueConfig_pitchCutoffHz = gpsRescueConfig()->pitchCutoffHz;
     return NULL;
 }
 
@@ -96,6 +98,8 @@ static const void *cms_menuGpsRescuePidOnExit(displayPort_t *pDisp, const OSD_En
     gpsRescueConfigMutable()->velI = gpsRescueConfig_velI;
     gpsRescueConfigMutable()->velD = gpsRescueConfig_velD;
 
+    gpsRescueConfigMutable()->pitchCutoffHz = gpsRescueConfig_pitchCutoffHz;
+
     return NULL;
 }
 
@@ -113,6 +117,8 @@ const OSD_Entry cms_menuGpsRescuePidEntries[] =
     { "VELOCITY I",        OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_velI, 0, 255, 1 } },
     { "VELOCITY D",        OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_velD, 0, 255, 1 } },
 
+    { "SMOOTHING",         OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_pitchCutoffHz, 10, 255, 1 } },
+
     {"BACK", OME_Back, NULL, NULL},
     {NULL, OME_END, NULL, NULL}
 };
@@ -139,7 +145,7 @@ static const void *cmsx_menuGpsRescueOnEnter(displayPort_t *pDisp)
 
     gpsRescueConfig_initialAltitudeM = gpsRescueConfig()->initialAltitudeM;
     gpsRescueConfig_rescueGroundspeed = gpsRescueConfig()->rescueGroundspeed;
-    gpsRescueConfig_angle = gpsRescueConfig()->angle;
+    gpsRescueConfig_angle = gpsRescueConfig()->maxRescueAngle;
 
     gpsRescueConfig_descentDistanceM = gpsRescueConfig()->descentDistanceM;
     gpsRescueConfig_descendRate = gpsRescueConfig()->descendRate;
@@ -167,7 +173,7 @@ static const void *cmsx_menuGpsRescueOnExit(displayPort_t *pDisp, const OSD_Entr
 
     gpsRescueConfigMutable()->initialAltitudeM = gpsRescueConfig_initialAltitudeM;
     gpsRescueConfigMutable()->rescueGroundspeed = gpsRescueConfig_rescueGroundspeed;
-    gpsRescueConfigMutable()->angle = gpsRescueConfig_angle;
+    gpsRescueConfigMutable()->maxRescueAngle = gpsRescueConfig_angle;
 
     gpsRescueConfigMutable()->descentDistanceM = gpsRescueConfig_descentDistanceM;
     gpsRescueConfigMutable()->descendRate = gpsRescueConfig_descendRate;

src/main/fc/parameter_names.h

@@ -143,12 +143,14 @@
 
 #define PARAM_NAME_GPS_RESCUE_RETURN_ALT "gps_rescue_return_alt"
 #define PARAM_NAME_GPS_RESCUE_RETURN_SPEED "gps_rescue_ground_speed"
-#define PARAM_NAME_GPS_RESCUE_PITCH_ANGLE_MAX "gps_rescue_pitch_angle_max"
+#define PARAM_NAME_GPS_RESCUE_MAX_RESCUE_ANGLE "gps_rescue_max_angle"
 #define PARAM_NAME_GPS_RESCUE_ROLL_MIX "gps_rescue_roll_mix"
+#define PARAM_NAME_GPS_RESCUE_PITCH_CUTOFF "gps_rescue_pitch_cutoff"
 
 #define PARAM_NAME_GPS_RESCUE_DESCENT_DIST "gps_rescue_descent_dist"
 #define PARAM_NAME_GPS_RESCUE_DESCEND_RATE "gps_rescue_descend_rate"
 #define PARAM_NAME_GPS_RESCUE_LANDING_ALT "gps_rescue_landing_alt"
+#define PARAM_NAME_GPS_RESCUE_DISARM_THRESHOLD "gps_rescue_disarm_threshold"
 
 #define PARAM_NAME_GPS_RESCUE_THROTTLE_MIN "gps_rescue_throttle_min"
 #define PARAM_NAME_GPS_RESCUE_THROTTLE_MAX "gps_rescue_throttle_max"

src/main/fc/rc_controls.c

@@ -177,10 +177,16 @@ void processRcStickPositions(void)
             resetTryingToArm();
             // Disarming via ARM BOX
             resetArmingDisabled();
-            const bool switchFailsafe = (failsafeIsActive() && (IS_RC_MODE_ACTIVE(BOXFAILSAFE) || IS_RC_MODE_ACTIVE(BOXGPSRESCUE)));
+            const bool boxFailsafeSwitchIsOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
-            if (ARMING_FLAG(ARMED) && (failsafeIsReceivingRxData() || switchFailsafe)) {
+            if (ARMING_FLAG(ARMED) && (failsafeIsReceivingRxData() || boxFailsafeSwitchIsOn)) {
+                // in a true signal loss situation, allow disarm only once we regain validated RxData (failsafeIsReceivingRxData = true),
+                // to avoid potentially false disarm signals soon after link recover
+                // Note that BOXFAILSAFE will also drive failsafeIsReceivingRxData false (immediately at start or end)
+                // That's why we explicitly allow disarm here BOXFAILSAFE switch is active
+                // Note that BOXGPSRESCUE mode does not trigger failsafe - we can always disarm in that mode
                 rcDisarmTicks++;
                 if (rcDisarmTicks > 3) {
+                    // require three duplicate disarm values in a row before we disarm
                     disarm(DISARM_REASON_SWITCH);
                 }
             }

src/main/flight/failsafe.c

@@ -104,7 +104,7 @@ void failsafeReset(void)
     failsafeState.receivingRxDataPeriodPreset = failsafeState.rxDataRecoveryPeriod;
     failsafeState.phase = FAILSAFE_IDLE;
     failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN;
-    failsafeState.failsafeSwitchWasOn = false;
+    failsafeState.boxFailsafeSwitchWasOn = false;
 }
 
 void failsafeInit(void)
@@ -125,7 +125,7 @@ bool failsafeIsMonitoring(void)
     return failsafeState.monitoring;
 }
 
-bool failsafeIsActive(void) // real or switch-induced stage 2 failsafe
+bool failsafeIsActive(void) // real or BOXFAILSAFE induced stage 2 failsafe is currently active
 {
     return failsafeState.active;
 }
@@ -143,9 +143,9 @@ static bool failsafeShouldHaveCausedLandingByNow(void)
 bool failsafeIsReceivingRxData(void)
 {
     return (failsafeState.rxLinkState == FAILSAFE_RXLINK_UP);
-    // False with failsafe switch or when no valid packets for 100ms or any flight channel invalid for 300ms,
+    // False with BOXFAILSAFE switch or when no valid packets for 100ms or any flight channel invalid for 300ms,
-    // stays false until after recovery period expires
+    // becomes true immediately BOXFAILSAFE switch reverts, or after recovery period expires when valid packets are received
-    // Link down is the trigger for the various failsafe stage 2 outcomes.
+    // rxLinkState RXLINK_DOWN (not up) is the trigger for the various failsafe stage 2 outcomes.
 }
 
 void failsafeOnRxSuspend(uint32_t usSuspendPeriod)
@@ -160,7 +160,10 @@ void failsafeOnRxResume(void)
 }
 
 void failsafeOnValidDataReceived(void)
-// runs when packets are received for more than the signal validation period (100ms)
+// enters stage 2
+// runs, after prior a signal loss, immediately when packets are received or the BOXFAILSAFE switch is reverted
+// rxLinkState will go RXLINK_UP immediately if BOXFAILSAFE goes back ON since receivingRxDataPeriodPreset is set to zero in that case
+// otherwise RXLINK_UP is delayed for the recovery period (failsafe_recovery_delay, default 1s, 0-20, min 0.2s)
 {
     unsetArmingDisabled(ARMING_DISABLED_RX_FAILSAFE);
     // clear RXLOSS in OSD immediately we get a good packet, and un-set its arming block
@@ -188,14 +191,16 @@ void failsafeOnValidDataReceived(void)
 }
 
 void failsafeOnValidDataFailed(void)
-// runs when packets are lost for more than the signal validation period (100ms)
+// run from rc.c when packets are lost for more than the signal validation period (100ms), or immediately BOXFAILSAFE switch is active
+// after the stage 1 delay has expired, sets the rxLinkState to RXLINK_DOWN, ie not up, causing failsafeIsReceivingRxData to become false
+// if failsafe is configured to go direct to stage 2, this is emulated immediately in failsafeUpdateState()
 {
     setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE);
     //  set RXLOSS in OSD and block arming after 100ms of signal loss (is restored in rx.c immediately signal returns)
 
     failsafeState.validRxDataFailedAt = millis();
     if ((cmp32(failsafeState.validRxDataFailedAt, failsafeState.validRxDataReceivedAt) > (int32_t)failsafeState.rxDataFailurePeriod)) {
-        // sets rxLinkState = DOWN to initiate stage 2 failsafe, if no validated signal for the stage 1 period
+        // sets rxLinkState = DOWN to initiate stage 2 failsafe
         failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN;
         // show RXLOSS and block arming
     }
@@ -225,19 +230,18 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
     }
 
     bool receivingRxData = failsafeIsReceivingRxData();
-    // returns state of FAILSAFE_RXLINK_UP
+    // returns state of FAILSAFE_RXLINK_UP, which 
-    // FAILSAFE_RXLINK_UP is set in failsafeOnValidDataReceived, after the various Stage 1 and recovery delays
+    // goes false after the stage 1 delay, whether from signal loss or BOXFAILSAFE switch activation
-    // failsafeOnValidDataReceived runs from detectAndApplySignalLossBehaviour
+    // goes true immediately BOXFAILSAFE switch is reverted, or after recovery delay once signal recovers
+    // essentially means 'should be in failsafe stage 2'
 
     DEBUG_SET(DEBUG_FAILSAFE, 2, receivingRxData); // from Rx alone, not considering switch
 
     bool armed = ARMING_FLAG(ARMED);
-    bool failsafeSwitchIsOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
     beeperMode_e beeperMode = BEEPER_SILENCE;
 
-    if (failsafeSwitchIsOn && (failsafeConfig()->failsafe_switch_mode == FAILSAFE_SWITCH_MODE_STAGE2)) {
+    if (IS_RC_MODE_ACTIVE(BOXFAILSAFE) && (failsafeConfig()->failsafe_switch_mode == FAILSAFE_SWITCH_MODE_STAGE2)) {
-        // Aux switch set to failsafe stage2 emulates immediate loss of signal without waiting
+        // Force immediate stage 2 responses if mode is failsafe stage2 to emulate immediate loss of signal without waiting
-        failsafeOnValidDataFailed();
         receivingRxData = false;
     }
 
@@ -253,13 +257,14 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
 
         switch (failsafeState.phase) {
             case FAILSAFE_IDLE:
-                failsafeState.failsafeSwitchWasOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
+                failsafeState.boxFailsafeSwitchWasOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
+                // store and use the switch state as it was at the start of the failsafe
                 if (armed) {
                     // Track throttle command below minimum time
                     if (calculateThrottleStatus() != THROTTLE_LOW) {
                         failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
                     }
-                    if (failsafeState.failsafeSwitchWasOn && (failsafeConfig()->failsafe_switch_mode == FAILSAFE_SWITCH_MODE_KILL)) {
+                    if (failsafeState.boxFailsafeSwitchWasOn && (failsafeConfig()->failsafe_switch_mode == FAILSAFE_SWITCH_MODE_KILL)) {
                         // Failsafe switch is configured as KILL switch and is switched ON
                         failsafeState.active = true;
                         failsafeState.events++;
@@ -291,7 +296,7 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
                     }
                 } else {
                     // When NOT armed, enable failsafe mode to show warnings in OSD
-                    if (failsafeState.failsafeSwitchWasOn) {
+                    if (failsafeState.boxFailsafeSwitchWasOn) {
                         ENABLE_FLIGHT_MODE(FAILSAFE_MODE);
                     } else {
                         DISABLE_FLIGHT_MODE(FAILSAFE_MODE);
@@ -327,7 +332,7 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
                             break;
 #endif
                     }
-                    if (failsafeState.failsafeSwitchWasOn) {
+                    if (failsafeState.boxFailsafeSwitchWasOn) {
                         failsafeState.receivingRxDataPeriodPreset = 0;
                         // recover immediately if failsafe was triggered by a switch
                     } else {
@@ -359,7 +364,7 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
 #ifdef USE_GPS_RESCUE
             case FAILSAFE_GPS_RESCUE:
                 if (receivingRxData) {
-                    if (areSticksActive(failsafeConfig()->failsafe_stick_threshold) || failsafeState.failsafeSwitchWasOn) {
+                    if (areSticksActive(failsafeConfig()->failsafe_stick_threshold) || failsafeState.boxFailsafeSwitchWasOn) {
                         // exits the rescue immediately if failsafe was initiated by switch, otherwise 
                         // requires stick input to exit the rescue after a true Rx loss failsafe
                         // NB this test requires stick inputs to be received during GPS Rescue see PR #7936 for rationale
@@ -418,7 +423,7 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
                 break;
         }
 
-    DEBUG_SET(DEBUG_FAILSAFE, 0, failsafeState.failsafeSwitchWasOn);
+    DEBUG_SET(DEBUG_FAILSAFE, 0, failsafeState.boxFailsafeSwitchWasOn);
     DEBUG_SET(DEBUG_FAILSAFE, 3, failsafeState.phase);
 
     } while (reprocessState);

src/main/flight/failsafe.h

@@ -90,7 +90,7 @@ typedef struct failsafeState_s {
     uint32_t receivingRxDataPeriodPreset;   // preset for the required period of valid rxData
     failsafePhase_e phase;
     failsafeRxLinkState_e rxLinkState;
-    bool failsafeSwitchWasOn;
+    bool boxFailsafeSwitchWasOn;
 } failsafeState_t;
 
 void failsafeInit(void);

src/main/flight/gps_rescue.c

@@ -89,6 +89,11 @@ typedef struct {
     float descentRateModifier;
     float yawAttenuator;
     float disarmThreshold;
+    float velocityITermAccumulator;
+    float velocityPidCutoff;
+    float velocityPidCutoffModifier;
+    float proximityToLandingArea;
+    float velocityItermRelax;
 } rescueIntent_s;
 
 typedef struct {
@@ -102,6 +107,7 @@ typedef struct {
     float errorAngle;
     float gpsDataIntervalSeconds;
     float altitudeDataIntervalSeconds;
+    float gpsRescueTaskIntervalSeconds;
     float velocityToHomeCmS;
     float alitutudeStepCm;
     float maxPitchStep;
@@ -118,10 +124,7 @@ typedef struct {
 
 #define GPS_RESCUE_MAX_YAW_RATE          180    // deg/sec max yaw rate
 #define GPS_RESCUE_MIN_DESCENT_DIST_M    5      // minimum descent distance
-#define GPS_RESCUE_MAX_ITERM_VELOCITY    1000   // max iterm value for velocity
+#define GPS_RESCUE_MAX_THROTTLE_ITERM    200    // max iterm value for throttle in degrees * 100
-#define GPS_RESCUE_MAX_ITERM_THROTTLE    200    // max iterm value for throttle
-#define GPS_RESCUE_MAX_PITCH_RATE        3000   // max change in pitch per second in degrees * 100
-#define GPS_RESCUE_DISARM_THRESHOLD      2.0f   // disarm threshold in G's
 
 static float rescueThrottle;
 static float rescueYaw;
@@ -129,27 +132,29 @@ float       gpsRescueAngle[ANGLE_INDEX_COUNT] = { 0, 0 };
 bool        magForceDisable = false;
 static bool newGPSData = false;
 static pt2Filter_t throttleDLpf;
-static pt2Filter_t velocityDLpf;
+static pt1Filter_t velocityDLpf;
-static pt3Filter_t pitchLpf;
+static pt3Filter_t velocityUpsampleLpf;
 
 rescueState_s rescueState;
 
 void gpsRescueInit(void)
 {
-    const float sampleTimeS = HZ_TO_INTERVAL(TASK_GPS_RESCUE_RATE_HZ);
+    rescueState.sensor.gpsRescueTaskIntervalSeconds = HZ_TO_INTERVAL(TASK_GPS_RESCUE_RATE_HZ);
-    float cutoffHz, gain;
 
+    float cutoffHz, gain;
     cutoffHz = positionConfig()->altitude_d_lpf / 100.0f;
-    gain = pt2FilterGain(cutoffHz, sampleTimeS);
+    gain = pt2FilterGain(cutoffHz, rescueState.sensor.gpsRescueTaskIntervalSeconds);
     pt2FilterInit(&throttleDLpf, gain);
 
-    cutoffHz = 0.8f;
+    cutoffHz = gpsRescueConfig()->pitchCutoffHz / 100.0f;
-    gain = pt2FilterGain(cutoffHz, 1.0f);
+    rescueState.intent.velocityPidCutoff = cutoffHz;
-    pt2FilterInit(&velocityDLpf, gain);
+    rescueState.intent.velocityPidCutoffModifier = 1.0f;
+    gain = pt1FilterGain(cutoffHz, 1.0f);
+    pt1FilterInit(&velocityDLpf, gain);
 
-    cutoffHz = 4.0f;
+    cutoffHz *= 4.0f; 
-    gain = pt3FilterGain(cutoffHz, sampleTimeS);
+    gain = pt3FilterGain(cutoffHz, rescueState.sensor.gpsRescueTaskIntervalSeconds);
-    pt3FilterInit(&pitchLpf, gain);
+    pt3FilterInit(&velocityUpsampleLpf, gain);
 }
 
 /*
@@ -210,7 +215,6 @@ static void rescueAttainPosition(void)
     // runs at 100hz, but only updates RPYT settings when new GPS Data arrives and when not in idle phase.
     static float previousVelocityError = 0.0f;
     static float velocityI = 0.0f;
-    static float previousPitchAdjustment = 0.0f;
     static float throttleI = 0.0f;
     static float previousAltitudeError = 0.0f;
     static int16_t throttleAdjustment = 0;
@@ -226,11 +230,10 @@ static void rescueAttainPosition(void)
         // Initialize internal variables each time GPS Rescue is started
         previousVelocityError = 0.0f;
         velocityI = 0.0f;
-        previousPitchAdjustment = 0.0f;
         throttleI = 0.0f;
         previousAltitudeError = 0.0f;
         throttleAdjustment = 0;
-        rescueState.intent.disarmThreshold = GPS_RESCUE_DISARM_THRESHOLD;
+        rescueState.intent.disarmThreshold = gpsRescueConfig()->disarmThreshold / 10.0f;
         return;
     case RESCUE_DO_NOTHING:
         // 20s of slow descent for switch induced sanity failures to allow time to recover
@@ -255,7 +258,7 @@ static void rescueAttainPosition(void)
 
     // I component
     throttleI += 0.1f * gpsRescueConfig()->throttleI * altitudeError * rescueState.sensor.altitudeDataIntervalSeconds;
-    throttleI = constrainf(throttleI, -1.0f * GPS_RESCUE_MAX_ITERM_THROTTLE, 1.0f * GPS_RESCUE_MAX_ITERM_THROTTLE);
+    throttleI = constrainf(throttleI, -1.0f * GPS_RESCUE_MAX_THROTTLE_ITERM, 1.0f * GPS_RESCUE_MAX_THROTTLE_ITERM);
     // up to 20% increase in throttle from I alone
 
     // D component is error based, so includes positive boost when climbing and negative boost on descent
@@ -266,8 +269,6 @@ static void rescueAttainPosition(void)
 
     float throttleD = pt2FilterApply(&throttleDLpf, verticalSpeed);
 
-    rescueState.intent.disarmThreshold = GPS_RESCUE_DISARM_THRESHOLD - throttleD / 15.0f; // make disarm more likely if throttle D is high
-
     throttleD = gpsRescueConfig()->throttleD * throttleD;
 
     // acceleration component not currently implemented - was needed previously due to GPS lag, maybe not needed now.
@@ -294,12 +295,13 @@ static void rescueAttainPosition(void)
     // if the course over ground, due to wind or pre-exiting movement, is different from the attitude of the quad, the GPS correction will be less accurate
     // the craft should not return much less than 5m/s during the rescue or the GPS corrections may be inaccurate.
     // the faster the return speed, the more accurate the IMU will be, but the consequences of IMU error at the start are greater
-    // A compass (magnetometer) is vital for accurate GPS rescue at slow speeds, but must be calibrated and validated.
+    // A compass (magnetometer) is vital for accurate GPS rescue at slow speeds, but must be calibrated and validated
-
+    // WARNING:  Some GPS units give false Home values!  Always check the arrow points to home on leaving home.
     rescueYaw = rescueState.sensor.errorAngle * gpsRescueConfig()->yawP * rescueState.intent.yawAttenuator * 0.1f;
     rescueYaw = constrainf(rescueYaw, -GPS_RESCUE_MAX_YAW_RATE, GPS_RESCUE_MAX_YAW_RATE);
     // rescueYaw is the yaw rate in deg/s to correct the heading error
 
+    // *** mix in some roll.  very important for heading tracking, since a yaw rate means the quad has drifted sideways
     const float rollMixAttenuator = constrainf(1.0f - fabsf(rescueYaw) * 0.01f, 0.0f, 1.0f);
     // less roll at higher yaw rates, no roll at 100 deg/s of yaw
     const float rollAdjustment = -rescueYaw * gpsRescueConfig()->rollMix * rollMixAttenuator;
@@ -307,12 +309,12 @@ static void rescueAttainPosition(void)
     // when gpsRescueConfig()->rollMix is zero, there is no roll adjustment
     // rollAdjustment is degrees * 100
     // note that the roll element has the opposite sign to the yaw element *before* GET_DIRECTION
-
     const float rollLimit = 100.0f * rescueState.intent.rollAngleLimitDeg;
     gpsRescueAngle[AI_ROLL] = constrainf(rollAdjustment, -rollLimit, rollLimit);
     // gpsRescueAngle is added to the normal roll Angle Mode corrections in pid.c
 
     rescueYaw *= GET_DIRECTION(rcControlsConfig()->yaw_control_reversed);
+    // rescueYaw is the yaw rate in deg/s to correct the heading error
 
     /**
         Pitch / velocity controller
@@ -322,51 +324,53 @@ static void rescueAttainPosition(void)
 
         const float sampleIntervalNormaliseFactor = rescueState.sensor.gpsDataIntervalSeconds * 10.0f;
 
-        const float velocityError = (rescueState.intent.targetVelocityCmS - rescueState.sensor.velocityToHomeCmS);
+        const float velocityError = rescueState.intent.targetVelocityCmS - rescueState.sensor.velocityToHomeCmS;
         // velocityError is in cm per second, positive means too slow.
         // NB positive pitch setpoint means nose down.
+        // target velocity can be very negative leading to large error before the start, with overshoot
 
         // P component
         const float velocityP = velocityError * gpsRescueConfig()->velP;
 
         // I component
-        velocityI += 0.01f * gpsRescueConfig()->velI * velocityError * sampleIntervalNormaliseFactor;
+        velocityI += 0.01f * gpsRescueConfig()->velI * velocityError * sampleIntervalNormaliseFactor * rescueState.intent.velocityItermRelax;
-        // increase amount added when GPS sample rate is slower
+        // velocityItermRelax is a time-based factor, 0->1 with time constant of 1s from when we start to fly home
-        velocityI = constrainf(velocityI, -1.0f * GPS_RESCUE_MAX_ITERM_VELOCITY, 1.0f * GPS_RESCUE_MAX_ITERM_VELOCITY);
+        // avoids excess iTerm during the initial acceleration phase.
-        // I component alone cannot exceed a pitch angle of 10%
+        velocityI *= rescueState.intent.proximityToLandingArea;
+        // reduce iTerm sharply when velocity decreases in landing phase, to minimise overshoot during deceleration
+
+        const float pitchAngleLimit = rescueState.intent.pitchAngleLimitDeg * 100.0f;
+        const float velocityPILimit = 0.5f * pitchAngleLimit;
+        velocityI = constrainf(velocityI, -velocityPILimit, velocityPILimit);
+        // I component alone cannot exceed half the max pitch angle
 
         // D component
         float velocityD = ((velocityError - previousVelocityError) / sampleIntervalNormaliseFactor);
         previousVelocityError = velocityError;
-        const float gain = pt2FilterGain(0.8f, HZ_TO_INTERVAL(gpsGetSampleRateHz()));
-        pt2FilterUpdateCutoff(&velocityDLpf, gain);
-        velocityD = pt2FilterApply(&velocityDLpf, velocityD);
         velocityD *= gpsRescueConfig()->velD;
 
-        const float velocityIAttenuator = rescueState.intent.targetVelocityCmS / gpsRescueConfig()->rescueGroundspeed;
+        // smooth the D steps
-        // reduces iTerm as target velocity decreases, to minimise overshoot during deceleration to landing phase
+        const float cutoffHz = rescueState.intent.velocityPidCutoff * rescueState.intent.velocityPidCutoffModifier;
+        // note that this cutoff is increased up to 2x as we get closer to landing point in descend()
+        const float gain = pt1FilterGain(cutoffHz, rescueState.sensor.gpsDataIntervalSeconds);
+        pt1FilterUpdateCutoff(&velocityDLpf, gain);
+        velocityD = pt1FilterApply(&velocityDLpf, velocityD);
 
-        pitchAdjustment = velocityP + velocityD;
+        pitchAdjustment = velocityP + velocityI + velocityD;
-        if (rescueState.phase == RESCUE_FLY_HOME) {
+        pitchAdjustment = constrainf(pitchAdjustment, -pitchAngleLimit, pitchAngleLimit);
-            pitchAdjustment *= 0.7f; // attenuate pitch PIDs during main fly home phase, tighten up in descent.
+        // limit to maximum allowed angle
-        }
-        pitchAdjustment += velocityI * velocityIAttenuator;
 
-        const float movingAvgPitchAdjustment = 0.5f * (previousPitchAdjustment + pitchAdjustment);
-         // moving average seems to work best here, a lot of sequential up and down in velocity data
-        previousPitchAdjustment = pitchAdjustment;
-        pitchAdjustment = movingAvgPitchAdjustment;
         // pitchAdjustment is the absolute Pitch angle adjustment value in degrees * 100
         // it gets added to the normal level mode Pitch adjustments in pid.c
         DEBUG_SET(DEBUG_GPS_RESCUE_VELOCITY, 0, lrintf(velocityP));
         DEBUG_SET(DEBUG_GPS_RESCUE_VELOCITY, 1, lrintf(velocityD));
     }
 
-    const float pitchAdjustmentFiltered = pt3FilterApply(&pitchLpf, pitchAdjustment);
     // upsampling and smoothing of pitch angle steps
+    float pitchAdjustmentFiltered = pt3FilterApply(&velocityUpsampleLpf, pitchAdjustment);
 
-    const float pitchAngleLimit = rescueState.intent.pitchAngleLimitDeg * 100.0f;
+    
-    gpsRescueAngle[AI_PITCH] = constrainf(pitchAdjustmentFiltered, -pitchAngleLimit, pitchAngleLimit);
+    gpsRescueAngle[AI_PITCH] = pitchAdjustmentFiltered;
     // this angle gets added to the normal pitch Angle Mode control values in pid.c - will be seen in pitch setpoint
 
     DEBUG_SET(DEBUG_GPS_RESCUE_VELOCITY, 3, lrintf(rescueState.intent.targetVelocityCmS));
@@ -447,7 +451,7 @@ static void performSanityChecks(void)
     // if the quad is stuck, or if GPS data packets stop, there will be no change in distance to home
     // we can't use rescueState.sensor.currentVelocity because it will be held at the last good value if GPS data updates stop
     if (rescueState.phase == RESCUE_FLY_HOME) {
-        const float velocityToHomeCmS = previousDistanceToHomeCm- rescueState.sensor.distanceToHomeCm; // cm/s
+        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 = constrain(rescueState.intent.secondsFailing, 0, 15);
@@ -540,7 +544,8 @@ static void sensorUpdate(void)
 
     if (rescueState.phase == RESCUE_LANDING) {
         // do this at sensor update rate, not the much slower GPS rate, for quick disarm
-        rescueState.sensor.accMagnitude = (float) sqrtf(sq(acc.accADC[Z]) + sq(acc.accADC[X]) + sq(acc.accADC[Y])) * acc.dev.acc_1G_rec;
+        rescueState.sensor.accMagnitude = (float) sqrtf(sq(acc.accADC[Z] - acc.dev.acc_1G) + sq(acc.accADC[X]) + sq(acc.accADC[Y])) * acc.dev.acc_1G_rec;
+        // Note: subtracting 1G from Z assumes the quad is 'flat' with respect to the horizon.  A true non-gravity acceleration value, regardless of attitude, may be better.
     }
 
     rescueState.sensor.directionToHome = GPS_directionToHome;
@@ -562,21 +567,15 @@ static void sensorUpdate(void)
     rescueState.sensor.distanceToHomeM = rescueState.sensor.distanceToHomeCm / 100.0f;
     rescueState.sensor.groundSpeedCmS = gpsSol.groundSpeed; // cm/s
 
-    static timeUs_t previousGPSDataTimeUs = 0;
+    rescueState.sensor.gpsDataIntervalSeconds = getGpsDataIntervalSeconds();
-    const timeDelta_t gpsDataIntervalUs = cmpTimeUs(currentTimeUs, previousGPSDataTimeUs);
-    rescueState.sensor.gpsDataIntervalSeconds = constrainf(gpsDataIntervalUs * 0.000001f, 0.01f, 1.0f);
     // Range from 10ms (100hz) to 1000ms (1Hz). Intended to cover common GPS data rates and exclude unusual values.
-    previousGPSDataTimeUs = currentTimeUs;
 
-    rescueState.sensor.velocityToHomeCmS = (prevDistanceToHomeCm - rescueState.sensor.distanceToHomeCm) / rescueState.sensor.gpsDataIntervalSeconds;
+    rescueState.sensor.velocityToHomeCmS = ((prevDistanceToHomeCm - rescueState.sensor.distanceToHomeCm) / rescueState.sensor.gpsDataIntervalSeconds);
     // positive = towards home.  First value is useless since prevDistanceToHomeCm was zero.
     prevDistanceToHomeCm = rescueState.sensor.distanceToHomeCm;
 
-    rescueState.sensor.maxPitchStep = rescueState.sensor.gpsDataIntervalSeconds * GPS_RESCUE_MAX_PITCH_RATE;
-
     DEBUG_SET(DEBUG_GPS_RESCUE_VELOCITY, 2, lrintf(rescueState.sensor.velocityToHomeCmS));
     DEBUG_SET(DEBUG_GPS_RESCUE_TRACKING, 0, lrintf(rescueState.sensor.velocityToHomeCmS));
-
 }
 
 // This function flashes "RESCUE N/A" in the OSD if:
@@ -643,15 +642,17 @@ void descend(void)
     if (newGPSData) {
         const float distanceToLandingAreaM = rescueState.sensor.distanceToHomeM - (rescueState.intent.targetLandingAltitudeCm / 200.0f);
         // considers home to be a circle half landing height around home to avoid overshooting home point
-        const float proximityToLandingArea = constrainf(distanceToLandingAreaM / rescueState.intent.descentDistanceM, 0.0f, 1.0f);
+        rescueState.intent.proximityToLandingArea = constrainf(distanceToLandingAreaM / rescueState.intent.descentDistanceM, 0.0f, 1.0f);
-        rescueState.intent.targetVelocityCmS = gpsRescueConfig()->rescueGroundspeed * proximityToLandingArea;
+        rescueState.intent.velocityPidCutoffModifier = 2.5f - rescueState.intent.proximityToLandingArea;
+         // 1.5 when starting descent, 2.5 when almost landed; multiplier for velocity step cutoff filter
+        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()->angle * proximityToLandingArea;
+        rescueState.intent.rollAngleLimitDeg = gpsRescueConfig()->maxRescueAngle * rescueState.intent.proximityToLandingArea;
         // reduce roll capability when closer to home, none within final 2m
     }
 
-    // adjust altitude step for interval between altitude readings
+    // configure altitude step for descent, considering interval between altitude readings
     rescueState.intent.altitudeStep = -1.0f * rescueState.sensor.altitudeDataIntervalSeconds * gpsRescueConfig()->descendRate;
 
     // descend more slowly if return altitude is less than 20m
@@ -665,15 +666,8 @@ void descend(void)
     // increase descent rate to max of 3x default above 50m, 2x above 25m, 1.2 at 5m, default by ground level.
 }
 
-void altitudeAchieved(void)
-{
-    rescueState.intent.targetAltitudeCm = rescueState.intent.returnAltitudeCm;
-    rescueState.intent.altitudeStep = 0;
-    rescueState.phase = RESCUE_ROTATE;
-}
-
 void gpsRescueUpdate(void)
-// this runs a lot faster than the GPS Data update rate, and runs whether or not rescue is active
+// runs at gpsRescueTaskIntervalSeconds, and runs whether or not rescue is active
 {
     if (!FLIGHT_MODE(GPS_RESCUE_MODE)) {
         rescueStop(); // sets phase to RESCUE_IDLE; does nothing else.  RESCUE_IDLE tasks still run.
@@ -687,7 +681,8 @@ void gpsRescueUpdate(void)
 
     sensorUpdate(); // always get latest GPS and Altitude data, update ascend and descend rates
 
-    bool startedLow = true;
+    static bool initialAltitudeLow = true;
+    static bool initialVelocityLow = true;
     rescueState.isAvailable = checkGPSRescueIsAvailable();
 
     switch (rescueState.phase) {
@@ -700,32 +695,41 @@ void gpsRescueUpdate(void)
         // target altitude is always set to current altitude.
 
     case RESCUE_INITIALIZE:
-        // Things that should abort the start of a Rescue
+        // Things that should be done at the start of a Rescue
+        rescueState.intent.targetLandingAltitudeCm = 100.0f * gpsRescueConfig()->targetLandingAltitudeM;
         if (!STATE(GPS_FIX_HOME)) {
             // we didn't get a home point on arming
             rescueState.failure = RESCUE_NO_HOME_POINT;
             // will result in a disarm via the sanity check system, with delay if switch induced
             // alternative is to prevent the rescue by returning to IDLE, but this could cause flyaways
         } else if (rescueState.sensor.distanceToHomeM < gpsRescueConfig()->minRescueDth) {
-            // Attempt to initiate inside minimum activation distance -> landing mode
+            if (rescueState.sensor.distanceToHomeM < 5.0f && rescueState.sensor.currentAltitudeCm < rescueState.intent.targetLandingAltitudeCm) {
-            rescueState.intent.altitudeStep = -rescueState.sensor.altitudeDataIntervalSeconds * gpsRescueConfig()->descendRate;
+                // attempted initiation within 5m of home, and 'on the ground' -> instant disarm, for safety reasons
-            rescueState.intent.targetVelocityCmS = 0; // zero forward velocity
+                rescueState.phase = RESCUE_ABORT;
-            rescueState.intent.pitchAngleLimitDeg = 0; // flat on pitch
+            } else {
-            rescueState.intent.rollAngleLimitDeg = 0.0f; // flat on roll also
+                // Otherwise, attempted initiation inside minimum activation distance, at any height -> landing mode
-            rescueState.intent.targetAltitudeCm = rescueState.sensor.currentAltitudeCm + rescueState.intent.altitudeStep;
+                rescueState.intent.altitudeStep = -rescueState.sensor.altitudeDataIntervalSeconds * gpsRescueConfig()->descendRate;
-            rescueState.phase = RESCUE_LANDING;
+                rescueState.intent.targetVelocityCmS = 0; // zero forward velocity
-            // start landing from current altitude
+                rescueState.intent.pitchAngleLimitDeg = 0; // flat on pitch
+                rescueState.intent.rollAngleLimitDeg = 0.0f; // flat on roll also
+                rescueState.intent.proximityToLandingArea = 0.0f; // force velocity iTerm to zero
+                rescueState.intent.targetAltitudeCm = rescueState.sensor.currentAltitudeCm + rescueState.intent.altitudeStep;
+                rescueState.phase = RESCUE_LANDING;
+                // start landing from current altitude
+            }
         } else {
             rescueState.phase = RESCUE_ATTAIN_ALT;
             rescueState.intent.secondsFailing = 0; // reset the sanity check timer for the climb
-            rescueState.intent.targetLandingAltitudeCm = 100.0f * gpsRescueConfig()->targetLandingAltitudeM;
+            initialAltitudeLow = (rescueState.sensor.currentAltitudeCm < rescueState.intent.returnAltitudeCm);
-            startedLow = (rescueState.sensor.currentAltitudeCm <= rescueState.intent.returnAltitudeCm);
             rescueState.intent.yawAttenuator = 0.0f;
-            rescueState.intent.targetVelocityCmS = 0.0f; // zero forward velocity while climbing
+            rescueState.intent.targetVelocityCmS = rescueState.sensor.velocityToHomeCmS;
             rescueState.intent.pitchAngleLimitDeg = 0.0f; // no pitch
             rescueState.intent.rollAngleLimitDeg = 0.0f; // no roll until flying home
             rescueState.intent.altitudeStep = 0.0f;
             rescueState.intent.descentRateModifier = 0.0f;
+            rescueState.intent.velocityPidCutoffModifier = 1.0f; // normal cutoff until descending when increases 150->250% during descent
+            rescueState.intent.proximityToLandingArea = 0.0f; // force velocity iTerm to zero
+            rescueState.intent.velocityItermRelax = 0.0f; // and don't accumulate any
         }
         break;
 
@@ -733,47 +737,62 @@ void gpsRescueUpdate(void)
         // gradually increment the target altitude until the craft reaches target altitude
         // note that this can mean the target altitude may increase above returnAltitude if the craft lags target
         // sanity check will abort if altitude gain is blocked for a cumulative period
-        if (startedLow) {
+        rescueState.intent.altitudeStep = ((initialAltitudeLow) ? gpsRescueConfig()->ascendRate : -1.0f * gpsRescueConfig()->descendRate) * rescueState.sensor.gpsRescueTaskIntervalSeconds;
-            if (rescueState.intent.targetAltitudeCm < rescueState.intent.returnAltitudeCm) {
+        const bool currentAltitudeLow = rescueState.sensor.currentAltitudeCm < rescueState.intent.returnAltitudeCm;
-                rescueState.intent.altitudeStep = rescueState.sensor.altitudeDataIntervalSeconds * gpsRescueConfig()->ascendRate;
+        if (initialAltitudeLow == currentAltitudeLow) {
-            } else if (rescueState.sensor.currentAltitudeCm > rescueState.intent.returnAltitudeCm) {
+            // we started low, and still are low; also true if we started high, and still are too high
-                altitudeAchieved();
+            rescueState.intent.targetAltitudeCm += rescueState.intent.altitudeStep;
-            } 
         } else {
-            if (rescueState.intent.targetAltitudeCm > rescueState.intent.returnAltitudeCm) {
+            // target altitude achieved - move on to ROTATE phase, returning at target altitude
-                rescueState.intent.altitudeStep = -rescueState.sensor.altitudeDataIntervalSeconds * gpsRescueConfig()->descendRate;
+            rescueState.intent.targetAltitudeCm = rescueState.intent.returnAltitudeCm;
-            } else if (rescueState.sensor.currentAltitudeCm < rescueState.intent.returnAltitudeCm) {
+            rescueState.intent.altitudeStep = 0.0f;
-                altitudeAchieved();
+            rescueState.phase = RESCUE_ROTATE;
-            }
         }
-        rescueState.intent.targetAltitudeCm += rescueState.intent.altitudeStep;
+
+        rescueState.intent.targetVelocityCmS = rescueState.sensor.velocityToHomeCmS;
+        // gives velocity P and I no error that otherwise would be present due to velocity drift at the start of the rescue
         break;
 
     case RESCUE_ROTATE:
-        if (rescueState.intent.yawAttenuator < 1.0f) { // gradually acquire yaw authority
+        if (rescueState.intent.yawAttenuator < 1.0f) { // acquire yaw authority over one second
-            rescueState.intent.yawAttenuator += 0.01f;
+            rescueState.intent.yawAttenuator += rescueState.sensor.gpsRescueTaskIntervalSeconds;
         }
         if (rescueState.sensor.absErrorAngle < 30.0f) {
-            rescueState.intent.pitchAngleLimitDeg = gpsRescueConfig()->angle; // allow pitch
+            rescueState.intent.pitchAngleLimitDeg = gpsRescueConfig()->maxRescueAngle; // allow pitch
             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
         }
+        initialVelocityLow = rescueState.sensor.velocityToHomeCmS < gpsRescueConfig()->rescueGroundspeed; // used to set direction of velocity target change
+        rescueState.intent.targetVelocityCmS = rescueState.sensor.velocityToHomeCmS;
         break;
 
     case RESCUE_FLY_HOME:
         if (rescueState.intent.yawAttenuator < 1.0f) { // be sure to accumulate full yaw authority
-            rescueState.intent.yawAttenuator += 0.01f;
+            rescueState.intent.yawAttenuator += rescueState.sensor.gpsRescueTaskIntervalSeconds;
         }
-        // steadily increase target velocity target until full return velocity is acquired
+
-        if (rescueState.intent.targetVelocityCmS < gpsRescueConfig()->rescueGroundspeed) {
+        // velocity PIDs are now active
-            rescueState.intent.targetVelocityCmS += 0.01f * gpsRescueConfig()->rescueGroundspeed;
+        // update target velocity gradually, aiming for rescueGroundspeed with a time constant of 1.0s
-        }
+        const float targetVelocityError = gpsRescueConfig()->rescueGroundspeed - rescueState.intent.targetVelocityCmS;
-        // acquire full roll authority slowly when pointing to home
+        const float velocityTargetStep = rescueState.sensor.gpsRescueTaskIntervalSeconds * targetVelocityError;
-        if (rescueState.sensor.absErrorAngle < 10.0f && rescueState.intent.rollAngleLimitDeg < gpsRescueConfig()->angle) {
+        // velocityTargetStep is positive when starting low, negative when starting high
-            // roll is primarily intended to deal with wind drift causing small yaw errors during return
+        const bool targetVelocityIsLow = rescueState.intent.targetVelocityCmS < gpsRescueConfig()->rescueGroundspeed;
-            rescueState.intent.rollAngleLimitDeg += 0.1f;
+        if (initialVelocityLow == targetVelocityIsLow) {
+            // also true if started faster than target velocity and target is still high
+            rescueState.intent.targetVelocityCmS += velocityTargetStep;
         }
 
+        rescueState.intent.velocityItermRelax += 0.5f * rescueState.sensor.gpsRescueTaskIntervalSeconds * (1.0f - rescueState.intent.velocityItermRelax);
+        // slowly introduce velocity iTerm accumulation at start, goes 0 ->1 with time constant 2.0s
+        // there is always a lot of lag at the start
+
+        rescueState.intent.velocityPidCutoffModifier = 2.0f - rescueState.intent.velocityItermRelax; 
+        // higher velocity cutoff for initial few seconds to improve accuracy; can be smoother later
+
+        rescueState.intent.rollAngleLimitDeg = 0.5f * rescueState.intent.velocityItermRelax * gpsRescueConfig()->maxRescueAngle;
+        // gradually gain roll capability to max of half of max pitch angle
+
         if (newGPSData) {
             if (rescueState.sensor.distanceToHomeM <= rescueState.intent.descentDistanceM) {
                 rescueState.phase = RESCUE_DESCENT;
@@ -795,6 +814,7 @@ void gpsRescueUpdate(void)
     case RESCUE_LANDING:
         // Reduce altitude target steadily until impact, then disarm.
         // control yaw angle and throttle and pitch, attenuate velocity, roll and pitch iTerm
+        // increase velocity smoothing cutoff as we get closer to ground
         descend();
         disarmOnImpact();
         break;
@@ -806,6 +826,7 @@ void gpsRescueUpdate(void)
     case RESCUE_ABORT:
         setArmingDisabled(ARMING_DISABLED_ARM_SWITCH);
         disarm(DISARM_REASON_FAILSAFE);
+        rescueState.intent.secondsFailing = 0; // reset sanity timers so we can re-arm
         rescueStop();
         break;
 

src/main/io/gps.c

@@ -82,7 +82,6 @@ uint16_t GPS_distanceToHome;        // distance to home point in meters
 uint32_t GPS_distanceToHomeCm;
 int16_t GPS_directionToHome;        // direction to home or hol point in degrees * 10
 uint32_t GPS_distanceFlownInCm;     // distance flown since armed in centimeters
-int16_t GPS_verticalSpeedInCmS;     // vertical speed in cm/s
 int16_t nav_takeoff_bearing;
 
 #define GPS_DISTANCE_FLOWN_MIN_SPEED_THRESHOLD_CM_S 15 // 0.54 km/h 0.335 mph
@@ -107,7 +106,7 @@ uint8_t GPS_svinfo_cno[GPS_SV_MAXSATS_M8N];
 #define UBLOX_ACK_TIMEOUT_MAX_COUNT (25)
 
 static serialPort_t *gpsPort;
-static float gpsSampleRateHz;
+static float gpsDataIntervalSeconds;
 
 typedef struct gpsInitData_s {
     uint8_t index;
@@ -327,8 +326,7 @@ static void gpsSetState(gpsState_e state)
 
 void gpsInit(void)
 {
-    gpsSampleRateHz = 0.0f;
+    gpsDataIntervalSeconds = 0.1f;
-
     gpsData.baudrateIndex = 0;
     gpsData.errors = 0;
     gpsData.timeouts = 0;
@@ -1843,19 +1841,15 @@ void GPS_calc_longitude_scaling(int32_t lat)
 }
 
 ////////////////////////////////////////////////////////////////////////////////////
-// Calculate the distance flown and vertical speed from gps position data
+// Calculate the distance flown from gps position data
 //
-static void GPS_calculateDistanceFlownVerticalSpeed(bool initialize)
+static void GPS_calculateDistanceFlown(bool initialize)
 {
     static int32_t lastCoord[2] = { 0, 0 };
     static int32_t lastAlt;
-    static int32_t lastMillis;
-
-    int currentMillis = millis();
 
     if (initialize) {
         GPS_distanceFlownInCm = 0;
-        GPS_verticalSpeedInCmS = 0;
     } else {
         if (STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED)) {
             uint16_t speed = gpsConfig()->gps_use_3d_speed ? gpsSol.speed3d : gpsSol.groundSpeed;
@@ -1870,13 +1864,10 @@ static void GPS_calculateDistanceFlownVerticalSpeed(bool initialize)
                 GPS_distanceFlownInCm += dist;
             }
         }
-        GPS_verticalSpeedInCmS = (gpsSol.llh.altCm - lastAlt) * 1000 / (currentMillis - lastMillis);
-        GPS_verticalSpeedInCmS = constrain(GPS_verticalSpeedInCmS, -1500, 1500);
     }
     lastCoord[GPS_LONGITUDE] = gpsSol.llh.lon;
     lastCoord[GPS_LATITUDE] = gpsSol.llh.lat;
     lastAlt = gpsSol.llh.altCm;
-    lastMillis = currentMillis;
 }
 
 void GPS_reset_home_position(void)
@@ -1895,7 +1886,7 @@ void GPS_reset_home_position(void)
             // PS: to test for gyro cal, check for !ARMED, since we cannot be here while disarmed other than via gyro cal
         }
     }
-    GPS_calculateDistanceFlownVerticalSpeed(true); // Initialize
+    GPS_calculateDistanceFlown(true); // Initialize
 }
 
 ////////////////////////////////////////////////////////////////////////////////////
@@ -1920,8 +1911,8 @@ void GPS_calculateDistanceAndDirectionToHome(void)
         uint32_t dist;
         int32_t dir;
         GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_home[GPS_LATITUDE], &GPS_home[GPS_LONGITUDE], &dist, &dir);
-        GPS_distanceToHome = dist / 100; // m/s
+        GPS_distanceToHome = dist / 100; // m
-        GPS_distanceToHomeCm = dist; // cm/sec
+        GPS_distanceToHomeCm = dist; // cm
         GPS_directionToHome = dir / 10; // degrees * 10 or decidegrees
     } else {
         // If we don't have home set, do not display anything
@@ -1933,11 +1924,21 @@ void GPS_calculateDistanceAndDirectionToHome(void)
 
 void onGpsNewData(void)
 {
-    static timeUs_t timeUs, lastTimeUs = 0;
+    static timeUs_t lastTimeUs = 0;
+    const timeUs_t timeUs = micros();
+
+    // calculate GPS solution interval
+    // !!! TOO MUCH JITTER TO BE USEFUL - need an exact time !!!
+    const float gpsDataIntervalS = cmpTimeUs(timeUs, lastTimeUs) / 1e6f;
+    // dirty hack to remove jitter from interval
+    if (gpsDataIntervalS < 0.15f) {
+        gpsDataIntervalSeconds = 0.1f;
+    } else if (gpsDataIntervalS < 0.4f) {
+        gpsDataIntervalSeconds = 0.2f;
+    } else {
+        gpsDataIntervalSeconds = 1.0f;
+    }
 
-    // Detect current sample rate of GPS solution
-    timeUs = micros();
-    gpsSampleRateHz = 1e6f / cmpTimeUs(timeUs, lastTimeUs);
     lastTimeUs = timeUs;
 
     if (!(STATE(GPS_FIX) && gpsSol.numSat >= GPS_MIN_SAT_COUNT)) {
@@ -1947,7 +1948,7 @@ void onGpsNewData(void)
 
     GPS_calculateDistanceAndDirectionToHome();
     if (ARMING_FLAG(ARMED)) {
-        GPS_calculateDistanceFlownVerticalSpeed(false);
+        GPS_calculateDistanceFlown(false);
     }
 
 #ifdef USE_GPS_RESCUE
@@ -1965,9 +1966,9 @@ void gpsSetFixState(bool state)
     }
 }
 
-float gpsGetSampleRateHz(void)
+float getGpsDataIntervalSeconds(void)
 {
-    return gpsSampleRateHz;
+    return gpsDataIntervalSeconds;
 }
 
 #endif // USE_GPS

src/main/io/gps.h

@@ -150,7 +150,6 @@ extern uint16_t GPS_distanceToHome;        // distance to home point in meters
 extern uint32_t GPS_distanceToHomeCm;      // distance to home point in cm
 extern int16_t GPS_directionToHome;        // direction to home or hol point in degrees
 extern uint32_t GPS_distanceFlownInCm;     // distance flown since armed in centimeters
-extern int16_t GPS_verticalSpeedInCmS;     // vertical speed in cm/s
 extern int16_t GPS_angle[ANGLE_INDEX_COUNT];                // it's the angles that must be applied for GPS correction
 extern float GPS_scaleLonDown;  // this is used to offset the shrinking longitude as we go towards the poles
 extern int16_t nav_takeoff_bearing;
@@ -214,4 +213,4 @@ void GPS_reset_home_position(void);
 void GPS_calc_longitude_scaling(int32_t lat);
 void GPS_distance_cm_bearing(int32_t *currentLat1, int32_t *currentLon1, int32_t *destinationLat2, int32_t *destinationLon2, uint32_t *dist, int32_t *bearing);
 void gpsSetFixState(bool state);
-float gpsGetSampleRateHz(void);
+float getGpsDataIntervalSeconds(void);

src/main/msp/msp.c

@@ -1535,7 +1535,7 @@ case MSP_NAME:
 
 #ifdef USE_GPS_RESCUE
     case MSP_GPS_RESCUE:
-        sbufWriteU16(dst, gpsRescueConfig()->angle);
+        sbufWriteU16(dst, gpsRescueConfig()->maxRescueAngle);
         sbufWriteU16(dst, gpsRescueConfig()->initialAltitudeM);
         sbufWriteU16(dst, gpsRescueConfig()->descentDistanceM);
         sbufWriteU16(dst, gpsRescueConfig()->rescueGroundspeed);
@@ -2798,7 +2798,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
 
 #ifdef USE_GPS_RESCUE
         case MSP_SET_GPS_RESCUE:
-        gpsRescueConfigMutable()->angle = sbufReadU16(src);
+        gpsRescueConfigMutable()->maxRescueAngle = sbufReadU16(src);
         gpsRescueConfigMutable()->initialAltitudeM = sbufReadU16(src);
         gpsRescueConfigMutable()->descentDistanceM = sbufReadU16(src);
         gpsRescueConfigMutable()->rescueGroundspeed = sbufReadU16(src);

src/main/pg/gps_rescue.c

@@ -29,26 +29,28 @@
 
 #include "gps_rescue.h"
 
-PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 3);
+PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 4);
 
 PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
 
     .minRescueDth = 30,
     .altitudeMode = GPS_RESCUE_ALT_MODE_MAX,
     .rescueAltitudeBufferM = 10,
-    .ascendRate = 500,          // cm/s, for altitude corrections on ascent
+    .ascendRate = 750,          // cm/s, for altitude corrections on ascent
 
     .initialAltitudeM = 30,
-    .rescueGroundspeed = 500,
+    .rescueGroundspeed = 750,
-    .angle = 40,
+    .maxRescueAngle = 70,
     .rollMix = 150,
+    .pitchCutoffHz = 75,
 
     .descentDistanceM = 20,
-    .descendRate = 100,         // cm/s, minimum for descent and landing phase, or for descending if starting high ascent
+    .descendRate = 150,         // cm/s, minimum for descent and landing phase, or for descending if starting high ascent
     .targetLandingAltitudeM = 4,
+    .disarmThreshold = 20,
 
     .throttleMin = 1100,
-    .throttleMax = 1600,
+    .throttleMax = 1700,
     .throttleHover = 1275,
 
     .allowArmingWithoutFix = false,
@@ -57,10 +59,10 @@ PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
 
     .throttleP = 15,
     .throttleI = 15,
-    .throttleD = 15,
+    .throttleD = 20,
     .velP = 8,
-    .velI = 30,
+    .velI = 40,
-    .velD = 20,
+    .velD = 12,
     .yawP = 20,
 
     .useMag = GPS_RESCUE_USE_MAG

src/main/pg/gps_rescue.h

@@ -26,7 +26,7 @@
 
 typedef struct gpsRescue_s {
 
-    uint16_t angle; // degrees
+    uint16_t maxRescueAngle; // degrees
     uint16_t initialAltitudeM; // meters
     uint16_t descentDistanceM; // meters
     uint16_t rescueGroundspeed; // centimeters per second
@@ -47,6 +47,8 @@ typedef struct gpsRescue_s {
     uint16_t descendRate;
     uint16_t rescueAltitudeBufferM; // meters
     uint8_t  rollMix;
+    uint8_t  disarmThreshold;
+    uint8_t  pitchCutoffHz;
 
 } gpsRescueConfig_t;
 

src/main/rx/rx.c

@@ -600,7 +600,7 @@ static uint16_t calculateChannelMovingAverage(uint8_t chan, uint16_t sample)
 static uint16_t getRxfailValue(uint8_t channel)
 {
     const rxFailsafeChannelConfig_t *channelFailsafeConfig = rxFailsafeChannelConfigs(channel);
-    const bool failsafeAuxSwitch = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
+    const bool boxFailsafeSwitchIsOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
 
     switch (channelFailsafeConfig->mode) {
     case RX_FAILSAFE_MODE_AUTO:
@@ -621,7 +621,7 @@ static uint16_t getRxfailValue(uint8_t channel)
     default:
     case RX_FAILSAFE_MODE_INVALID:
     case RX_FAILSAFE_MODE_HOLD:
-        if (failsafeAuxSwitch) {
+        if (boxFailsafeSwitchIsOn) {
             return rcRaw[channel]; // current values are allowed through on held channels with switch induced failsafe
         } else {
             return rcData[channel]; // last good value
@@ -672,41 +672,46 @@ static void readRxChannelsApplyRanges(void)
 void detectAndApplySignalLossBehaviour(void)
 {
     const uint32_t currentTimeMs = millis();
-    const bool failsafeAuxSwitch = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
+    const bool boxFailsafeSwitchIsOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
-    rxFlightChannelsValid = rxSignalReceived && !failsafeAuxSwitch;
+    rxFlightChannelsValid = rxSignalReceived && !boxFailsafeSwitchIsOn;
-    //  set rxFlightChannelsValid false when a packet is bad or we use a failsafe switch
+    // rxFlightChannelsValid is false after 100ms of no packets, or as soon as use the BOXFAILSAFE switch is actioned
+    // rxFlightChannelsValid is true the instant we get a good packet or the BOXFAILSAFE switch is reverted
+    // can also go false with good packets but where one flight channel is bad > 300ms (PPM type receiver error)
 
     for (int channel = 0; channel < rxChannelCount; channel++) {
         float sample = rcRaw[channel]; // sample has latest RC value, rcData has last 'accepted valid' value
         const bool thisChannelValid = rxFlightChannelsValid && isPulseValid(sample);
-        // if the whole packet is bad, consider all channels bad
+        // if the whole packet is bad, or BOXFAILSAFE switch is actioned, consider all channels bad
-
         if (thisChannelValid) {
             //  reset the invalid pulse period timer for every good channel
             validRxSignalTimeout[channel] = currentTimeMs + MAX_INVALID_PULSE_TIME_MS;
         }
 
-        if (ARMING_FLAG(ARMED) && failsafeIsActive()) {
+        if (failsafeIsActive()) {
-            // while in failsafe Stage 2, whether Rx loss or switch induced, pass valid incoming flight channel values
+            // we are in failsafe Stage 2, whether Rx loss or BOXFAILSAFE induced
-            // this allows GPS Rescue to detect the 30% requirement for termination
+            // pass valid incoming flight channel values to FC,
+            // so that GPS Rescue can get the 30% requirement for termination of the rescue
             if (channel < NON_AUX_CHANNEL_COUNT) {
                 if (!thisChannelValid) {
                     if (channel == THROTTLE ) {
-                        sample = failsafeConfig()->failsafe_throttle; // stage 2 failsafe throttle value
+                        sample = failsafeConfig()->failsafe_throttle;
+                        // stage 2 failsafe throttle value. In GPS Rescue Flight mode, gpsRescueGetThrottle overrides, late in mixer.c
                     } else {
                         sample = rxConfig()->midrc;
                     }
                 }
             } else {
-                //  During Stage 2, set aux channels as per Stage 1 configuration
+                // set aux channels as per Stage 1 failsafe hold/set values, allow all for Failsafe and GPS rescue MODE switches
                 sample = getRxfailValue(channel);
             }
         } else {
-            if (failsafeAuxSwitch) {
+            // we are normal, or in failsafe stage 1
+            if (boxFailsafeSwitchIsOn) {
+                // BOXFAILSAFE active, but not in stage 2 yet, use stage 1 values
                 sample = getRxfailValue(channel);
                 //  set channels to Stage 1 values immediately failsafe switch is activated
             } else if (!thisChannelValid) {
-                // everything was normal and this channel was invalid
+                // everything is normal but this channel was invalid
                 if (cmp32(currentTimeMs, validRxSignalTimeout[channel]) < 0) {
                     // first 300ms of Stage 1 failsafe
                     sample = rcData[channel];
@@ -721,6 +726,7 @@ void detectAndApplySignalLossBehaviour(void)
                     // set channels that are invalid for more than 300ms to Stage 1 values
                 }
             }
+            // everything is normal, ie rcData[channel] will be set to rcRaw(channel) via 'sample'
         }
 
         sample = constrainf(sample, PWM_PULSE_MIN, PWM_PULSE_MAX);
@@ -740,10 +746,10 @@ void detectAndApplySignalLossBehaviour(void)
 
     if (rxFlightChannelsValid) {
         failsafeOnValidDataReceived();
-        //  --> start the timer to exit stage 2 failsafe
+        //  --> start the timer to exit stage 2 failsafe 100ms after losing all packets or the BOXFAILSAFE switch is actioned
     } else {
         failsafeOnValidDataFailed();
-        //  -> start timer to enter stage2 failsafe
+        //  -> start timer to enter stage2 failsafe the instant we get a good packet or the BOXFAILSAFE switch is reverted
     }
 
     DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 3, rcData[THROTTLE]);

src/test/unit/arming_prevention_unittest.cc

@@ -97,8 +97,10 @@ extern "C" {
     bool mockIsUpright = false;
     uint8_t activePidLoopDenom = 1;
 
-    float gpsGetSampleRateHz(void) { return 10.0f; }
+    float getGpsDataIntervalSeconds(void) { return 0.1f; }
+    void pt1FilterUpdateCutoff(pt1Filter_t *filter, float k) { filter->k = k; }
     void pt2FilterUpdateCutoff(pt2Filter_t *filter, float k) { filter->k = k; }
+    void pt3FilterUpdateCutoff(pt3Filter_t *filter, float k) { filter->k = k; }
 }
 
 uint32_t simulationFeatureFlags = 0;
@@ -1143,11 +1145,18 @@ extern "C" {
         UNUSED(throttleDLpf);
         return 0.0f;
     }
-    void pt3FilterInit(pt3Filter_t *pitchLpf, float) {
+    void pt1FilterInit(pt1Filter_t *velocityDLpf, float) {
-        UNUSED(pitchLpf);
+        UNUSED(velocityDLpf);
     }
-    float pt3FilterApply(pt3Filter_t *pitchLpf, float) {
+    float pt1FilterApply(pt1Filter_t *velocityDLpf, float) {
-        UNUSED(pitchLpf);
+        UNUSED(velocityDLpf);
+        return 0.0f;
+    }
+    void pt3FilterInit(pt3Filter_t *velocityUpsampleLpf, float) {
+        UNUSED(velocityUpsampleLpf);
+    }
+    float pt3FilterApply(pt3Filter_t *velocityUpsampleLpf, float) {
+        UNUSED(velocityUpsampleLpf);
         return 0.0f;
     }
     void getRcDeflectionAbs(void) {}

src/test/unit/flight_imu_unittest.cc

@@ -213,7 +213,6 @@ extern "C" {
     mag_t mag;
 
     gpsSolutionData_t gpsSol;
-    int16_t GPS_verticalSpeedInCmS;
 
     uint8_t debugMode;
     int16_t debug[DEBUG16_VALUE_COUNT];