diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 4fc5852b3..9a28371b4 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -875,6 +875,8 @@ const clivalue_t valueTable[] = { { "gps_rescue_angle", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, angle) }, { "gps_rescue_initial_alt", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 20, 100 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, initialAltitudeM) }, { "gps_rescue_descent_dist", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 30, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, descentDistanceM) }, + { "gps_rescue_landing_alt", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 3, 10 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, targetLandingAltitudeM) }, + { "gps_rescue_landing_dist", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 5, 15 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, targetLandingDistanceM) }, { "gps_rescue_ground_speed", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 30, 3000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, rescueGroundspeed) }, { "gps_rescue_throttle_p", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleP) }, { "gps_rescue_throttle_i", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleI) }, diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c index 7ceeb161b..de0d58cea 100644 --- a/src/main/flight/gps_rescue.c +++ b/src/main/flight/gps_rescue.c @@ -116,8 +116,9 @@ typedef struct { bool isAvailable; } rescueState_s; -#define GPS_RESCUE_MAX_YAW_RATE 180 // deg/sec max yaw rate -#define GPS_RESCUE_RATE_SCALE_DEGREES 45 // Scale the commanded yaw rate when the error is less then this angle +#define GPS_RESCUE_MAX_YAW_RATE 180 // deg/sec max yaw rate +#define GPS_RESCUE_RATE_SCALE_DEGREES 45 // Scale the commanded yaw rate when the error is less then this angle +#define GPS_RESCUE_SLOWDOWN_DISTANCE_M 200 // distance from home to start decreasing speed PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 1); @@ -140,7 +141,9 @@ PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, .minSats = 8, .minRescueDth = 100, .allowArmingWithoutFix = false, - .useMag = true + .useMag = true, + .targetLandingAltitudeM = 5, + .targetLandingDistanceM = 10, ); static uint16_t rescueThrottle; @@ -470,8 +473,11 @@ static bool checkGPSRescueIsAvailable(void) */ void updateGPSRescueState(void) { - static uint16_t descentDistance; - + static uint16_t newDescentDistanceM; + static float_t lineSlope; + static float_t lineOffsetM; + static int32_t newSpeed; + if (!FLIGHT_MODE(GPS_RESCUE_MODE)) { rescueStop(); } else if (FLIGHT_MODE(GPS_RESCUE_MODE) && rescueState.phase == RESCUE_IDLE) { @@ -513,12 +519,17 @@ void updateGPSRescueState(void) // When not in failsafe mode: leave it up to the sanity check setting. } + newSpeed = gpsRescueConfig()->rescueGroundspeed; //set new descent distance if actual distance to home is lower if (rescueState.sensor.distanceToHomeM < gpsRescueConfig()->descentDistanceM) { - descentDistance = rescueState.sensor.distanceToHomeM - 5; + newDescentDistanceM = rescueState.sensor.distanceToHomeM - 5; } else { - descentDistance = gpsRescueConfig()->descentDistanceM; + newDescentDistanceM = gpsRescueConfig()->descentDistanceM; } + + //Calculate angular coefficient and offset for equation of line from 2 points needed for RESCUE_LANDING_APPROACH + lineSlope = ((float)gpsRescueConfig()->initialAltitudeM - gpsRescueConfig()->targetLandingAltitudeM) / (newDescentDistanceM - gpsRescueConfig()->targetLandingDistanceM); + lineOffsetM = gpsRescueConfig()->initialAltitudeM - lineSlope * newDescentDistanceM; rescueState.phase = RESCUE_ATTAIN_ALT; FALLTHROUGH; @@ -535,7 +546,7 @@ void updateGPSRescueState(void) rescueState.intent.maxAngleDeg = 15; break; case RESCUE_CROSSTRACK: - if (rescueState.sensor.distanceToHomeM < descentDistance) { + if (rescueState.sensor.distanceToHomeM <= newDescentDistanceM) { rescueState.phase = RESCUE_LANDING_APPROACH; } @@ -549,19 +560,23 @@ void updateGPSRescueState(void) break; case RESCUE_LANDING_APPROACH: // We are getting close to home in the XY plane, get Z where it needs to be to move to landing phase - if (rescueState.sensor.distanceToHomeM < 10 && rescueState.sensor.currentAltitudeCm <= 1000) { + if (rescueState.sensor.distanceToHomeM <= gpsRescueConfig()->targetLandingDistanceM && rescueState.sensor.currentAltitudeCm <= gpsRescueConfig()->targetLandingAltitudeM * 100) { rescueState.phase = RESCUE_LANDING; } // Only allow new altitude and new speed to be equal or lower than the current values (to prevent parabolic movement on overshoot) - const int32_t newAlt = gpsRescueConfig()->initialAltitudeM * 100 * rescueState.sensor.distanceToHomeM / descentDistance; - const int32_t newSpeed = gpsRescueConfig()->rescueGroundspeed * rescueState.sensor.distanceToHomeM / descentDistance; + const int32_t newAlt = (lineSlope * rescueState.sensor.distanceToHomeM + lineOffsetM) * 100; + + // Start to decrease proportionally the quad's speed when the distance to home is less or equal than GPS_RESCUE_SLOWDOWN_DISTANCE_M + if (rescueState.sensor.distanceToHomeM <= GPS_RESCUE_SLOWDOWN_DISTANCE_M) { + newSpeed = gpsRescueConfig()->rescueGroundspeed * rescueState.sensor.distanceToHomeM / GPS_RESCUE_SLOWDOWN_DISTANCE_M; + } rescueState.intent.targetAltitudeCm = constrain(newAlt, 100, rescueState.intent.targetAltitudeCm); rescueState.intent.targetGroundspeed = constrain(newSpeed, 100, rescueState.intent.targetGroundspeed); rescueState.intent.crosstrack = true; rescueState.intent.minAngleDeg = 10; - rescueState.intent.maxAngleDeg = 20; + rescueState.intent.maxAngleDeg = gpsRescueConfig()->angle; break; case RESCUE_LANDING: // We have reached the XYZ envelope to be considered at "home". We need to land gently and check our accelerometer for abnormal data. diff --git a/src/main/flight/gps_rescue.h b/src/main/flight/gps_rescue.h index 4f03a0ab7..bb0328488 100644 --- a/src/main/flight/gps_rescue.h +++ b/src/main/flight/gps_rescue.h @@ -25,7 +25,7 @@ typedef struct gpsRescue_s { uint16_t angle; //degrees uint16_t initialAltitudeM; //meters uint16_t descentDistanceM; //meters - uint16_t rescueGroundspeed; // centimeters per second + uint16_t rescueGroundspeed; //centimeters per second uint16_t throttleP, throttleI, throttleD; uint16_t yawP; uint16_t throttleMin; @@ -37,6 +37,8 @@ typedef struct gpsRescue_s { uint8_t sanityChecks; uint8_t allowArmingWithoutFix; uint8_t useMag; + uint16_t targetLandingAltitudeM; //meters + uint16_t targetLandingDistanceM; //meters } gpsRescueConfig_t; PG_DECLARE(gpsRescueConfig_t, gpsRescueConfig);