diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 96d75afdee..8c07030cca 100644 --- a/src/main/blackbox/blackbox.c +++ b/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) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index e99482fa4f..ffebcb6ef1 100644 --- a/src/main/cli/settings.c +++ b/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) }, diff --git a/src/main/cms/cms_menu_gps_rescue.c b/src/main/cms/cms_menu_gps_rescue.c index a68ac9f564..d3350e1b9f 100644 --- a/src/main/cms/cms_menu_gps_rescue.c +++ b/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; diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h index b1be0b58d4..be12766b2b 100644 --- a/src/main/fc/parameter_names.h +++ b/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" diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 0872abea86..167df0e737 100644 --- a/src/main/fc/rc_controls.c +++ b/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))); - if (ARMING_FLAG(ARMED) && (failsafeIsReceivingRxData() || switchFailsafe)) { + const bool boxFailsafeSwitchIsOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE); + 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); } } diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 94aa551e18..d1a9aac1cc 100644 --- a/src/main/flight/failsafe.c +++ b/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, - // stays false until after recovery period expires - // Link down is the trigger for the various failsafe stage 2 outcomes. + // False with BOXFAILSAFE switch or when no valid packets for 100ms or any flight channel invalid for 300ms, + // becomes true immediately BOXFAILSAFE switch reverts, or after recovery period expires when valid packets are received + // 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 - // FAILSAFE_RXLINK_UP is set in failsafeOnValidDataReceived, after the various Stage 1 and recovery delays - // failsafeOnValidDataReceived runs from detectAndApplySignalLossBehaviour + // returns state of FAILSAFE_RXLINK_UP, which + // goes false after the stage 1 delay, whether from signal loss or BOXFAILSAFE switch activation + // 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)) { - // Aux switch set to failsafe stage2 emulates immediate loss of signal without waiting - failsafeOnValidDataFailed(); + if (IS_RC_MODE_ACTIVE(BOXFAILSAFE) && (failsafeConfig()->failsafe_switch_mode == FAILSAFE_SWITCH_MODE_STAGE2)) { + // Force immediate stage 2 responses if mode is failsafe stage2 to emulate immediate loss of signal without waiting 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); diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h index 350c9e2df2..959ab9644f 100644 --- a/src/main/flight/failsafe.h +++ b/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); diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c index b8e44b4e3c..14a18932b7 100644 --- a/src/main/flight/gps_rescue.c +++ b/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_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 +#define GPS_RESCUE_MAX_THROTTLE_ITERM 200 // max iterm value for throttle in degrees * 100 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 pt3Filter_t pitchLpf; +static pt1Filter_t velocityDLpf; +static pt3Filter_t velocityUpsampleLpf; rescueState_s rescueState; void gpsRescueInit(void) { - const float sampleTimeS = HZ_TO_INTERVAL(TASK_GPS_RESCUE_RATE_HZ); - float cutoffHz, gain; + rescueState.sensor.gpsRescueTaskIntervalSeconds = HZ_TO_INTERVAL(TASK_GPS_RESCUE_RATE_HZ); + 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; - gain = pt2FilterGain(cutoffHz, 1.0f); - pt2FilterInit(&velocityDLpf, gain); + cutoffHz = gpsRescueConfig()->pitchCutoffHz / 100.0f; + rescueState.intent.velocityPidCutoff = cutoffHz; + rescueState.intent.velocityPidCutoffModifier = 1.0f; + gain = pt1FilterGain(cutoffHz, 1.0f); + pt1FilterInit(&velocityDLpf, gain); - cutoffHz = 4.0f; - gain = pt3FilterGain(cutoffHz, sampleTimeS); - pt3FilterInit(&pitchLpf, gain); + cutoffHz *= 4.0f; + gain = pt3FilterGain(cutoffHz, rescueState.sensor.gpsRescueTaskIntervalSeconds); + 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; - // increase amount added when GPS sample rate is slower - velocityI = constrainf(velocityI, -1.0f * GPS_RESCUE_MAX_ITERM_VELOCITY, 1.0f * GPS_RESCUE_MAX_ITERM_VELOCITY); - // I component alone cannot exceed a pitch angle of 10% + velocityI += 0.01f * gpsRescueConfig()->velI * velocityError * sampleIntervalNormaliseFactor * rescueState.intent.velocityItermRelax; + // velocityItermRelax is a time-based factor, 0->1 with time constant of 1s from when we start to fly home + // avoids excess iTerm during the initial acceleration phase. + 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; - // reduces iTerm as target velocity decreases, to minimise overshoot during deceleration to landing phase + // smooth the D steps + 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; - if (rescueState.phase == RESCUE_FLY_HOME) { - pitchAdjustment *= 0.7f; // attenuate pitch PIDs during main fly home phase, tighten up in descent. - } - pitchAdjustment += velocityI * velocityIAttenuator; + pitchAdjustment = velocityP + velocityI + velocityD; + pitchAdjustment = constrainf(pitchAdjustment, -pitchAngleLimit, pitchAngleLimit); + // limit to maximum allowed angle - 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; - const timeDelta_t gpsDataIntervalUs = cmpTimeUs(currentTimeUs, previousGPSDataTimeUs); - rescueState.sensor.gpsDataIntervalSeconds = constrainf(gpsDataIntervalUs * 0.000001f, 0.01f, 1.0f); + rescueState.sensor.gpsDataIntervalSeconds = getGpsDataIntervalSeconds(); // 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.targetVelocityCmS = gpsRescueConfig()->rescueGroundspeed * proximityToLandingArea; + rescueState.intent.proximityToLandingArea = constrainf(distanceToLandingAreaM / rescueState.intent.descentDistanceM, 0.0f, 1.0f); + 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 - rescueState.intent.altitudeStep = -rescueState.sensor.altitudeDataIntervalSeconds * gpsRescueConfig()->descendRate; - rescueState.intent.targetVelocityCmS = 0; // zero forward velocity - rescueState.intent.pitchAngleLimitDeg = 0; // flat on pitch - rescueState.intent.rollAngleLimitDeg = 0.0f; // flat on roll also - rescueState.intent.targetAltitudeCm = rescueState.sensor.currentAltitudeCm + rescueState.intent.altitudeStep; - rescueState.phase = RESCUE_LANDING; - // start landing from current altitude + if (rescueState.sensor.distanceToHomeM < 5.0f && rescueState.sensor.currentAltitudeCm < rescueState.intent.targetLandingAltitudeCm) { + // attempted initiation within 5m of home, and 'on the ground' -> instant disarm, for safety reasons + rescueState.phase = RESCUE_ABORT; + } else { + // Otherwise, attempted initiation inside minimum activation distance, at any height -> landing mode + rescueState.intent.altitudeStep = -rescueState.sensor.altitudeDataIntervalSeconds * gpsRescueConfig()->descendRate; + rescueState.intent.targetVelocityCmS = 0; // zero forward velocity + 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; - startedLow = (rescueState.sensor.currentAltitudeCm <= rescueState.intent.returnAltitudeCm); + initialAltitudeLow = (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,46 +737,61 @@ 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) { - if (rescueState.intent.targetAltitudeCm < rescueState.intent.returnAltitudeCm) { - rescueState.intent.altitudeStep = rescueState.sensor.altitudeDataIntervalSeconds * gpsRescueConfig()->ascendRate; - } else if (rescueState.sensor.currentAltitudeCm > rescueState.intent.returnAltitudeCm) { - altitudeAchieved(); - } + rescueState.intent.altitudeStep = ((initialAltitudeLow) ? gpsRescueConfig()->ascendRate : -1.0f * gpsRescueConfig()->descendRate) * rescueState.sensor.gpsRescueTaskIntervalSeconds; + const bool currentAltitudeLow = rescueState.sensor.currentAltitudeCm < rescueState.intent.returnAltitudeCm; + if (initialAltitudeLow == currentAltitudeLow) { + // we started low, and still are low; also true if we started high, and still are too high + rescueState.intent.targetAltitudeCm += rescueState.intent.altitudeStep; } else { - if (rescueState.intent.targetAltitudeCm > rescueState.intent.returnAltitudeCm) { - rescueState.intent.altitudeStep = -rescueState.sensor.altitudeDataIntervalSeconds * gpsRescueConfig()->descendRate; - } else if (rescueState.sensor.currentAltitudeCm < rescueState.intent.returnAltitudeCm) { - altitudeAchieved(); - } + // target altitude achieved - move on to ROTATE phase, returning at target altitude + rescueState.intent.targetAltitudeCm = rescueState.intent.returnAltitudeCm; + rescueState.intent.altitudeStep = 0.0f; + 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 - rescueState.intent.yawAttenuator += 0.01f; + if (rescueState.intent.yawAttenuator < 1.0f) { // acquire yaw authority over one second + 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) { - rescueState.intent.targetVelocityCmS += 0.01f * gpsRescueConfig()->rescueGroundspeed; + + // 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; + const float velocityTargetStep = rescueState.sensor.gpsRescueTaskIntervalSeconds * targetVelocityError; + // velocityTargetStep is positive when starting low, negative when starting high + const bool targetVelocityIsLow = rescueState.intent.targetVelocityCmS < gpsRescueConfig()->rescueGroundspeed; + if (initialVelocityLow == targetVelocityIsLow) { + // also true if started faster than target velocity and target is still high + rescueState.intent.targetVelocityCmS += velocityTargetStep; } - // acquire full roll authority slowly when pointing to home - if (rescueState.sensor.absErrorAngle < 10.0f && rescueState.intent.rollAngleLimitDeg < gpsRescueConfig()->angle) { - // roll is primarily intended to deal with wind drift causing small yaw errors during return - rescueState.intent.rollAngleLimitDeg += 0.1f; - } + + 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) { @@ -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; diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 63cb3cb280..c6e169238d 100644 --- a/src/main/io/gps.c +++ b/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_distanceToHomeCm = dist; // cm/sec + GPS_distanceToHome = dist / 100; // m + 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 diff --git a/src/main/io/gps.h b/src/main/io/gps.h index ae609a931e..ecb1460a59 100644 --- a/src/main/io/gps.h +++ b/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); diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 72e8da47ef..1b89604f50 100644 --- a/src/main/msp/msp.c +++ b/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); diff --git a/src/main/pg/gps_rescue.c b/src/main/pg/gps_rescue.c index d0ade463fe..525c32ca43 100644 --- a/src/main/pg/gps_rescue.c +++ b/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, - .angle = 40, + .rescueGroundspeed = 750, + .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, - .velD = 20, + .velI = 40, + .velD = 12, .yawP = 20, .useMag = GPS_RESCUE_USE_MAG diff --git a/src/main/pg/gps_rescue.h b/src/main/pg/gps_rescue.h index 0a78dbbc89..4d9bed050d 100644 --- a/src/main/pg/gps_rescue.h +++ b/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; diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 5d88c723d4..4c206f40d1 100644 --- a/src/main/rx/rx.c +++ b/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); - rxFlightChannelsValid = rxSignalReceived && !failsafeAuxSwitch; - // set rxFlightChannelsValid false when a packet is bad or we use a failsafe switch + const bool boxFailsafeSwitchIsOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE); + rxFlightChannelsValid = rxSignalReceived && !boxFailsafeSwitchIsOn; + // 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()) { - // while in failsafe Stage 2, whether Rx loss or switch induced, pass valid incoming flight channel values - // this allows GPS Rescue to detect the 30% requirement for termination + if (failsafeIsActive()) { + // we are in failsafe Stage 2, whether Rx loss or BOXFAILSAFE induced + // 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]); diff --git a/src/test/unit/arming_prevention_unittest.cc b/src/test/unit/arming_prevention_unittest.cc index 6c1a8d672c..2fceb23724 100644 --- a/src/test/unit/arming_prevention_unittest.cc +++ b/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) { - UNUSED(pitchLpf); + void pt1FilterInit(pt1Filter_t *velocityDLpf, float) { + UNUSED(velocityDLpf); } - float pt3FilterApply(pt3Filter_t *pitchLpf, float) { - UNUSED(pitchLpf); + float pt1FilterApply(pt1Filter_t *velocityDLpf, float) { + 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) {} diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc index c901df0fb0..a78daf0421 100644 --- a/src/test/unit/flight_imu_unittest.cc +++ b/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];