From a6507c02d96f05f40ebf4318a827a126ec31be34 Mon Sep 17 00:00:00 2001 From: ctzsnooze Date: Fri, 22 Jul 2022 11:12:21 +1000 Subject: [PATCH] GPS Rescue refactoring and landing improvements - run Ublox GPS units at 10Hz - Bugfix for level mode offset after a rescue (force gpsRescueAngle[] to zero while idle) - Bugfix for entering landing mode too high, (ignore landing distance, enter landing mode only on height criteria) - Remove landing distance. - Default GPS mode to UBlox, not NMEA - refactor idle tasks - don't keep setting targetVelocityCmS to current velocity since it will be set to zero when rotating anyway - remove unused velocityToHomeCmS - remove unused max distance to home value - log current altitude at 100hz in throttle_pid debug - share the sanity timer code - fix bug where yaw error should have been absolute - remove unused code - refactor rescue phases so intent values are not repeatedly set - refactor the rescue modes - fix bug that could fail to set velocity target on fly_home - refactor to simplify pitch limits - refactor to MAX and constrainf - set 2m descent region to improve landing - fade roll and pitch to zero at 2m from home --- src/main/blackbox/blackbox.c | 1 - src/main/cli/settings.c | 1 - src/main/cms/cms_menu_gps_rescue.c | 22 ++- src/main/fc/parameter_names.h | 1 - src/main/flight/gps_rescue.c | 243 ++++++++++++----------------- src/main/flight/gps_rescue.h | 3 +- src/main/io/gps.c | 4 +- 7 files changed, 116 insertions(+), 159 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 00bdbe4a21..91689e1119 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1525,7 +1525,6 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_INITIAL_ALT, "%d", gpsRescueConfig()->initialAltitudeM) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_DESCENT_DIST, "%d", gpsRescueConfig()->descentDistanceM) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_LANDING_ALT, "%d", gpsRescueConfig()->targetLandingAltitudeM) - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_LANDING_DIST, "%d", gpsRescueConfig()->targetLandingDistanceM) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_GROUND_SPEED, "%d", gpsRescueConfig()->rescueGroundspeed) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_P, "%d", gpsRescueConfig()->throttleP) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_I, "%d", gpsRescueConfig()->throttleI) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index ef6a7b43f6..c50d79ce6d 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1020,7 +1020,6 @@ const clivalue_t valueTable[] = { { PARAM_NAME_GPS_RESCUE_INITIAL_ALT, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 2, 100 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, initialAltitudeM) }, { 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_LANDING_ALT, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 3, 15 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, targetLandingAltitudeM) }, - { PARAM_NAME_GPS_RESCUE_LANDING_DIST, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 3, 15 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, targetLandingDistanceM) }, { PARAM_NAME_GPS_RESCUE_GROUND_SPEED, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 250, 3000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, rescueGroundspeed) }, { PARAM_NAME_GPS_RESCUE_THROTTLE_P, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleP) }, { PARAM_NAME_GPS_RESCUE_THROTTLE_I, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleI) }, diff --git a/src/main/cms/cms_menu_gps_rescue.c b/src/main/cms/cms_menu_gps_rescue.c index bcce51f1b1..195a2aa8da 100644 --- a/src/main/cms/cms_menu_gps_rescue.c +++ b/src/main/cms/cms_menu_gps_rescue.c @@ -53,8 +53,7 @@ static uint8_t gpsRescueConfig_allowArmingWithoutFix; static uint16_t gpsRescueConfig_throttleP, gpsRescueConfig_throttleI, gpsRescueConfig_throttleD; static uint16_t gpsRescueConfig_velP, gpsRescueConfig_velI, gpsRescueConfig_velD; static uint16_t gpsRescueConfig_yawP; -static uint16_t gpsRescueConfig_targetLandingAltitudeM; -static uint16_t gpsRescueConfig_targetLandingDistanceM; +static uint8_t gpsRescueConfig_targetLandingAltitudeM; static uint8_t gpsRescueConfig_altitudeMode; static uint16_t gpsRescueConfig_ascendRate; static uint16_t gpsRescueConfig_descendRate; @@ -137,7 +136,6 @@ static const void *cmsx_menuGpsRescueOnEnter(displayPort_t *pDisp) gpsRescueConfig_minSats = gpsRescueConfig()->minSats; gpsRescueConfig_minRescueDth = gpsRescueConfig()->minRescueDth; gpsRescueConfig_allowArmingWithoutFix = gpsRescueConfig()->allowArmingWithoutFix; - gpsRescueConfig_targetLandingDistanceM = gpsRescueConfig()->targetLandingDistanceM; gpsRescueConfig_targetLandingAltitudeM = gpsRescueConfig()->targetLandingAltitudeM; gpsRescueConfig_altitudeMode = gpsRescueConfig()->altitudeMode; gpsRescueConfig_ascendRate = gpsRescueConfig()->ascendRate; @@ -161,7 +159,6 @@ static const void *cmsx_menuGpsRescueOnExit(displayPort_t *pDisp, const OSD_Entr gpsRescueConfigMutable()->minSats = gpsRescueConfig_minSats; gpsRescueConfigMutable()->minRescueDth = gpsRescueConfig_minRescueDth; gpsRescueConfigMutable()->allowArmingWithoutFix = gpsRescueConfig_allowArmingWithoutFix; - gpsRescueConfigMutable()->targetLandingDistanceM = gpsRescueConfig_targetLandingDistanceM; gpsRescueConfigMutable()->targetLandingAltitudeM = gpsRescueConfig_targetLandingAltitudeM; gpsRescueConfigMutable()->altitudeMode = gpsRescueConfig_altitudeMode; gpsRescueConfigMutable()->ascendRate = gpsRescueConfig_ascendRate; @@ -174,19 +171,18 @@ const OSD_Entry cmsx_menuGpsRescueEntries[] = { {"--- GPS RESCUE ---", OME_Label, NULL, NULL}, - { "ANGLE", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_angle, 0, 200 ,1 } }, - { "MIN DIST HOME M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_minRescueDth, 25, 1000 ,1 } }, - { "INITAL ALT M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_initialAltitudeM, 5, 100, 1 } }, + { "ANGLE", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_angle, 0, 60, 1 } }, + { "MIN DIST HOME M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_minRescueDth, 20, 1000, 1 } }, + { "INITAL ALT M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_initialAltitudeM, 2, 100, 1 } }, { "ALTITUDE MODE" , OME_TAB | REBOOT_REQUIRED, NULL, &(OSD_TAB_t) { &gpsRescueConfig_altitudeMode, 2, lookupTableRescueAltitudeMode} }, - { "DESCENT DIST M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_descentDistanceM, 15, 500, 1 } }, - { "LANDING ALT M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_targetLandingAltitudeM, 3, 10, 1 } }, - { "LANDING DIST M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_targetLandingDistanceM, 3, 15, 1 } }, - { "GROUND SPEED CM/S", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_rescueGroundspeed, 30, 3000, 1 } }, + { "DESCENT DIST M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_descentDistanceM, 10, 500, 1 } }, + { "LANDING ALT M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_targetLandingAltitudeM, 3, 15, 1 } }, + { "GROUND SPEED CM/S", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_rescueGroundspeed, 250, 3000, 1 } }, { "THROTTLE MIN", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_throttleMin, 1000, 2000, 1 } }, { "THROTTLE MAX", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_throttleMax, 1000, 2000, 1 } }, { "THROTTLE HOV", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_throttleHover, 1000, 2000, 1 } }, - { "ASCEND RATE", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_ascendRate, 100, 2500, 1 } }, - { "DESCEND RATE", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_descendRate, 50, 500, 1 } }, + { "ASCEND RATE", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_ascendRate, 50, 2500, 1 } }, + { "DESCEND RATE", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_descendRate, 25, 500, 1 } }, { "ARM WITHOUT FIX", OME_Bool | REBOOT_REQUIRED, NULL, &gpsRescueConfig_allowArmingWithoutFix }, { "MIN SATELITES", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_minSats, 5, 50, 1 } }, { "GPS RESCUE PID", OME_Submenu, cmsMenuChange, &cms_menuGpsRescuePid}, diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h index 4ae140b6bd..b90ab960b3 100644 --- a/src/main/fc/parameter_names.h +++ b/src/main/fc/parameter_names.h @@ -135,7 +135,6 @@ #define PARAM_NAME_GPS_RESCUE_INITIAL_ALT "gps_rescue_initial_alt" #define PARAM_NAME_GPS_RESCUE_DESCENT_DIST "gps_rescue_descent_dist" #define PARAM_NAME_GPS_RESCUE_LANDING_ALT "gps_rescue_landing_alt" -#define PARAM_NAME_GPS_RESCUE_LANDING_DIST "gps_rescue_landing_dist" #define PARAM_NAME_GPS_RESCUE_GROUND_SPEED "gps_rescue_ground_speed" #define PARAM_NAME_GPS_RESCUE_THROTTLE_P "gps_rescue_throttle_p" #define PARAM_NAME_GPS_RESCUE_THROTTLE_I "gps_rescue_throttle_i" diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c index 0145c11a98..c2caebfeaf 100644 --- a/src/main/flight/gps_rescue.c +++ b/src/main/flight/gps_rescue.c @@ -86,11 +86,11 @@ typedef struct { float returnAltitudeCm; float targetAltitudeCm; uint16_t targetVelocityCmS; - int8_t minAnglePitchDeg; // can be negative - uint8_t maxAnglePitchDeg; // must be positive - int8_t maxAngleRollDeg; // only one value since must always have a negative to positive range + uint8_t pitchAngleLimitDeg; + int8_t rollAngleLimitDeg; // must have a negative to positive range bool updateYaw; float descentDistanceM; + int8_t secondsFailing; } rescueIntent_s; typedef struct { @@ -98,7 +98,6 @@ typedef struct { int32_t currentAltitudeCm; float distanceToHomeCm; float distanceToHomeM; - uint16_t maxDistanceToHomeM; uint16_t groundSpeedCmS; //cm/s int16_t directionToHome; uint8_t numSat; @@ -111,13 +110,9 @@ typedef struct { float descendStepCm; float maxPitchStep; float filterK; + float absErrorAngle; } rescueSensorData_s; -typedef struct { - bool bumpDetection; - bool convergenceDetection; -} rescueSanityFlags; - typedef struct { rescuePhase_e phase; rescueFailureState_e failure; @@ -144,7 +139,7 @@ typedef enum { #define GPS_RESCUE_USE_MAG false #endif -PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 1); +PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 2); PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, .angle = 32, @@ -167,7 +162,6 @@ PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, .allowArmingWithoutFix = false, .useMag = GPS_RESCUE_USE_MAG, .targetLandingAltitudeM = 5, - .targetLandingDistanceM = 5, .altitudeMode = MAX_ALT, .ascendRate = 500, // cm/s, for altitude corrections on ascent .descendRate = 125, // cm/s, for descent and landing phase, or negative ascent @@ -177,7 +171,6 @@ PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, static float rescueThrottle; static float rescueYaw; -float velocityToHomeCmS; float gpsRescueAngle[ANGLE_INDEX_COUNT] = { 0, 0 }; bool magForceDisable = false; static bool newGPSData = false; @@ -202,28 +195,27 @@ static void rescueStop() rescueState.phase = RESCUE_IDLE; } -// Things that need to run regardless of GPS rescue mode being enabled or not (?? huh ?? ct) +// Things that need to run when GPS Rescue is enabled, and while armed, but while there is no Rescue in place static void idleTasks() { - // Do not calculate any of the idle task values when we are not flying + // Don't calculate these values while disarmed if (!ARMING_FLAG(ARMED)) { rescueState.sensor.maxAltitudeCm = 0; - rescueState.sensor.maxDistanceToHomeM = 0; return; } - // Don't update any rescue flight statistics if we haven't applied a proper altitude offset yet + // Don't update any altitude related stuff if we haven't applied a proper altitude offset if (!isAltitudeOffset()) { return; } // Store the max altitude we see not during RTH so we know our fly-back minimum alt rescueState.sensor.maxAltitudeCm = MAX(rescueState.sensor.currentAltitudeCm, rescueState.sensor.maxAltitudeCm); - // Store the max distance to home during normal flight so we know if a flyaway is happening - rescueState.sensor.maxDistanceToHomeM = MAX(rescueState.sensor.maxDistanceToHomeM, rescueState.sensor.distanceToHomeCm / 100.0f); - // Keep the descent distance and intended altitude up to date with latest GPS values, to be available immediately when needed if (newGPSData) { + // set the target altitude and velocity 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); const float initialAltitudeCm = gpsRescueConfig()->initialAltitudeM * 100.0f; const float rescueAltitudeBufferCm = gpsRescueConfig()->rescueAltitudeBufferM * 100.0f; @@ -239,14 +231,7 @@ static void idleTasks() rescueState.intent.returnAltitudeCm = rescueState.sensor.maxAltitudeCm + rescueAltitudeBufferCm; break; } - - // set the target altitude and velocity to current values, so there will be no D kick on first run - rescueState.intent.targetAltitudeCm = rescueState.sensor.currentAltitudeCm; - rescueState.intent.targetVelocityCmS = rescueState.sensor.velocityToHomeCmS; - // set the landingInitiationAltitude for sanity check of Landing Mode. } - - rescueThrottle = rcCommand[THROTTLE]; // value to be returned when idle = normal throttle TODO ?? is this needed ?? } static void rescueAttainPosition() @@ -263,9 +248,16 @@ static void rescueAttainPosition() static float previousThrottleD2 = 0.0f; // for additional D first order smoothing static int16_t throttleAdjustment = 0; - if (rescueState.phase == RESCUE_INITIALIZE) { + switch (rescueState.phase) { + case RESCUE_IDLE: + // values to be returned when no rescue is active + gpsRescueAngle[AI_PITCH] = 0.0f; + gpsRescueAngle[AI_ROLL] = 0.0f; + rescueThrottle = rcCommand[THROTTLE]; + return; + case RESCUE_INITIALIZE: // Initialize internal variables each time GPS Rescue is started - // Note valid sensor data can't be used here. Use idleTasks() for those kind of values + // Note that sensor values can't be initialised here. Use idleTasks() to initialise them. previousVelocityError = 0.0f; velocityI = 0.0f; previousVelocityD = 0.0f; @@ -276,18 +268,15 @@ static void rescueAttainPosition() previousThrottleDVal = 0.0f; previousThrottleD2 = 0.0f; throttleAdjustment = 0; - velocityToHomeCmS = 0.0f; - gpsRescueAngle[AI_ROLL] = 0.0f; - gpsRescueAngle[AI_PITCH] = 0.0f; return; - } - - if (rescueState.phase == RESCUE_DO_NOTHING) { + case RESCUE_DO_NOTHING: gpsRescueAngle[AI_PITCH] = 0.0f; gpsRescueAngle[AI_ROLL] = 0.0f; rescueThrottle = gpsRescueConfig()->throttleHover; return; - } + default: + break; + } if (!newGPSData) { return; @@ -318,7 +307,7 @@ static void rescueAttainPosition() // when gpsRescueConfig()->rollMix is zero, there is no roll adjustment // rollAdjustment is degrees * 100 // note that the roll element has the same sign as the yaw element *before* GET_DIRECTION - gpsRescueAngle[AI_ROLL] = constrainf(rollAdjustment, rescueState.intent.maxAngleRollDeg * -100.0f, rescueState.intent.maxAngleRollDeg * 100.0f); + gpsRescueAngle[AI_ROLL] = constrainf(rollAdjustment, -rescueState.intent.rollAngleLimitDeg * 100.0f, rescueState.intent.rollAngleLimitDeg * 100.0f); // gpsRescueAngle is added to the normal roll Angle Mode corrections in pid.c rescueYaw *= GET_DIRECTION(rcControlsConfig()->yaw_control_reversed); @@ -332,7 +321,11 @@ static void rescueAttainPosition() /** Pitch / velocity controller */ - float velocityError = (rescueState.intent.targetVelocityCmS - rescueState.sensor.velocityToHomeCmS); + const float velocityTargetLimiter = constrainf((60.0f -rescueState.sensor.absErrorAngle) / 60.0f, 0.0f, 1.0f); + // attenuate velocity target when quad is not pointing towards home + // stops attempting to gain velocity when pointing the wrong way, eg overshooting home point (sudden heading error) + float velocityError = (rescueState.intent.targetVelocityCmS * velocityTargetLimiter - rescueState.sensor.velocityToHomeCmS); + // velocityError is in cm per second, positive means too slow. // NB positive pitch setpoint means nose down. // quite heavily smoothed @@ -373,7 +366,7 @@ static void rescueAttainPosition() // pitchAdjustment is the absolute Pitch angle adjustment value in degrees * 100 // it gets added to the normal level mode Pitch adjustments in pid.c - gpsRescueAngle[AI_PITCH] = constrainf(pitchAdjustment, rescueState.intent.minAnglePitchDeg * 100.0f, rescueState.intent.maxAnglePitchDeg * 100.0f); + gpsRescueAngle[AI_PITCH] = constrainf(pitchAdjustment, -rescueState.intent.pitchAngleLimitDeg * 100.0f, rescueState.intent.pitchAngleLimitDeg * 100.0f); // this angle gets added to the normal pitch Angle Mode control values in pid.c - will be seen in pitch setpoint DEBUG_SET(DEBUG_RTH, 0, gpsRescueAngle[AI_PITCH]); @@ -436,9 +429,6 @@ static void rescueAttainPosition() static void performSanityChecks() { static timeUs_t previousTimeUs = 0; // Last time Stalled/LowSat was checked - static int8_t secondsStalled = 0; // Stalled movement detection - static int8_t secondsNotDescending = 0; // Stalled descent detection - static int8_t secondsNotAscending = 0; // Stalled ascent detection static int32_t prevAltitudeCm = 0.0f; // to calculate ascent or descent change static int8_t secondsLowSats = 0; // Minimum sat detection static int8_t secondsDoingNothing = 0; // Limit on doing nothing @@ -450,9 +440,6 @@ static void performSanityChecks() } else if (rescueState.phase == RESCUE_INITIALIZE) { // Initialize internal variables each time GPS Rescue is started previousTimeUs = currentTimeUs; - secondsStalled = 5; // Start the count at 5 to be less forgiving at the beginning - secondsNotDescending = 0; - secondsNotAscending = 0; prevAltitudeCm = rescueState.sensor.currentAltitudeCm; secondsLowSats = 5; // Start the count at 5 to be less forgiving at the beginning secondsDoingNothing = 0; @@ -471,8 +458,6 @@ static void performSanityChecks() } } - DEBUG_SET(DEBUG_RTH, 2, rescueState.failure); - // Check if crash recovery mode is active if (crashRecoveryModeActive()) { rescueState.failure = RESCUE_CRASH_FLIP_DETECTED; @@ -482,7 +467,6 @@ static void performSanityChecks() if (!rescueState.sensor.healthy) { rescueState.failure = RESCUE_GPSLOST; } - // Things that should run at a low refresh rate (such as flyaway detection, etc) // This runs at 1hz const timeDelta_t dTime = cmpTimeUs(currentTimeUs, previousTimeUs); @@ -492,14 +476,15 @@ static void performSanityChecks() previousTimeUs = currentTimeUs; if (rescueState.phase == RESCUE_FLY_HOME) { - secondsStalled = constrain(secondsStalled + ((rescueState.sensor.velocityToHomeCmS < (0.5 * rescueState.intent.targetVelocityCmS)) ? 1 : -1), 0, 20); - if (secondsStalled == 20) { + rescueState.intent.secondsFailing += (rescueState.sensor.velocityToHomeCmS < 0.5 * rescueState.intent.targetVelocityCmS) ? 1 : -1; + rescueState.intent.secondsFailing = constrain(rescueState.intent.secondsFailing, 0, 20); + if (rescueState.intent.secondsFailing == 20) { #ifdef USE_MAG //If there is a mag and has not been disabled, we have to assume is healthy and has been used in imu.c if (sensors(SENSOR_MAG) && gpsRescueConfig()->useMag && !magForceDisable) { //Try again with mag disabled magForceDisable = true; - secondsStalled = 0; + rescueState.intent.secondsFailing = 0; } else #endif { @@ -510,16 +495,17 @@ static void performSanityChecks() // These conditions are 'special', in that even with sanity checks off, they should still apply if (rescueState.phase == RESCUE_ATTAIN_ALT) { - secondsNotAscending = constrain(secondsNotAscending + (((rescueState.sensor.currentAltitudeCm - prevAltitudeCm) > (0.5f * gpsRescueConfig()->ascendRate)) ? -1 : 1), 0, 10); - if (secondsNotAscending == 10) { + rescueState.intent.secondsFailing += (rescueState.sensor.currentAltitudeCm - prevAltitudeCm) > (0.5f * gpsRescueConfig()->ascendRate) ? -1 : 1; + rescueState.intent.secondsFailing = constrain(rescueState.intent.secondsFailing, 0, 10);if (rescueState.intent.secondsFailing == 10) { { rescueState.phase = RESCUE_ABORT; // if stuck in a tree while climbing, or otherwise can't climb, stop motors and disarm } } } else if (rescueState.phase == RESCUE_LANDING || rescueState.phase == RESCUE_DESCENT) { - secondsNotDescending = constrain(secondsNotDescending + (((prevAltitudeCm - rescueState.sensor.currentAltitudeCm) > (0.5f * gpsRescueConfig()->descendRate)) ? -1 : 1), 0, 10); - if (secondsNotDescending == 10) { + rescueState.intent.secondsFailing += (prevAltitudeCm - rescueState.sensor.currentAltitudeCm) > (0.5f * gpsRescueConfig()->descendRate) ? -1 : 1; + rescueState.intent.secondsFailing = constrain(rescueState.intent.secondsFailing, 0, 10); + if (rescueState.intent.secondsFailing == 10) { { rescueState.phase = RESCUE_ABORT; // if stuck in a tree while climbing, or don't disarm on impact, or enable GPS rescue on the ground too close @@ -536,13 +522,15 @@ static void performSanityChecks() } prevAltitudeCm = rescueState.sensor.currentAltitudeCm; - secondsLowSats = constrain(secondsLowSats + ((rescueState.sensor.numSat < (gpsRescueConfig()->minSats / 2)) ? 1 : -1), 0, 10); + secondsLowSats += rescueState.sensor.numSat < (gpsRescueConfig()->minSats / 2) ? 1 : -1; + secondsLowSats = constrain(secondsLowSats, 0, 10); if (secondsLowSats == 10) { rescueState.failure = RESCUE_LOWSATS; } - DEBUG_SET(DEBUG_RTH, 3, (secondsStalled * 100 + secondsLowSats)); //Failure can change with no new GPS Data + DEBUG_SET(DEBUG_RTH, 2, rescueState.failure); + DEBUG_SET(DEBUG_RTH, 3, (rescueState.intent.secondsFailing * 100 + secondsLowSats)); //Failure can change with no new GPS Data } static void sensorUpdate() @@ -552,6 +540,7 @@ static void sensorUpdate() rescueState.sensor.currentAltitudeCm = getEstimatedAltitudeCm(); DEBUG_SET(DEBUG_GPS_RESCUE_TRACKING, 2, rescueState.sensor.currentAltitudeCm); + DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 2, rescueState.sensor.currentAltitudeCm); // may be updated more frequently than GPS data rescueState.sensor.healthy = gpsIsHealthy(); @@ -577,6 +566,7 @@ static void sensorUpdate() } else if (rescueState.sensor.errorAngle > 180) { rescueState.sensor.errorAngle -= 360; } + rescueState.sensor.absErrorAngle = fabsf(rescueState.sensor.errorAngle); const timeUs_t currentTimeUs = micros(); const timeDelta_t gpsDataIntervalUs = cmpTimeUs(currentTimeUs, previousDataTimeUs); @@ -598,7 +588,6 @@ static void sensorUpdate() DEBUG_SET(DEBUG_GPS_RESCUE_HEADING, 2, attitude.values.yaw); // degrees * 10 DEBUG_SET(DEBUG_GPS_RESCUE_HEADING, 3, rescueState.sensor.directionToHome); // degrees * 10 DEBUG_SET(DEBUG_GPS_RESCUE_VELOCITY, 2, rescueState.sensor.velocityToHomeCmS); - DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 2, rescueState.sensor.currentAltitudeCm); DEBUG_SET(DEBUG_GPS_RESCUE_TRACKING, 0, rescueState.sensor.velocityToHomeCmS); } @@ -654,14 +643,14 @@ void updateGPSRescueState(void) // this runs a lot faster than the GPS Data update rate, and runs whether or not rescue is active { if (!FLIGHT_MODE(GPS_RESCUE_MODE)) { - rescueStop(); // sets phase to idle and does nothing else + rescueStop(); // sets phase to idle; does nothing else. Idle tasks still run. } else if (FLIGHT_MODE(GPS_RESCUE_MODE) && rescueState.phase == RESCUE_IDLE) { - rescueStart(); // sets phase to initialise if we enter GPS Rescue mode while idle + rescueStart(); // sets phase to rescue_initialise if we enter GPS Rescue mode while idle rescueAttainPosition(); // Initialise basic parameters when a Rescue starts (can't initialise sensor data reliably) performSanityChecks(); // Initialises sanity check values when a Rescue starts } - // Will now be in INITIALIZE mode, if just entered Rescue while IDLE, otherwise stays IDLE + // Will now be in RESCUE_INITIALIZE mode, if just entered Rescue while IDLE, otherwise stays IDLE sensorUpdate(); // always get latest GPS / Altitude data @@ -684,69 +673,67 @@ void updateGPSRescueState(void) // Things that should abort the start of a Rescue if (!STATE(GPS_FIX_HOME)) { // we didn't get a home point on arming - rescueState.failure = RESCUE_NO_HOME_POINT; // abort, or 20s of do nothing, as per sanity check settings - rescueState.phase = RESCUE_DO_NOTHING; + 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) { // Attempt to initiate inside minimum activation distance -> landing mode rescueState.intent.targetAltitudeCm = rescueState.sensor.currentAltitudeCm - rescueState.sensor.descendStepCm; rescueState.phase = RESCUE_LANDING; // start landing from current altitude } else { - startedLow = (rescueState.sensor.currentAltitudeCm <= rescueState.intent.returnAltitudeCm); rescueState.phase = RESCUE_ATTAIN_ALT; + rescueState.intent.secondsFailing = 0; // reset the sanity check timer for the climb + startedLow = (rescueState.sensor.currentAltitudeCm <= rescueState.intent.returnAltitudeCm); + rescueState.intent.updateYaw = true; // point the nose to home at all times during the rescue + rescueState.intent.targetVelocityCmS = 0; // zero forward velocity while climbing + rescueState.intent.pitchAngleLimitDeg = halfAngle; // only half pitch authority + rescueState.intent.rollAngleLimitDeg = 0; // don't roll yet } break; case RESCUE_ATTAIN_ALT: - // increase target altitude from current altitude towards the "fly home" altitude at ascent or descent rate - // sanity check will abort if altitude gain is blocked in a reasonable time - // exit once we are within one ascend/descend step of the target altitude - // while climbing, rotate; use pitch only to get velocity to home close to zero if possible + // gradually increment the target altitude until the final target altitude value is set + // also require that the final target altitude has been achieved before moving on + // sanity check will abort if altitude gain is blocked for a cumulative period + // TO DO: if overshoots are a problem after craft achieves target altitude changes, adjust termination threshold with current vertical velocity if (newGPSData) { if (startedLow) { - if (rescueState.sensor.currentAltitudeCm > (rescueState.intent.returnAltitudeCm - rescueState.sensor.ascendStepCm)) { - rescueState.phase = RESCUE_ROTATE; - } else if (rescueState.intent.targetAltitudeCm < rescueState.intent.returnAltitudeCm) { + if (rescueState.intent.targetAltitudeCm < rescueState.intent.returnAltitudeCm) { rescueState.intent.targetAltitudeCm += rescueState.sensor.ascendStepCm; - // note targetAltitudeCm is dynamically updated each new GPS data in idleTasks() - } - } else { - if (rescueState.sensor.currentAltitudeCm < (rescueState.intent.returnAltitudeCm + rescueState.sensor.descendStepCm)) { + } else if (rescueState.sensor.currentAltitudeCm > rescueState.intent.returnAltitudeCm) { + rescueState.intent.targetAltitudeCm = rescueState.intent.returnAltitudeCm; rescueState.phase = RESCUE_ROTATE; - } else if (rescueState.intent.targetAltitudeCm > rescueState.intent.returnAltitudeCm) { + } + } else { + if (rescueState.intent.targetAltitudeCm > rescueState.intent.returnAltitudeCm) { rescueState.intent.targetAltitudeCm -= rescueState.sensor.descendStepCm; + } else if (rescueState.sensor.currentAltitudeCm < rescueState.intent.returnAltitudeCm) { + rescueState.intent.targetAltitudeCm = rescueState.intent.returnAltitudeCm; + rescueState.phase = RESCUE_ROTATE; } } - // try to correct velocity relative to home by allowing pitch corrections with zero target velocity - rescueState.intent.targetVelocityCmS = 0; - rescueState.intent.minAnglePitchDeg = -halfAngle; // reduces forward velocity - rescueState.intent.maxAnglePitchDeg = halfAngle; - rescueState.intent.updateYaw = true; // allow yaw during the climb, since this helps gain altitude } - rescueState.intent.maxAngleRollDeg = 0; // no roll yet break; case RESCUE_ROTATE: // we may bypass attain_alt so this must stand alone. // complete the rotation, allowing pitch when pointing towards home + // keep in mind that rotation may already be complete if (newGPSData) { - if (rescueState.sensor.errorAngle < 15.0f) { - rescueState.phase = RESCUE_FLY_HOME; - // include roll corrections - } else if (rescueState.sensor.errorAngle < 45.0f) { - // pitch forward - rescueState.intent.minAnglePitchDeg = -gpsRescueConfig()->angle; - rescueState.intent.maxAnglePitchDeg = gpsRescueConfig()->angle; + if (rescueState.sensor.absErrorAngle < 60.0f) { + // give the craft a forward velocity target (will be attenuated by heading) and full pitch authority rescueState.intent.targetVelocityCmS = gpsRescueConfig()->rescueGroundspeed; - } else { - rescueState.intent.minAnglePitchDeg = -halfAngle; - rescueState.intent.maxAnglePitchDeg = halfAngle; - rescueState.intent.targetVelocityCmS = 0; + rescueState.intent.pitchAngleLimitDeg = gpsRescueConfig()->angle; + if (rescueState.sensor.absErrorAngle < 15.0f) { + // enable roll, enter full fly home phase + rescueState.phase = RESCUE_FLY_HOME; + rescueState.intent.secondsFailing = 0; // reset sanity timer for flight home + rescueState.intent.rollAngleLimitDeg = gpsRescueConfig()->angle; + // enable roll with yaw + } } - rescueState.intent.targetAltitudeCm = rescueState.intent.returnAltitudeCm; - rescueState.intent.updateYaw = true; } - rescueState.intent.maxAngleRollDeg = 0; // no roll until yaw error is less than 15 degrees break; case RESCUE_FLY_HOME: @@ -754,14 +741,9 @@ void updateGPSRescueState(void) if (newGPSData) { if (rescueState.sensor.distanceToHomeM <= rescueState.intent.descentDistanceM) { rescueState.phase = RESCUE_DESCENT; + rescueState.intent.secondsFailing = 0; // reset sanity timer for descent } } - rescueState.intent.targetAltitudeCm = rescueState.intent.returnAltitudeCm; - rescueState.intent.targetVelocityCmS = gpsRescueConfig()->rescueGroundspeed; - rescueState.intent.minAnglePitchDeg = -gpsRescueConfig()->angle; - rescueState.intent.maxAnglePitchDeg = gpsRescueConfig()->angle; - rescueState.intent.updateYaw = true; - rescueState.intent.maxAngleRollDeg = gpsRescueConfig()->angle; break; case RESCUE_DESCENT: @@ -769,56 +751,40 @@ void updateGPSRescueState(void) // once inside the landing box, stop rotating, just descend if (newGPSData) { const int32_t targetLandingAltitudeCm = 100.0f * gpsRescueConfig()->targetLandingAltitudeM; - if (rescueState.sensor.distanceToHomeM > gpsRescueConfig()->targetLandingDistanceM && rescueState.sensor.currentAltitudeCm > targetLandingAltitudeCm) { - // outside the landing box - const float proximityToHome = constrainf(rescueState.sensor.distanceToHomeM / rescueState.intent.descentDistanceM, 0.0f, 1.0f); - const float targetAltitudeModified = (proximityToHome * (rescueState.intent.returnAltitudeCm - targetLandingAltitudeCm)) + targetLandingAltitudeCm; - // aim for top of box - float targetAltitudeWhileDescendingCm = 0.0f; - targetAltitudeWhileDescendingCm = MIN(targetAltitudeWhileDescendingCm, targetAltitudeModified); - // don't let target altitude increase. leads to landing 'short of the box' but much safer if it overshoots, won't climb again - float landingDescendStepCm = rescueState.sensor.descendStepCm * (1.0f + proximityToHome); - // this is the maximum allowed step in target altitude, a bit faster at the start, since velocity also will be faster at the start - rescueState.intent.targetAltitudeCm -= constrain((rescueState.sensor.currentAltitudeCm - targetAltitudeWhileDescendingCm), 0, landingDescendStepCm); - // reduce current altitude towards target altitude, but not by more than landingDescendStepCm - rescueState.intent.targetVelocityCmS = gpsRescueConfig()->rescueGroundspeed * proximityToHome; - // slow down as we get closer to home, allowing zero for vertical drops - } else { - // go to landing mode once inside the box, with zero groundspeed target + if (rescueState.sensor.currentAltitudeCm < targetLandingAltitudeCm) { + // enter landing mode once below landing altitude rescueState.phase = RESCUE_LANDING; rescueState.intent.targetAltitudeCm -= rescueState.sensor.descendStepCm; - // take one descent step off our target altitude now + rescueState.intent.secondsFailing = 0; // reset sanity timer for landing + rescueState.intent.targetVelocityCmS = 0; // zero velocity to home + rescueState.intent.pitchAngleLimitDeg = halfAngle; // reduced pitch angles + rescueState.intent.rollAngleLimitDeg = 0; // no roll while landing + } else { + const float distanceToLandingAreaM = MAX(rescueState.sensor.distanceToHomeM - 2.0f, 0.0f); + // considers home to be within a 2m circle of home to avoid searching around when crossing home + const float proximityToLandingArea = constrainf(distanceToLandingAreaM / rescueState.intent.descentDistanceM, 0.0f, 1.0f); + rescueState.intent.targetAltitudeCm -= rescueState.sensor.descendStepCm * (1.0f + proximityToLandingArea); + // reduce current altitude inexorably, by not less than descendStepCm and not more than 2*descendStepCm + rescueState.intent.targetVelocityCmS = gpsRescueConfig()->rescueGroundspeed * 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, and pitch is allowed + rescueState.intent.rollAngleLimitDeg = gpsRescueConfig()->angle * proximityToLandingArea; + // reduce roll capability when closer to home, none within final 2m } } - rescueState.intent.minAnglePitchDeg = -gpsRescueConfig()->angle; // aids slowing down - rescueState.intent.maxAnglePitchDeg = gpsRescueConfig()->angle; - rescueState.intent.updateYaw = true; - rescueState.intent.maxAngleRollDeg = gpsRescueConfig()->angle; break; case RESCUE_LANDING: - // let Level Mode keep the quad flat and true and just lose altitude until it hits the ground - // it will be moved crabwise by wind and momentum but we don't know how best to respond; better to do nothing at all - // Note: no iTerm on throttle during landing + // keep reducing target altitude, keep nose to home, zero velocity target with limited pitch control, no roll if (newGPSData) { rescueState.intent.targetAltitudeCm -= rescueState.sensor.descendStepCm; // take one step off target altitude every time we get new GPS data } - // keep descending at descendStepCm until we hit the ground - // can't use altitude since the "zero altitude" value may be incorrect - if (rescueState.sensor.accMagnitude > 2.0f) { setArmingDisabled(ARMING_DISABLED_ARM_SWITCH); disarm(DISARM_REASON_GPS_RESCUE); rescueState.phase = RESCUE_COMPLETE; } - - rescueState.intent.minAnglePitchDeg = -halfAngle; - rescueState.intent.maxAnglePitchDeg = halfAngle; - rescueState.intent.targetVelocityCmS = 0.0f; - rescueState.intent.updateYaw = true; - rescueState.intent.maxAngleRollDeg = 0; - // keep current heading and attitude, just drop break; case RESCUE_COMPLETE: @@ -833,6 +799,7 @@ void updateGPSRescueState(void) case RESCUE_DO_NOTHING: break; + default: break; } @@ -842,10 +809,7 @@ void updateGPSRescueState(void) DEBUG_SET(DEBUG_RTH, 1, rescueState.phase); performSanityChecks(); - - if (rescueState.phase != RESCUE_IDLE) { - rescueAttainPosition(); - } + rescueAttainPosition(); newGPSData = false; } @@ -877,6 +841,7 @@ bool gpsRescueIsAvailable(void) } bool gpsRescueIsDisabled(void) +// used for OSD warning { return (!STATE(GPS_FIX_HOME)); } diff --git a/src/main/flight/gps_rescue.h b/src/main/flight/gps_rescue.h index 381e788102..615d7066d7 100644 --- a/src/main/flight/gps_rescue.h +++ b/src/main/flight/gps_rescue.h @@ -37,8 +37,7 @@ typedef struct gpsRescue_s { uint8_t sanityChecks; uint8_t allowArmingWithoutFix; uint8_t useMag; - uint16_t targetLandingAltitudeM; //meters - uint16_t targetLandingDistanceM; //meters + uint8_t targetLandingAltitudeM; //meters uint8_t altitudeMode; uint16_t ascendRate; uint16_t descendRate; diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 1d05d79a4f..6742c3ed92 100644 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -276,7 +276,7 @@ gpsData_t gpsData; PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0); PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig, - .provider = GPS_NMEA, + .provider = GPS_UBLOX, .sbasMode = SBAS_NONE, .autoConfig = GPS_AUTOCONFIG_ON, .autoBaud = GPS_AUTOBAUD_OFF, @@ -681,7 +681,7 @@ void gpsInitUblox(void) } break; case 12: - ubloxSetNavRate(0xC8, 1, 1); // set rate to 5Hz (measurement period: 200ms, navigation rate: 1 cycle) + ubloxSetNavRate(0x64, 1, 1); // set rate to 10Hz (measurement period: 100ms, navigation rate: 1 cycle) (for 5Hz use 0xC8) break; case 13: ubloxSetSbas();