Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 05:15:25 +03:00
Code Issues Wiki Activity

Merge pull request #7838 from IllusionFpv/gps-rescue-improve-landing-approach

Browse source 
Improve target landing altitude in gps-rescue
This commit is contained in:
Michael Keller 2019-04-15 00:29:15 +12:00 • committed by GitHub
commit eb7e6c0b39
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
3 changed files with 32 additions and 13 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -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_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_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_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_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_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) }, { "gps_rescue_throttle_i", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleI) },

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

@ -116,8 +116,9 @@ typedef struct {
bool isAvailable; bool isAvailable;
} rescueState_s; } rescueState_s;
#define GPS_RESCUE_MAX_YAW_RATE 180 // deg/sec max yaw rate #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_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); PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 1);
@ -140,7 +141,9 @@ PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
.minSats = 8, .minSats = 8,
.minRescueDth = 100, .minRescueDth = 100,
.allowArmingWithoutFix = false, .allowArmingWithoutFix = false,
.useMag = true .useMag = true,
.targetLandingAltitudeM = 5,
.targetLandingDistanceM = 10,
); );
static uint16_t rescueThrottle; static uint16_t rescueThrottle;
@ -470,8 +473,11 @@ static bool checkGPSRescueIsAvailable(void)
*/ */
void updateGPSRescueState(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)) { if (!FLIGHT_MODE(GPS_RESCUE_MODE)) {
rescueStop(); rescueStop();
} else if (FLIGHT_MODE(GPS_RESCUE_MODE) && rescueState.phase == RESCUE_IDLE) { } 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. // 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 //set new descent distance if actual distance to home is lower
if (rescueState.sensor.distanceToHomeM < gpsRescueConfig()->descentDistanceM) { if (rescueState.sensor.distanceToHomeM < gpsRescueConfig()->descentDistanceM) {
descentDistance = rescueState.sensor.distanceToHomeM - 5; newDescentDistanceM = rescueState.sensor.distanceToHomeM - 5;
} else { } 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; rescueState.phase = RESCUE_ATTAIN_ALT;
FALLTHROUGH; FALLTHROUGH;
@ -535,7 +546,7 @@ void updateGPSRescueState(void)
rescueState.intent.maxAngleDeg = 15; rescueState.intent.maxAngleDeg = 15;
break; break;
case RESCUE_CROSSTRACK: case RESCUE_CROSSTRACK:
if (rescueState.sensor.distanceToHomeM < descentDistance) { if (rescueState.sensor.distanceToHomeM <= newDescentDistanceM) {
rescueState.phase = RESCUE_LANDING_APPROACH; rescueState.phase = RESCUE_LANDING_APPROACH;
} }
@ -549,19 +560,23 @@ void updateGPSRescueState(void)
break; break;
case RESCUE_LANDING_APPROACH: 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 // 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; 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) // 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 newAlt = (lineSlope * rescueState.sensor.distanceToHomeM + lineOffsetM) * 100;
const int32_t newSpeed = gpsRescueConfig()->rescueGroundspeed * rescueState.sensor.distanceToHomeM / descentDistance;
// 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.targetAltitudeCm = constrain(newAlt, 100, rescueState.intent.targetAltitudeCm);
rescueState.intent.targetGroundspeed = constrain(newSpeed, 100, rescueState.intent.targetGroundspeed); rescueState.intent.targetGroundspeed = constrain(newSpeed, 100, rescueState.intent.targetGroundspeed);
rescueState.intent.crosstrack = true; rescueState.intent.crosstrack = true;
rescueState.intent.minAngleDeg = 10; rescueState.intent.minAngleDeg = 10;
rescueState.intent.maxAngleDeg = 20; rescueState.intent.maxAngleDeg = gpsRescueConfig()->angle;
break; break;
case RESCUE_LANDING: 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. // We have reached the XYZ envelope to be considered at "home". We need to land gently and check our accelerometer for abnormal data.

4
src/main/flight/gps_rescue.h
View file

@ -25,7 +25,7 @@ typedef struct gpsRescue_s {
uint16_t angle; //degrees uint16_t angle; //degrees
uint16_t initialAltitudeM; //meters uint16_t initialAltitudeM; //meters
uint16_t descentDistanceM; //meters uint16_t descentDistanceM; //meters
uint16_t rescueGroundspeed; // centimeters per second uint16_t rescueGroundspeed; //centimeters per second
uint16_t throttleP, throttleI, throttleD; uint16_t throttleP, throttleI, throttleD;
uint16_t yawP; uint16_t yawP;
uint16_t throttleMin; uint16_t throttleMin;
@ -37,6 +37,8 @@ typedef struct gpsRescue_s {
uint8_t sanityChecks; uint8_t sanityChecks;
uint8_t allowArmingWithoutFix; uint8_t allowArmingWithoutFix;
uint8_t useMag; uint8_t useMag;
uint16_t targetLandingAltitudeM; //meters
uint16_t targetLandingDistanceM; //meters
} gpsRescueConfig_t; } gpsRescueConfig_t;
PG_DECLARE(gpsRescueConfig_t, gpsRescueConfig); PG_DECLARE(gpsRescueConfig_t, gpsRescueConfig);