From f69fedd1f28df1400f39d53b4fe6ce0f5651476f Mon Sep 17 00:00:00 2001 From: ctzsnooze Date: Thu, 25 May 2023 18:02:24 +1000 Subject: [PATCH] Improve behaviour when GPS Rescue is initiated close to home (#12715) * When initiated close to home, descend then land, 50 deg max * restore divisions thanks -ffast-math * fix typo in comment * refactoring, improving flight behaviour * modify comments, add a constraint * refactor throttle D Multiplier and velocity iterm, improve comments * move out if too close, boost IMU gain in fly home --- src/main/cli/settings.c | 4 +- src/main/flight/gps_rescue.c | 229 +++++++++++++++++++++-------------- src/main/pg/gps_rescue.c | 2 +- 3 files changed, 139 insertions(+), 96 deletions(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 8be5bb51d1..b0b95c7b8c 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1014,7 +1014,7 @@ const clivalue_t valueTable[] = { #ifdef USE_GPS_RESCUE // PG_GPS_RESCUE - { PARAM_NAME_GPS_RESCUE_MIN_START_DIST, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 20, 1000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, minRescueDth) }, + { PARAM_NAME_GPS_RESCUE_MIN_START_DIST, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 30 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, minRescueDth) }, { PARAM_NAME_GPS_RESCUE_ALT_MODE, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_RESCUE_ALT_MODE }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, altitudeMode) }, { PARAM_NAME_GPS_RESCUE_INITIAL_CLIMB, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, rescueAltitudeBufferM) }, { PARAM_NAME_GPS_RESCUE_ASCEND_RATE, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 50, 2500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, ascendRate) }, @@ -1025,7 +1025,7 @@ const clivalue_t valueTable[] = { { 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_DESCENT_DIST, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 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) }, diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c index a542946651..0aa0069dd4 100644 --- a/src/main/flight/gps_rescue.c +++ b/src/main/flight/gps_rescue.c @@ -85,14 +85,13 @@ typedef struct { float rollAngleLimitDeg; float descentDistanceM; int8_t secondsFailing; - float altitudeStep; - float descentRateModifier; + float throttleDMultiplier; float yawAttenuator; float disarmThreshold; float velocityITermAccumulator; float velocityPidCutoff; float velocityPidCutoffModifier; - float proximityToLandingArea; + float velocityItermAttenuator; float velocityItermRelax; } rescueIntent_s; @@ -125,7 +124,6 @@ typedef struct { } rescueState_s; #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_THROTTLE_ITERM 200 // max iterm value for throttle in degrees * 100 static float rescueThrottle; @@ -193,8 +191,10 @@ static void setReturnAltitude(void) // set the target altitude to current values, so there will be no D kick on first run rescueState.intent.targetAltitudeCm = rescueState.sensor.currentAltitudeCm; - // Keep the descent distance and intended altitude up to date with latest GPS values - rescueState.intent.descentDistanceM = constrainf(rescueState.sensor.distanceToHomeM, GPS_RESCUE_MIN_DESCENT_DIST_M, gpsRescueConfig()->descentDistanceM); + // Intended descent distance for rescues that start outside the minRescueDth distance + // Set this to the user's intended descent distance, but not more than half the distance to home to ensure some fly home time + rescueState.intent.descentDistanceM = fminf(0.5f * rescueState.sensor.distanceToHomeM, gpsRescueConfig()->descentDistanceM); + const float initialAltitudeCm = gpsRescueConfig()->initialAltitudeM * 100.0f; const float rescueAltitudeBufferCm = gpsRescueConfig()->rescueAltitudeBufferM * 100.0f; switch (gpsRescueConfig()->altitudeMode) { @@ -252,7 +252,7 @@ static void rescueAttainPosition(void) Altitude (throttle) controller */ // currentAltitudeCm is updated at TASK_GPS_RESCUE_RATE_HZ - const float altitudeError = (rescueState.intent.targetAltitudeCm - rescueState.sensor.currentAltitudeCm) * 0.01f; + const float altitudeError = (rescueState.intent.targetAltitudeCm - rescueState.sensor.currentAltitudeCm) / 100.0f; // height above target in metres (negative means too low) // at the start, the target starts at current altitude plus one step. Increases stepwise to intended value. @@ -265,14 +265,14 @@ static void rescueAttainPosition(void) // 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 - float verticalSpeed = ((altitudeError - previousAltitudeError) / rescueState.sensor.altitudeDataIntervalSeconds); + float throttleD = ((altitudeError - previousAltitudeError) / rescueState.sensor.altitudeDataIntervalSeconds); previousAltitudeError = altitudeError; - verticalSpeed += rescueState.intent.descentRateModifier * verticalSpeed; - // add up to 2x D when descent rate is faster - - float throttleD = pt2FilterApply(&throttleDLpf, verticalSpeed); - - throttleD = gpsRescueConfig()->throttleD * throttleD; + // increase by up to 2x when descent rate is faster + throttleD *= rescueState.intent.throttleDMultiplier; + // apply user's throttle D gain + throttleD *= gpsRescueConfig()->throttleD; + // smooth + throttleD = pt2FilterApply(&throttleDLpf, throttleD); // acceleration component not currently implemented - was needed previously due to GPS lag, maybe not needed now. @@ -300,7 +300,7 @@ static void rescueAttainPosition(void) // 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 // 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 = rescueState.sensor.errorAngle * gpsRescueConfig()->yawP * rescueState.intent.yawAttenuator / 10.0f; 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 @@ -338,14 +338,16 @@ static void rescueAttainPosition(void) // I component 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 + // avoids excess iTerm accumulation during the initial acceleration phase and during descent. + + // force iTerm to zero, to minimise overshoot during deceleration with descent + // and because if we over-fly the home point, we need to re-accumulate from zero, not the previously accumulated value + velocityI *= rescueState.intent.velocityItermAttenuator; const float pitchAngleLimit = rescueState.intent.pitchAngleLimitDeg * 100.0f; const float velocityILimit = 0.5f * pitchAngleLimit; - velocityI = constrainf(velocityI, -velocityILimit, velocityILimit); // I component alone cannot exceed half the max pitch angle + velocityI = constrainf(velocityI, -velocityILimit, velocityILimit); // D component float velocityD = ((velocityError - previousVelocityError) / sampleIntervalNormaliseFactor); @@ -360,8 +362,8 @@ static void rescueAttainPosition(void) velocityD = pt1FilterApply(&velocityDLpf, velocityD); pitchAdjustment = velocityP + velocityI + velocityD; - pitchAdjustment = constrainf(pitchAdjustment, -pitchAngleLimit, pitchAngleLimit); // limit to maximum allowed angle + pitchAdjustment = constrainf(pitchAdjustment, -pitchAngleLimit, pitchAngleLimit); // pitchAdjustment is the absolute Pitch angle adjustment value in degrees * 100 // it gets added to the normal level mode Pitch adjustments in pid.c @@ -369,6 +371,12 @@ static void rescueAttainPosition(void) DEBUG_SET(DEBUG_GPS_RESCUE_VELOCITY, 1, lrintf(velocityD)); } + // if initiated too close, and in the climb phase, pitch forward in whatever direction the nose is oriented until min distance away + // intent is to get far enough away that, with an IMU error, the quad will have enough distance to home to correct that error in the fly home phase + if ((rescueState.phase == RESCUE_ATTAIN_ALT) && (rescueState.sensor.distanceToHomeM < gpsRescueConfig()->minRescueDth)) { + pitchAdjustment = 1500.0f; // 15 degree pitch forward + } + // upsampling and smoothing of pitch angle steps float pitchAdjustmentFiltered = pt3FilterApply(&velocityUpsampleLpf, pitchAdjustment); @@ -550,7 +558,7 @@ static void sensorUpdate(void) } rescueState.sensor.directionToHome = GPS_directionToHome; - rescueState.sensor.errorAngle = (attitude.values.yaw - rescueState.sensor.directionToHome) * 0.1f; + rescueState.sensor.errorAngle = (attitude.values.yaw - rescueState.sensor.directionToHome) / 10.0f; // both attitude and direction are in degrees * 10, errorAngle is degrees if (rescueState.sensor.errorAngle <= -180) { rescueState.sensor.errorAngle += 360; @@ -568,27 +576,44 @@ static void sensorUpdate(void) rescueState.sensor.distanceToHomeM = rescueState.sensor.distanceToHomeCm / 100.0f; rescueState.sensor.groundSpeedCmS = gpsSol.groundSpeed; // cm/s + rescueState.sensor.gpsDataIntervalSeconds = getGpsDataIntervalSeconds(); + // Range from 10ms (100hz) to 1000ms (1Hz). Intended to cover common GPS data rates and exclude unusual values. + // when there is significant velocity error, increase IMU COG Gain for yaw to a higher value, and reduce max pitch angle if (gpsRescueConfig()->rescueGroundspeed) { const float rescueGroundspeed = 1000.0f; // in cm/s, fixed 10 m/s groundspeed // rescueGroundspeed sets the slope of the increase in IMU COG Gain as velocity error increases + const float groundspeedErrorRatio = fabsf(rescueState.sensor.groundSpeedCmS - rescueState.sensor.velocityToHomeCmS) / rescueGroundspeed; // 0 if groundspeed = velocity to home, // 1 if moving sideways at 10m/s but zero home velocity, // 2 if moving backwards (away from home) at 10 m/s // 4 if moving backwards (away from home) at 20 m/s - if ((rescueState.phase == RESCUE_ATTAIN_ALT) || (rescueState.phase == RESCUE_ROTATE)) { - // zero IMU CoG yaw during climb or rotate, to stop IMU error accumulation arising from drift during long climbs - rescueState.sensor.imuYawCogGain = 0; - } else { - // up to 6x increase in CoG IMU Yaw gain - rescueState.sensor.imuYawCogGain = constrainf(1.0f + (2.0f * groundspeedErrorRatio), 1.0f, 6.0f); - } - rescueState.sensor.groundspeedPitchAttenuator = 1.0f / constrainf(1.0f + groundspeedErrorRatio, 1.0f, 3.0f); // cut pitch angle by up to one third - } - rescueState.sensor.gpsDataIntervalSeconds = getGpsDataIntervalSeconds(); - // Range from 10ms (100hz) to 1000ms (1Hz). Intended to cover common GPS data rates and exclude unusual values. + const float pitchAngleFactor = (gpsRescueAngle[AI_PITCH] > 0.0f) ? gpsRescueAngle[AI_PITCH] / 1000.0f : 0.0f; + // increase IMU COG Gain with higher positive pitch angles, which arise early in a rescue when the IMU is in error + // this will be particularly useful if the IMU is error and there is little drift, the initial pitch angle will be high + // positive pitch angles should associate with a nose forward ground course, though it takes time to acquire velocity + // gpsRescueAngle angle is in degrees * 100 + // pitchAngleFactor is 1 if pitch angle is 10 degrees + // pitchAngleFactor is 2 if pitch angle is 20 degrees + + // prevent IMU disorientation arising from drift during climb, rotate or do nothing phases, where there is no pitch forward + if ((rescueState.phase == RESCUE_ATTAIN_ALT) || (rescueState.phase == RESCUE_ROTATE) || (rescueState.phase == RESCUE_DO_NOTHING)) { + rescueState.sensor.imuYawCogGain = 0.0f; + } else if (rescueState.phase == RESCUE_FLY_HOME) { + // allow stronger IMU yaw adaptation to CoG during fly home phase + // up to 6x increase in CoG IMU Yaw gain, with inputs from groundspeed error and pitch angle + rescueState.sensor.imuYawCogGain = constrainf(1.0f + pitchAngleFactor + (2.0f * groundspeedErrorRatio), 1.0f, 6.0f); + } else { + // normal IMU updating in other phases + rescueState.sensor.imuYawCogGain = 1.0f; + } + + // cut pitch angle by up to one third when groundspeed error is high + // minimises flyaway velocity, tightening turns to fix IMU error issues, but reduces ability to handle high wind + rescueState.sensor.groundspeedPitchAttenuator = 1.0f / constrainf(1.0f + groundspeedErrorRatio, 1.0f, 3.0f); + } rescueState.sensor.velocityToHomeCmS = ((prevDistanceToHomeCm - rescueState.sensor.distanceToHomeCm) / rescueState.sensor.gpsDataIntervalSeconds); // positive = towards home. First value is useless since prevDistanceToHomeCm was zero. @@ -660,34 +685,64 @@ void disarmOnImpact(void) void descend(void) { if (newGPSData) { + // consider landing area to be a circle half landing height around home, to avoid overshooting home point 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 - 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; + const float proximityToLandingArea = constrainf(distanceToLandingAreaM / rescueState.intent.descentDistanceM, 0.0f, 1.0f); + + // increase the velocity lowpass filter cutoff for more aggressive responses when descending, especially close to home + // 1.5x when starting descent, 2.5x (smoother) when almost landed + rescueState.intent.velocityPidCutoffModifier = 2.5f - 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 = 0.5f * gpsRescueConfig()->maxRescueAngle * rescueState.intent.proximityToLandingArea; - // reduce roll capability when closer to home, none within final 2m - // limit roll angle from GPS Rescue code to half the max pitch angle; earth referenced yaw may add more roll via angle code + // if it does overshoot, allow pitch angle limit to build up to correct the overshoot once rotated + rescueState.intent.targetVelocityCmS = gpsRescueConfig()->rescueGroundspeed * proximityToLandingArea; + + // attenuate velocity iterm towards zero as we get closer to the landing area + rescueState.intent.velocityItermAttenuator = proximityToLandingArea; + + // reduce pitch angle limit if there is a significant groundspeed error - eg on overshooting home rescueState.intent.pitchAngleLimitDeg = gpsRescueConfig()->maxRescueAngle * rescueState.sensor.groundspeedPitchAttenuator; + + // limit roll angle to half the allowed pitch angle and attenuate when closer to home + // keep some roll when at the landing circle distance to avoid endless circling + const float proximityToHome = constrainf(rescueState.sensor.distanceToHomeM / rescueState.intent.descentDistanceM, 0.0f, 1.0f); + rescueState.intent.rollAngleLimitDeg = 0.5f * rescueState.intent.pitchAngleLimitDeg * proximityToHome; } - rescueState.intent.yawAttenuator = 1.0f; // just in case entered descend phase before yaw authority was complete + // ensure we have full yaw authority in case we entered descent mode without enough time in fly home to acquire it gracefully + rescueState.intent.yawAttenuator = 1.0f; - // configure altitude step for descent, considering interval between altitude readings - rescueState.intent.altitudeStep = -1.0f * rescueState.sensor.altitudeDataIntervalSeconds * gpsRescueConfig()->descendRate; + // set the altitude step, considering the interval between altitude readings and the descent rate + float altitudeStep = rescueState.sensor.altitudeDataIntervalSeconds * gpsRescueConfig()->descendRate; - // descend more slowly if return altitude is less than 20m - const float descentAttenuator = rescueState.intent.returnAltitudeCm / 2000.0f; - if (descentAttenuator < 1.0f) { - rescueState.intent.altitudeStep *= descentAttenuator; - } - // descend more quickly from higher altitude - rescueState.intent.descentRateModifier = constrainf(rescueState.intent.targetAltitudeCm / 5000.0f, 0.0f, 1.0f); - rescueState.intent.targetAltitudeCm += rescueState.intent.altitudeStep * (1.0f + (2.0f * rescueState.intent.descentRateModifier)); - // increase descent rate to max of 3x default above 50m, 2x above 25m, 1.2 at 5m, default by ground level. + // descend more slowly if the return home altitude was less than 20m + const float descentRateAttenuator = constrainf (rescueState.intent.returnAltitudeCm / 2000.0f, 0.25f, 1.0f); + altitudeStep *= descentRateAttenuator; + // slowest descent rate will be 1/4 of normal when we start descending at or below 5m above take-off point. + // otherwise a rescue initiated very low and close may not get all the way home + + // descend faster while the quad is at higher altitudes + const float descentRateMultiplier = constrainf(rescueState.intent.targetAltitudeCm / 5000.0f, 0.0f, 1.0f); + altitudeStep *= 1.0f + (2.0f * descentRateMultiplier); + // maximum descent rate increase is 3x default above 50m, 2x above 25m, 1.2x at 5m, default by ground level + + // also increase throttle D up to 2x in the descent phase when altitude descent rate is faster, for better control + rescueState.intent.throttleDMultiplier = 1.0f + descentRateMultiplier; + + rescueState.intent.targetAltitudeCm -= altitudeStep; +} + +void initialiseRescueValues (void) +{ + rescueState.intent.secondsFailing = 0; // reset the sanity check timer + rescueState.intent.yawAttenuator = 0.0f; // no yaw in the climb + rescueState.intent.targetVelocityCmS = rescueState.sensor.velocityToHomeCmS; // avoid snap from D at the start + rescueState.intent.rollAngleLimitDeg = 0.0f; // no roll until flying home + rescueState.intent.throttleDMultiplier = 1.0f; + rescueState.intent.velocityPidCutoffModifier = 1.0f; // normal velocity lowpass filter cutoff + rescueState.intent.pitchAngleLimitDeg = 0.0f; // force pitch adjustment to zero - level mode will level out + rescueState.intent.velocityItermAttenuator = 0.0f; // multiply velocity iTerm by zero + rescueState.intent.velocityItermRelax = 0.0f; // don't accumulate any } void gpsRescueUpdate(void) @@ -724,36 +779,25 @@ void gpsRescueUpdate(void) 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) { + // will result in a disarm via the sanity check system, or float around if switch induced + } else { 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 + // attempted initiation within minimum rescue distance requires us to fly out to at least that distance + if (rescueState.sensor.distanceToHomeM < gpsRescueConfig()->minRescueDth) { + // climb above current height by buffer height, to at least 10m altitude + rescueState.intent.returnAltitudeCm = fmaxf(1000.0f, rescueState.sensor.currentAltitudeCm + (gpsRescueConfig()->rescueAltitudeBufferM * 100.0f)); + // note that the pitch controller will pitch forward to fly out to minRescueDth + // set the descent distance to half the minimum rescue distance. which we should have moved out to in the climb phase + rescueState.intent.descentDistanceM = 0.5f * gpsRescueConfig()->minRescueDth; + } + // otherwise behave as for a normal rescue + initialiseRescueValues (); + initialAltitudeLow = (rescueState.sensor.currentAltitudeCm < rescueState.intent.returnAltitudeCm); + rescueState.phase = RESCUE_ATTAIN_ALT; } - } else { - rescueState.phase = RESCUE_ATTAIN_ALT; - rescueState.intent.secondsFailing = 0; // reset the sanity check timer for the climb - initialAltitudeLow = (rescueState.sensor.currentAltitudeCm < rescueState.intent.returnAltitudeCm); - rescueState.intent.yawAttenuator = 0.0f; - 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; @@ -761,20 +805,21 @@ 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 - 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) { + if (initialAltitudeLow == (rescueState.sensor.currentAltitudeCm < rescueState.intent.returnAltitudeCm)) { // we started low, and still are low; also true if we started high, and still are too high - rescueState.intent.targetAltitudeCm += rescueState.intent.altitudeStep; + const float altitudeStep = ((initialAltitudeLow) ? gpsRescueConfig()->ascendRate : -1.0f * gpsRescueConfig()->descendRate) * rescueState.sensor.gpsRescueTaskIntervalSeconds; + rescueState.intent.targetAltitudeCm += altitudeStep; } else { // 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; + // if initiated too close, do not rotate or do anything else until sufficiently far away that a 'normal' rescue can happen + if (rescueState.sensor.distanceToHomeM > gpsRescueConfig()->minRescueDth) { + rescueState.phase = RESCUE_ROTATE; + } } + // give velocity P and I no error that otherwise could be present due to velocity drift at the start of the rescue 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: @@ -786,7 +831,7 @@ void gpsRescueUpdate(void) rescueState.intent.pitchAngleLimitDeg = gpsRescueConfig()->maxRescueAngle * rescueState.sensor.groundspeedPitchAttenuator; rescueState.phase = RESCUE_FLY_HOME; // enter fly home phase rescueState.intent.secondsFailing = 0; // reset sanity timer for flight home - rescueState.intent.proximityToLandingArea = 1.0f; // velocity iTerm activated, initialise proximity for descent phase at 1.0 + rescueState.intent.velocityItermRelax = 1.0f; // velocity iTerm activated } initialVelocityLow = rescueState.sensor.velocityToHomeCmS < gpsRescueConfig()->rescueGroundspeed; // used to set direction of velocity target change rescueState.intent.targetVelocityCmS = rescueState.sensor.velocityToHomeCmS; @@ -807,20 +852,18 @@ void gpsRescueUpdate(void) 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.velocityItermRelax += 0.5f * rescueState.sensor.gpsRescueTaskIntervalSeconds * (1.0f - rescueState.intent.velocityItermRelax); + // there is always a lot of lag at the start, this gradual start avoids excess iTerm accumulation 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 - // limit roll angle from GPS Rescue code to half the max pitch angle; earth referenced yaw may add more roll via angle code + // higher velocity filter cutoff for initial few seconds to improve accuracy; can be smoother later if (newGPSData) { // cut back on allowed angle if there is a high groundspeed error rescueState.intent.pitchAngleLimitDeg = gpsRescueConfig()->maxRescueAngle * rescueState.sensor.groundspeedPitchAttenuator; + // introduce roll slowly and limit to half the max pitch angle; earth referenced yaw may add more roll via angle code + rescueState.intent.rollAngleLimitDeg = 0.5f * rescueState.intent.pitchAngleLimitDeg * rescueState.intent.velocityItermRelax; if (rescueState.sensor.distanceToHomeM <= rescueState.intent.descentDistanceM) { rescueState.phase = RESCUE_DESCENT; rescueState.intent.secondsFailing = 0; // reset sanity timer for descent diff --git a/src/main/pg/gps_rescue.c b/src/main/pg/gps_rescue.c index f7893135ce..0bfeba3fa0 100644 --- a/src/main/pg/gps_rescue.c +++ b/src/main/pg/gps_rescue.c @@ -33,7 +33,7 @@ PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCU PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, - .minRescueDth = 30, + .minRescueDth = 15, .altitudeMode = GPS_RESCUE_ALT_MODE_MAX, .rescueAltitudeBufferM = 10, .ascendRate = 750, // cm/s, for altitude corrections on ascent