Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-13 19:40:31 +03:00
Code Issues Wiki Activity

Improve behaviour when GPS Rescue is initiated close to home (#12715)

Browse source 
* 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
This commit is contained in:
ctzsnooze 2023-05-25 18:02:24 +10:00 • committed by GitHub
parent aad197f791
commit f69fedd1f2
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
3 changed files with 139 additions and 96 deletions
Download patch file Download diff file Expand all files Collapse all files

4
src/main/cli/settings.c
View file

@ -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) },

229
src/main/flight/gps_rescue.c
View file

@ -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

2
src/main/pg/gps_rescue.c
View file

@ -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