src/main/flight/gps_rescue.c

   1 /*
   2  * This file is part of Betaflight.
   3  *
   4  * Betaflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Betaflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Betaflight. If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdint.h>
  19 #include <math.h>
  20
  21 #include "platform.h"
  22
  23 #ifdef USE_GPS_RESCUE
  24
  25 #include "build/debug.h"
  26
  27 #include "common/axis.h"
  28 #include "common/maths.h"
  29 #include "common/utils.h"
  30
  31 #include "drivers/time.h"
  32
  33 #include "io/gps.h"
  34
  35 #include "config/config.h"
  36 #include "fc/core.h"
  37 #include "fc/rc_controls.h"
  38 #include "fc/rc_modes.h"
  39 #include "fc/runtime_config.h"
  40
  41 #include "flight/failsafe.h"
  42 #include "flight/imu.h"
  43 #include "flight/pid.h"
  44 #include "flight/position.h"
  45
  46 #include "pg/pg.h"
  47 #include "pg/pg_ids.h"
  48
  49 #include "rx/rx.h"
  50
  51 #include "sensors/acceleration.h"
  52
  53 #include "gps_rescue.h"
  54
  55 typedef enum {
  56     RESCUE_SANITY_OFF = 0,
  57     RESCUE_SANITY_ON,
  58     RESCUE_SANITY_FS_ONLY
  59 } gpsRescueSanity_e;
  60
  61 typedef enum {
  62     RESCUE_IDLE,
  63     RESCUE_INITIALIZE,
  64     RESCUE_ATTAIN_ALT,
  65     RESCUE_CROSSTRACK,
  66     RESCUE_LANDING_APPROACH,
  67     RESCUE_LANDING,
  68     RESCUE_ABORT,
  69     RESCUE_COMPLETE
  70 } rescuePhase_e;
  71
  72 typedef enum {
  73     RESCUE_HEALTHY,
  74     RESCUE_FLYAWAY,
  75     RESCUE_GPSLOST,
  76     RESCUE_LOWSATS,
  77     RESCUE_CRASH_FLIP_DETECTED,
  78     RESCUE_STALLED,
  79     RESCUE_TOO_CLOSE
  80 } rescueFailureState_e;
  81
  82 typedef struct {
  83     int32_t targetAltitudeCm;
  84     int32_t targetGroundspeed;
  85     uint8_t minAngleDeg;
  86     uint8_t maxAngleDeg;
  87     bool crosstrack;
  88 } rescueIntent_s;
  89
  90 typedef struct {
  91     int32_t maxAltitudeCm;
  92     int32_t currentAltitudeCm;
  93     uint16_t distanceToHomeM;
  94     uint16_t maxDistanceToHomeM;
  95     int16_t directionToHome;
  96     uint16_t groundSpeed;
  97     uint8_t numSat;
  98     float zVelocity; // Up/down movement in cm/s
  99     float zVelocityAvg; // Up/down average in cm/s
 100     float accMagnitude;
 101     float accMagnitudeAvg;
 102     bool healthy;
 103 } rescueSensorData_s;
 104
 105 typedef struct {
 106     bool bumpDetection;
 107     bool convergenceDetection;
 108 } rescueSanityFlags;
 109
 110 typedef struct {
 111     rescuePhase_e phase;
 112     rescueFailureState_e failure;
 113     rescueSensorData_s sensor;
 114     rescueIntent_s intent;
 115     bool isFailsafe;
 116     bool isAvailable;
 117 } rescueState_s;
 118
 119 typedef enum {
 120     MAX_ALT,
 121     FIXED_ALT,
 122     CURRENT_ALT
 123 } altitudeMode_e;
 124
 125 typedef struct {
 126     float Kp;
 127     float Ki;
 128     float Kd;
 129 } throttle_s;
 130
 131 #define GPS_RESCUE_MAX_YAW_RATE         180 // deg/sec max yaw rate
 132 #define GPS_RESCUE_RATE_SCALE_DEGREES    45 // Scale the commanded yaw rate when the error is less then this angle
 133 #define GPS_RESCUE_SLOWDOWN_DISTANCE_M  200 // distance from home to start decreasing speed
 134 #define GPS_RESCUE_MIN_DESCENT_DIST_M    30 // minimum descent distance allowed
 135 #define GPS_RESCUE_ZVELOCITY_THRESHOLD  300 // altitude threshold for start decreasing z velocity
 136 #define GPS_RESCUE_LANDING_ZVELOCITY     80 // descend velocity for final landing phase
 137 #define GPS_RESCUE_ITERM_WINDUP         100 // reset I term after z velocity error of 100 cm/s
 138 #define GPS_RESCUE_MAX_ITERM_ACC        250.0f //max allowed iterm value
 139 #define GPS_RESCUE_SLOWDOWN_ALT         500 // the altitude after which the quad begins to slow down the descend velocity
 140 #define GPS_RESCUE_MINIMUM_ZVELOCITY     50 // minimum speed for final landing phase
 141 #define GPS_RESCUE_ALMOST_LANDING_ALT   100 // altitude after which the quad increases ground detection sensitivity
 142
 143 #define GPS_RESCUE_THROTTLE_P_SCALE 0.0003125f // pid scaler for P term
 144 #define GPS_RESCUE_THROTTLE_I_SCALE 0.1f       // pid scaler for I term
 145 #define GPS_RESCUE_THROTTLE_D_SCALE 0.0003125f // pid scaler for D term
 146
 147 #ifdef USE_MAG
 148 #define GPS_RESCUE_USE_MAG              true
 149 #else
 150 #define GPS_RESCUE_USE_MAG              false
 151 #endif
 152
 153 PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 1);
 154
 155 PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
 156     .angle = 32,
 157     .initialAltitudeM = 50,
 158     .descentDistanceM = 200,
 159     .rescueGroundspeed = 2000,
 160     .throttleP = 150,
 161     .throttleI = 20,
 162     .throttleD = 50,
 163     .velP = 80,
 164     .velI = 20,
 165     .velD = 15,
 166     .yawP = 40,
 167     .throttleMin = 1100,
 168     .throttleMax = 1600,
 169     .throttleHover = 1280,
 170     .sanityChecks = RESCUE_SANITY_ON,
 171     .minSats = 8,
 172     .minRescueDth = 100,
 173     .allowArmingWithoutFix = false,
 174     .useMag = GPS_RESCUE_USE_MAG,
 175     .targetLandingAltitudeM = 5,
 176     .targetLandingDistanceM = 10,
 177     .altitudeMode = MAX_ALT,
 178     .ascendRate = 500,
 179     .descendRate = 150,
 180     .rescueAltitudeBufferM = 15,
 181 );
 182
 183 static uint16_t rescueThrottle;
 184 static float    rescueYaw;
 185
 186 int32_t       gpsRescueAngle[ANGLE_INDEX_COUNT] = { 0, 0 };
 187 uint16_t      hoverThrottle = 0;
 188 float         averageThrottle = 0.0;
 189 float         altitudeError = 0.0;
 190 uint32_t      throttleSamples = 0;
 191 bool          magForceDisable = false;
 192
 193 static bool newGPSData = false;
 194
 195 rescueState_s rescueState;
 196 throttle_s throttle;
 197
 198 /*
 199  If we have new GPS data, update home heading
 200  if possible and applicable.
 201 */
 202 void rescueNewGpsData(void)
 203 {
 204     newGPSData = true;
 205 }
 206
 207 static void rescueStart()
 208 {
 209     rescueState.phase = RESCUE_INITIALIZE;
 210 }
 211
 212 static void rescueStop()
 213 {
 214     rescueState.phase = RESCUE_IDLE;
 215 }
 216
 217 // Things that need to run regardless of GPS rescue mode being enabled or not
 218 static void idleTasks()
 219 {
 220     // Do not calculate any of the idle task values when we are not flying
 221     if (!ARMING_FLAG(ARMED)) {
 222         rescueState.sensor.maxAltitudeCm = 0;
 223         rescueState.sensor.maxDistanceToHomeM = 0;
 224         return;
 225     }
 226
 227     // Don't update any rescue flight statistics if we haven't applied a proper altitude offset yet
 228     if (!isAltitudeOffset()) {
 229         return;
 230     }
 231
 232     gpsRescueAngle[AI_PITCH] = 0;
 233     gpsRescueAngle[AI_ROLL] = 0;
 234
 235     // Store the max altitude we see not during RTH so we know our fly-back minimum alt
 236     rescueState.sensor.maxAltitudeCm = MAX(rescueState.sensor.currentAltitudeCm, rescueState.sensor.maxAltitudeCm);
 237     // Store the max distance to home during normal flight so we know if a flyaway is happening
 238     rescueState.sensor.maxDistanceToHomeM = MAX(rescueState.sensor.distanceToHomeM, rescueState.sensor.maxDistanceToHomeM);
 239
 240     rescueThrottle = rcCommand[THROTTLE];
 241
 242     //to do: have a default value for hoverThrottle
 243
 244     // FIXME: GPS Rescue throttle handling should take into account min_check as the
 245     // active throttle is from min_check through PWM_RANGE_MAX. Currently adjusting for this
 246     // in gpsRescueGetThrottle() but it would be better handled here.
 247
 248     const float ct = getCosTiltAngle();
 249     if (ct > 0.5 && ct < 0.96 && throttleSamples < 1E6 && rescueThrottle > 1070) { //5 to 45 degrees tilt
 250         //TO DO: only sample when acceleration is low
 251         uint16_t adjustedThrottle = 1000 + (rescueThrottle - PWM_RANGE_MIN) * ct;
 252         if (throttleSamples == 0) {
 253             averageThrottle = adjustedThrottle;
 254         } else {
 255             averageThrottle += (adjustedThrottle - averageThrottle) / (throttleSamples + 1);
 256         }
 257         hoverThrottle = lrintf(averageThrottle);
 258         throttleSamples++;
 259     }
 260 }
 261
 262 // Very similar to maghold function on betaflight/cleanflight
 263 static void setBearing(int16_t desiredHeading)
 264 {
 265     float errorAngle = (attitude.values.yaw / 10.0f) - desiredHeading;
 266
 267     // Determine the most efficient direction to rotate
 268     if (errorAngle <= -180) {
 269         errorAngle += 360;
 270     } else if (errorAngle > 180) {
 271         errorAngle -= 360;
 272     }
 273
 274     errorAngle *= -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed);
 275
 276     // Calculate a desired yaw rate based on a maximum limit beyond
 277     // an error window and then scale the requested rate down inside
 278     // the window as error approaches 0.
 279     rescueYaw = -constrainf(errorAngle / GPS_RESCUE_RATE_SCALE_DEGREES * GPS_RESCUE_MAX_YAW_RATE, -GPS_RESCUE_MAX_YAW_RATE, GPS_RESCUE_MAX_YAW_RATE);
 280 }
 281
 282 static void rescueAttainPosition()
 283 {
 284     // Speed and altitude controller internal variables
 285     static int16_t previousSpeedError = 0;
 286     static int16_t speedIntegral = 0;
 287     static int previousZVelocityError = 0;
 288     static float zVelocityIntegral = 0;
 289     static int16_t altitudeAdjustment = 0;
 290
 291     if (rescueState.phase == RESCUE_INITIALIZE) {
 292         // Initialize internal variables each time GPS Rescue is started
 293         previousSpeedError = 0;
 294         speedIntegral = 0;
 295         previousZVelocityError = 0;
 296         zVelocityIntegral = 0;
 297         altitudeAdjustment = 0;
 298     }
 299
 300     // Point to home if that is in our intent
 301     if (rescueState.intent.crosstrack) {
 302         setBearing(rescueState.sensor.directionToHome);
 303     }
 304
 305     DEBUG_SET(DEBUG_RTH, 3, rescueState.failure); //Failure can change with no new GPS Data
 306
 307     if (!newGPSData) {
 308         return;
 309     }
 310
 311     /**
 312         Speed controller
 313     */
 314     const int16_t speedError = (rescueState.intent.targetGroundspeed - rescueState.sensor.groundSpeed) / 100;
 315     const int16_t speedDerivative = speedError - previousSpeedError;
 316
 317     speedIntegral = constrain(speedIntegral + speedError, -100, 100);
 318
 319     previousSpeedError = speedError;
 320
 321     const int16_t angleAdjustment =  gpsRescueConfig()->velP * speedError + (gpsRescueConfig()->velI * speedIntegral) / 100 +  gpsRescueConfig()->velD * speedDerivative;
 322
 323     gpsRescueAngle[AI_PITCH] = constrain(gpsRescueAngle[AI_PITCH] + MIN(angleAdjustment, 80), rescueState.intent.minAngleDeg * 100, rescueState.intent.maxAngleDeg * 100);
 324
 325     const float ct = cosf(DECIDEGREES_TO_RADIANS(gpsRescueAngle[AI_PITCH] / 10));
 326
 327     /**
 328         Altitude controller
 329     */
 330     const int16_t altitudeError = rescueState.intent.targetAltitudeCm - rescueState.sensor.currentAltitudeCm;
 331
 332     // P component
 333     float scalingRate;
 334     if (ABS(altitudeError) > 0 && ABS(altitudeError) < GPS_RESCUE_ZVELOCITY_THRESHOLD) {
 335         scalingRate = (float)altitudeError / GPS_RESCUE_ZVELOCITY_THRESHOLD;
 336     } else {
 337         scalingRate = 1;
 338     }
 339
 340     int zVelocityError;
 341     if (altitudeError > 0) {
 342         zVelocityError = gpsRescueConfig()->ascendRate * scalingRate - rescueState.sensor.zVelocity;
 343     } else if (altitudeError < 0) {
 344         if (rescueState.sensor.currentAltitudeCm <= GPS_RESCUE_SLOWDOWN_ALT) {
 345             const int16_t rescueLandingDescendVel = MAX(GPS_RESCUE_LANDING_ZVELOCITY * rescueState.sensor.currentAltitudeCm / GPS_RESCUE_SLOWDOWN_ALT, GPS_RESCUE_MINIMUM_ZVELOCITY);
 346             zVelocityError = -rescueLandingDescendVel - rescueState.sensor.zVelocity;
 347         } else {
 348             zVelocityError = -gpsRescueConfig()->descendRate * scalingRate - rescueState.sensor.zVelocity;
 349         }
 350     } else {
 351         zVelocityError = 0;
 352     }
 353
 354     // I component
 355     if (ABS(zVelocityError) < GPS_RESCUE_ITERM_WINDUP) {
 356         zVelocityIntegral = constrainf(zVelocityIntegral + zVelocityError / 100.0f, -GPS_RESCUE_MAX_ITERM_ACC, GPS_RESCUE_MAX_ITERM_ACC);
 357     } else {
 358         zVelocityIntegral = 0;
 359     }
 360
 361     // D component
 362     const int zVelocityDerivative = zVelocityError - previousZVelocityError;
 363     previousZVelocityError = zVelocityError;
 364
 365     const int16_t hoverAdjustment = (hoverThrottle - 1000) / ct;
 366     altitudeAdjustment = constrain(altitudeAdjustment + (throttle.Kp * zVelocityError + throttle.Ki * zVelocityIntegral + throttle.Kd * zVelocityDerivative),
 367                                     gpsRescueConfig()->throttleMin - 1000 - hoverAdjustment, gpsRescueConfig()->throttleMax - 1000 - hoverAdjustment);
 368
 369     rescueThrottle = constrain(1000 + altitudeAdjustment + hoverAdjustment, gpsRescueConfig()->throttleMin, gpsRescueConfig()->throttleMax);
 370
 371     DEBUG_SET(DEBUG_RTH, 0, rescueThrottle);
 372     DEBUG_SET(DEBUG_RTH, 1, gpsRescueAngle[AI_PITCH]);
 373     DEBUG_SET(DEBUG_RTH, 2, altitudeAdjustment);
 374
 375     DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 0, throttle.Kp * zVelocityError);
 376     DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 1, throttle.Ki * zVelocityIntegral);
 377     DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 2, throttle.Kd * zVelocityDerivative);
 378     DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 3, rescueState.sensor.zVelocity);
 379 }
 380
 381 static void performSanityChecks()
 382 {
 383     static uint32_t previousTimeUs = 0; // Last time Stalled/LowSat was checked
 384     static int8_t secondsStalled = 0; // Stalled movement detection
 385     static uint16_t lastDistanceToHomeM = 0; // Fly Away detection
 386     static int8_t secondsFlyingAway = 0;
 387     static int8_t secondsLowSats = 0; // Minimum sat detection
 388
 389     const uint32_t currentTimeUs = micros();
 390
 391     if (rescueState.phase == RESCUE_IDLE) {
 392         rescueState.failure = RESCUE_HEALTHY;
 393         return;
 394     } else if (rescueState.phase == RESCUE_INITIALIZE) {
 395         // Initialize internal variables each time GPS Rescue is started
 396         previousTimeUs = currentTimeUs;
 397         secondsStalled = 10; // Start the count at 10 to be less forgiving at the beginning
 398         lastDistanceToHomeM = rescueState.sensor.distanceToHomeM;
 399         secondsFlyingAway = 0;
 400         secondsLowSats = 5;  // Start the count at 5 to be less forgiving at the beginning
 401         return;
 402     }
 403
 404     // Do not abort until each of these items is fully tested
 405     if (rescueState.failure != RESCUE_HEALTHY) {
 406         if (gpsRescueConfig()->sanityChecks == RESCUE_SANITY_ON
 407             || (gpsRescueConfig()->sanityChecks == RESCUE_SANITY_FS_ONLY && rescueState.isFailsafe == true)) {
 408             rescueState.phase = RESCUE_ABORT;
 409         }
 410     }
 411
 412     // Check if crash recovery mode is active, disarm if so.
 413     if (crashRecoveryModeActive()) {
 414         rescueState.failure = RESCUE_CRASH_FLIP_DETECTED;
 415     }
 416
 417     // Check if GPS comms are healthy
 418     if (!rescueState.sensor.healthy) {
 419         rescueState.failure = RESCUE_GPSLOST;
 420     }
 421
 422     //  Things that should run at a low refresh rate (such as flyaway detection, etc)
 423     //  This runs at ~1hz
 424     const uint32_t dTime = currentTimeUs - previousTimeUs;
 425     if (dTime < 1000000) { //1hz
 426         return;
 427     }
 428
 429     previousTimeUs = currentTimeUs;
 430
 431     if (rescueState.phase == RESCUE_CROSSTRACK) {
 432         secondsStalled = constrain(secondsStalled + ((rescueState.sensor.groundSpeed < 150) ? 1 : -1), 0, 20);
 433
 434         if (secondsStalled == 20) {
 435             rescueState.failure = RESCUE_STALLED;
 436         }
 437
 438         secondsFlyingAway = constrain(secondsFlyingAway + ((lastDistanceToHomeM < rescueState.sensor.distanceToHomeM) ? 1 : -1), 0, 10);
 439         lastDistanceToHomeM = rescueState.sensor.distanceToHomeM;
 440
 441         if (secondsFlyingAway == 10) {
 442 #ifdef USE_MAG
 443             //If there is a mag and has not been disabled, we have to assume is healthy and has been used in imu.c
 444             if (sensors(SENSOR_MAG) && gpsRescueConfig()->useMag && !magForceDisable) {
 445                 //Try again with mag disabled
 446                 magForceDisable = true;
 447                 secondsFlyingAway = 0;
 448             } else
 449 #endif
 450             {
 451                 rescueState.failure = RESCUE_FLYAWAY;
 452             }
 453         }
 454     }
 455
 456     secondsLowSats = constrain(secondsLowSats + ((rescueState.sensor.numSat < gpsRescueConfig()->minSats) ? 1 : -1), 0, 10);
 457
 458     if (secondsLowSats == 10) {
 459         rescueState.failure = RESCUE_LOWSATS;
 460     }
 461 }
 462
 463 static void sensorUpdate()
 464 {
 465     rescueState.sensor.currentAltitudeCm = getEstimatedAltitudeCm();
 466     rescueState.sensor.healthy = gpsIsHealthy();
 467
 468     // Calculate altitude velocity
 469     static uint32_t previousTimeUs;
 470     static int32_t previousAltitudeCm;
 471
 472     const uint32_t currentTimeUs = micros();
 473     const float dTime = currentTimeUs - previousTimeUs;
 474
 475     if (newGPSData) { // Calculate velocity at lowest common denominator
 476         rescueState.sensor.distanceToHomeM = GPS_distanceToHome;
 477         rescueState.sensor.directionToHome = GPS_directionToHome;
 478         rescueState.sensor.numSat = gpsSol.numSat;
 479         rescueState.sensor.groundSpeed = gpsSol.groundSpeed;
 480
 481         rescueState.sensor.zVelocity = (rescueState.sensor.currentAltitudeCm - previousAltitudeCm) * 1000000.0f / dTime;
 482         rescueState.sensor.zVelocityAvg = 0.8f * rescueState.sensor.zVelocityAvg + rescueState.sensor.zVelocity * 0.2f;
 483
 484         rescueState.sensor.accMagnitude = (float) sqrtf(sq(acc.accADC[Z]) + sq(acc.accADC[X]) + sq(acc.accADC[Y])) * acc.dev.acc_1G_rec;
 485         rescueState.sensor.accMagnitudeAvg = (rescueState.sensor.accMagnitudeAvg * 0.8f) + (rescueState.sensor.accMagnitude * 0.2f);
 486
 487         previousAltitudeCm = rescueState.sensor.currentAltitudeCm;
 488         previousTimeUs = currentTimeUs;
 489     }
 490 }
 491
 492 // This function checks the following conditions to determine if GPS rescue is available:
 493 // 1. sensor healthy - GPS data is being received.
 494 // 2. GPS has a valid fix.
 495 // 3. GPS number of satellites is less than the minimum configured for GPS rescue.
 496 // Note: this function does not take into account the distance from homepoint etc. (gps_rescue_min_dth) and
 497 // is also independent of the gps_rescue_sanity_checks configuration
 498 static bool checkGPSRescueIsAvailable(void)
 499 {
 500     static uint32_t previousTimeUs = 0; // Last time LowSat was checked
 501     const uint32_t currentTimeUs = micros();
 502     static int8_t secondsLowSats = 0; // Minimum sat detection
 503     static bool lowsats = false;
 504     static bool noGPSfix = false;
 505     bool result = true;
 506
 507     if (!gpsIsHealthy() || !STATE(GPS_FIX_HOME)) {
 508         return false;
 509     }
 510
 511     //  Things that should run at a low refresh rate >> ~1hz
 512     const uint32_t dTime = currentTimeUs - previousTimeUs;
 513     if (dTime < 1000000) { //1hz
 514         if (noGPSfix || lowsats) {
 515             result = false;
 516         }
 517         return result;
 518     }
 519
 520     previousTimeUs = currentTimeUs;
 521
 522     if (!STATE(GPS_FIX)) {
 523         result = false;
 524         noGPSfix = true;
 525     } else {
 526         noGPSfix = false;
 527     }
 528
 529     secondsLowSats = constrain(secondsLowSats + ((gpsSol.numSat < gpsRescueConfig()->minSats) ? 1 : -1), 0, 2);
 530     if (secondsLowSats == 2) {
 531         lowsats = true;
 532         result = false;
 533     } else {
 534         lowsats = false;
 535     }
 536
 537     return result;
 538 }
 539
 540 /*
 541     Determine what phase we are in, determine if all criteria are met to move to the next phase
 542 */
 543 void updateGPSRescueState(void)
 544 {
 545     static uint16_t newDescentDistanceM;
 546     static float_t lineSlope;
 547     static float_t lineOffsetM;
 548     static int32_t newSpeed;
 549     static int32_t newAltitude;
 550     float magnitudeTrigger;
 551
 552     if (!FLIGHT_MODE(GPS_RESCUE_MODE)) {
 553         rescueStop();
 554     } else if (FLIGHT_MODE(GPS_RESCUE_MODE) && rescueState.phase == RESCUE_IDLE) {
 555         rescueStart();
 556         rescueAttainPosition(); // Initialize
 557         performSanityChecks(); // Initialize
 558     }
 559
 560     rescueState.isFailsafe = failsafeIsActive();
 561
 562     sensorUpdate();
 563
 564     rescueState.isAvailable = checkGPSRescueIsAvailable();
 565
 566     switch (rescueState.phase) {
 567     case RESCUE_IDLE:
 568         idleTasks();
 569         break;
 570     case RESCUE_INITIALIZE:
 571         if (hoverThrottle == 0) { //no actual throttle data yet, let's use the default.
 572             hoverThrottle = gpsRescueConfig()->throttleHover;
 573         }
 574
 575         throttle.Kp = gpsRescueConfig()->throttleP * GPS_RESCUE_THROTTLE_P_SCALE;
 576         throttle.Ki = gpsRescueConfig()->throttleI * GPS_RESCUE_THROTTLE_I_SCALE;
 577         throttle.Kd = gpsRescueConfig()->throttleD * GPS_RESCUE_THROTTLE_D_SCALE;
 578
 579         if (!STATE(GPS_FIX_HOME)) {
 580             setArmingDisabled(ARMING_DISABLED_ARM_SWITCH);
 581             disarm(DISARM_REASON_GPS_RESCUE);
 582         }
 583
 584         // Minimum distance detection.
 585         if (rescueState.sensor.distanceToHomeM < gpsRescueConfig()->minRescueDth) {
 586             rescueState.failure = RESCUE_TOO_CLOSE;
 587
 588             // Never allow rescue mode to engage as a failsafe when too close.
 589             if (rescueState.isFailsafe) {
 590                 setArmingDisabled(ARMING_DISABLED_ARM_SWITCH);
 591                 disarm(DISARM_REASON_GPS_RESCUE);
 592             }
 593
 594             // When not in failsafe mode: leave it up to the sanity check setting.
 595         }
 596
 597         newSpeed = gpsRescueConfig()->rescueGroundspeed;
 598         //set new descent distance if actual distance to home is lower
 599         if (rescueState.sensor.distanceToHomeM < gpsRescueConfig()->descentDistanceM) {
 600             newDescentDistanceM = MAX(rescueState.sensor.distanceToHomeM - 5, GPS_RESCUE_MIN_DESCENT_DIST_M);
 601         } else {
 602             newDescentDistanceM = gpsRescueConfig()->descentDistanceM;
 603         }
 604
 605         switch (gpsRescueConfig()->altitudeMode) {
 606             case FIXED_ALT:
 607                 newAltitude = gpsRescueConfig()->initialAltitudeM * 100;
 608                 break;
 609             case CURRENT_ALT:
 610                 newAltitude = rescueState.sensor.currentAltitudeCm + gpsRescueConfig()->rescueAltitudeBufferM * 100;
 611                 break;
 612             case MAX_ALT:
 613             default:
 614                 newAltitude = MAX(gpsRescueConfig()->initialAltitudeM * 100, rescueState.sensor.maxAltitudeCm + gpsRescueConfig()->rescueAltitudeBufferM * 100);
 615                 break;
 616         }
 617
 618         //Calculate angular coefficient and offset for equation of line from 2 points needed for RESCUE_LANDING_APPROACH
 619         lineSlope = ((float)gpsRescueConfig()->initialAltitudeM - gpsRescueConfig()->targetLandingAltitudeM) / (newDescentDistanceM - gpsRescueConfig()->targetLandingDistanceM);
 620         lineOffsetM = gpsRescueConfig()->initialAltitudeM - lineSlope * newDescentDistanceM;
 621
 622         rescueState.phase = RESCUE_ATTAIN_ALT;
 623         FALLTHROUGH;
 624     case RESCUE_ATTAIN_ALT:
 625         // Get to a safe altitude at a low velocity ASAP
 626         if (ABS(rescueState.intent.targetAltitudeCm - rescueState.sensor.currentAltitudeCm) < 1000) {
 627             rescueState.phase = RESCUE_CROSSTRACK;
 628         }
 629
 630         rescueState.intent.targetGroundspeed = 500;
 631         rescueState.intent.targetAltitudeCm = newAltitude;
 632         rescueState.intent.crosstrack = true;
 633         rescueState.intent.minAngleDeg = 10;
 634         rescueState.intent.maxAngleDeg = 15;
 635         break;
 636     case RESCUE_CROSSTRACK:
 637         if (rescueState.sensor.distanceToHomeM <= newDescentDistanceM) {
 638             rescueState.phase = RESCUE_LANDING_APPROACH;
 639         }
 640
 641         // We can assume at this point that we are at or above our RTH height, so we need to try and point to home and tilt while maintaining alt
 642         // Is our altitude way off?  We should probably kick back to phase RESCUE_ATTAIN_ALT
 643         rescueState.intent.targetGroundspeed = gpsRescueConfig()->rescueGroundspeed;
 644         rescueState.intent.targetAltitudeCm = newAltitude;
 645         rescueState.intent.crosstrack = true;
 646         rescueState.intent.minAngleDeg = 15;
 647         rescueState.intent.maxAngleDeg = gpsRescueConfig()->angle;
 648         break;
 649     case RESCUE_LANDING_APPROACH:
 650         // We are getting close to home in the XY plane, get Z where it needs to be to move to landing phase
 651         if (rescueState.sensor.distanceToHomeM <= gpsRescueConfig()->targetLandingDistanceM && rescueState.sensor.currentAltitudeCm <= gpsRescueConfig()->targetLandingAltitudeM * 100) {
 652             rescueState.phase = RESCUE_LANDING;
 653         }
 654
 655         // Only allow new altitude and new speed to be equal or lower than the current values (to prevent parabolic movement on overshoot)
 656         const int32_t newAlt = MAX((lineSlope * rescueState.sensor.distanceToHomeM + lineOffsetM) * 100, 0);
 657
 658         // Start to decrease proportionally the quad's speed when the distance to home is less or equal than GPS_RESCUE_SLOWDOWN_DISTANCE_M
 659         if (rescueState.sensor.distanceToHomeM <= GPS_RESCUE_SLOWDOWN_DISTANCE_M) {
 660             newSpeed = gpsRescueConfig()->rescueGroundspeed * rescueState.sensor.distanceToHomeM / GPS_RESCUE_SLOWDOWN_DISTANCE_M;
 661         }
 662
 663         rescueState.intent.targetAltitudeCm = constrain(newAlt, 100, rescueState.intent.targetAltitudeCm);
 664         rescueState.intent.targetGroundspeed = constrain(newSpeed, 100, rescueState.intent.targetGroundspeed);
 665         rescueState.intent.crosstrack = true;
 666         rescueState.intent.minAngleDeg = 10;
 667         rescueState.intent.maxAngleDeg = gpsRescueConfig()->angle;
 668         break;
 669     case RESCUE_LANDING:
 670         // We have reached the XYZ envelope to be considered at "home".  We need to land gently and check our accelerometer for abnormal data.
 671         // At this point, do not let the target altitude go up anymore, so if we overshoot, we dont' move in a parabolic trajectory
 672         // If we are over 150% of average magnitude, just disarm since we're pretty much home
 673         if (rescueState.sensor.currentAltitudeCm < GPS_RESCUE_ALMOST_LANDING_ALT) {
 674             magnitudeTrigger = rescueState.sensor.accMagnitudeAvg * 1.2;
 675         } else {
 676             magnitudeTrigger = rescueState.sensor.accMagnitudeAvg * 1.5;
 677         }
 678
 679         if (rescueState.sensor.accMagnitude > magnitudeTrigger) {
 680             setArmingDisabled(ARMING_DISABLED_ARM_SWITCH);
 681             disarm(DISARM_REASON_GPS_RESCUE);
 682             rescueState.phase = RESCUE_COMPLETE;
 683         }
 684
 685         rescueState.intent.targetGroundspeed = 0;
 686         rescueState.intent.targetAltitudeCm = 0;
 687         rescueState.intent.crosstrack = true;
 688         rescueState.intent.minAngleDeg = 0;
 689         rescueState.intent.maxAngleDeg = 15;
 690         break;
 691     case RESCUE_COMPLETE:
 692         rescueStop();
 693         break;
 694     case RESCUE_ABORT:
 695         setArmingDisabled(ARMING_DISABLED_ARM_SWITCH);
 696         disarm(DISARM_REASON_GPS_RESCUE);
 697         rescueStop();
 698         break;
 699     default:
 700         break;
 701     }
 702
 703     performSanityChecks();
 704
 705     if (rescueState.phase != RESCUE_IDLE) {
 706         rescueAttainPosition();
 707     }
 708
 709     newGPSData = false;
 710 }
 711
 712 float gpsRescueGetYawRate(void)
 713 {
 714     return rescueYaw;
 715 }
 716
 717 float gpsRescueGetThrottle(void)
 718 {
 719     // Calculated a desired commanded throttle scaled from 0.0 to 1.0 for use in the mixer.
 720     // We need to compensate for min_check since the throttle value set by gps rescue
 721     // is based on the raw rcCommand value commanded by the pilot.
 722     float commandedThrottle = scaleRangef(rescueThrottle, MAX(rxConfig()->mincheck, PWM_RANGE_MIN), PWM_RANGE_MAX, 0.0f, 1.0f);
 723     commandedThrottle = constrainf(commandedThrottle, 0.0f, 1.0f);
 724
 725     return commandedThrottle;
 726 }
 727
 728 bool gpsRescueIsConfigured(void)
 729 {
 730     return failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_GPS_RESCUE || isModeActivationConditionPresent(BOXGPSRESCUE);
 731 }
 732
 733 bool gpsRescueIsAvailable(void)
 734 {
 735     return rescueState.isAvailable;
 736 }
 737
 738 bool gpsRescueIsDisabled(void)
 739 {
 740     return (!STATE(GPS_FIX_HOME));
 741 }
 742
 743 #ifdef USE_MAG
 744 bool gpsRescueDisableMag(void)
 745 {
 746     return ((!gpsRescueConfig()->useMag || magForceDisable) && (rescueState.phase >= RESCUE_INITIALIZE) && (rescueState.phase <= RESCUE_LANDING));
 747 }
 748 #endif
 749 #endif