From bb5f59a94de2c70fdff19d16287071ce83f8c430 Mon Sep 17 00:00:00 2001 From: Nicola De Pasquale Date: Mon, 15 Apr 2019 00:17:20 +0200 Subject: [PATCH 1/3] new gps rescue pid controller based on vertical velocity [ci skip] --- src/main/build/debug.c | 1 + src/main/build/debug.h | 1 + src/main/cli/settings.c | 4 +- src/main/flight/gps_rescue.c | 113 ++++++++++++++++++++++++++++------- src/main/flight/gps_rescue.h | 2 + 5 files changed, 97 insertions(+), 24 deletions(-) diff --git a/src/main/build/debug.c b/src/main/build/debug.c index 410dcf6da3..21f3e005fc 100644 --- a/src/main/build/debug.c +++ b/src/main/build/debug.c @@ -91,4 +91,5 @@ const char * const debugModeNames[DEBUG_COUNT] = { "CRSF_LINK_STATISTICS_PWR", "CRSF_LINK_STATISTICS_DOWN", "BARO", + "GPS_RESCUE_THROTTLE_PID", }; diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 3ac537eea3..be92a3fa05 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -107,6 +107,7 @@ typedef enum { DEBUG_CRSF_LINK_STATISTICS_PWR, DEBUG_CRSF_LINK_STATISTICS_DOWN, DEBUG_BARO, + DEBUG_GPS_RESCUE_THROTTLE_PID, DEBUG_COUNT } debugType_e; diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 91d3550ffa..1ff56cfe19 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -908,10 +908,12 @@ const clivalue_t valueTable[] = { { "gps_rescue_throttle_min", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleMin) }, { "gps_rescue_throttle_max", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleMax) }, + { "gps_rescue_ascend_rate", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 100, 2500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, ascendRate) }, + { "gps_rescue_descend_rate", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 100, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, descendRate) }, { "gps_rescue_throttle_hover", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleHover) }, { "gps_rescue_sanity_checks", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_RESCUE_SANITY_CHECK }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, sanityChecks) }, { "gps_rescue_min_sats", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 5, 50 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, minSats) }, - { "gps_rescue_min_dth", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 50, 1000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, minRescueDth) }, + { "gps_rescue_min_dth", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 50, 1000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, minRescueDth) }, { "gps_rescue_allow_arming_without_fix", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, allowArmingWithoutFix) }, { "gps_rescue_alt_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_RESCUE_ALT_MODE }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, altitudeMode) }, #ifdef USE_MAG diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c index e419a9f2fc..8bca3f84d2 100644 --- a/src/main/flight/gps_rescue.c +++ b/src/main/flight/gps_rescue.c @@ -122,10 +122,27 @@ typedef enum { CURRENT_ALT } altitudeMode_e; +typedef struct { + float Kp; + float Ki; + float Kd; +} throttle_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_SLOWDOWN_DISTANCE_M 200 // distance from home to start decreasing speed #define GPS_RESCUE_MIN_DESCENT_DIST_M 30 // minimum descent distance allowed +#define GPS_RESCUE_ZVELOCITY_THRESHOLD 300 // altitude threshold for start decreasing z velocity +#define GPS_RESCUE_LANDING_ZVELOCITY 80 // descend velocity for final landing phase +#define GPS_RESCUE_ITERM_WINDUP 100 // reset I term after z velocity error of 100 cm/s +#define GPS_RESCUE_MAX_ITERM_ACC 250.0f //max allowed iterm value +#define GPS_RESCUE_SLOWDOWN_ALT 500 // the altitude after which the quad begins to slow down the descend velocity +#define GPS_RESCUE_MINIMUM_ZVELOCITY 50 // minimum speed for final landing phase +#define GPS_RESCUE_ALMOST_LANDING_ALT 100 // altitude after which the quad increases ground detection sensitivity + +#define GPS_RESCUE_THROTTLE_P_SCALE 0.0003125f // pid scaler for P term +#define GPS_RESCUE_THROTTLE_I_SCALE 0.1f // pid scaler for I term +#define GPS_RESCUE_THROTTLE_D_SCALE 0.0003125f // pid scaler for D term #ifdef USE_MAG #define GPS_RESCUE_USE_MAG true @@ -147,7 +164,7 @@ PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, .velI = 20, .velD = 15, .yawP = 40, - .throttleMin = 1200, + .throttleMin = 1100, .throttleMax = 1600, .throttleHover = 1280, .sanityChecks = RESCUE_SANITY_ON, @@ -158,6 +175,8 @@ PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, .targetLandingAltitudeM = 5, .targetLandingDistanceM = 10, .altitudeMode = MAX_ALT, + .ascendRate = 500, + .descendRate = 150, ); static uint16_t rescueThrottle; @@ -174,6 +193,7 @@ static bool newGPSData = false; rescueState_s rescueState; altitudeMode_e altitudeMode; +throttle_s throttle; /* If we have new GPS data, update home heading @@ -264,15 +284,19 @@ static void rescueAttainPosition() // Speed and altitude controller internal variables static float previousSpeedError = 0; static int16_t speedIntegral = 0; - static float previousAltitudeError = 0; - static int16_t altitudeIntegral = 0; + int zVelocityError; + static int previousZVelocityError = 0; + static float zVelocityIntegral = 0; + static float scalingRate = 0; + static int16_t altitudeAdjustment = 0; if (rescueState.phase == RESCUE_INITIALIZE) { // Initialize internal variables each time GPS Rescue is started previousSpeedError = 0; speedIntegral = 0; - previousAltitudeError = 0; - altitudeIntegral = 0; + previousZVelocityError = 0; + zVelocityIntegral = 0; + altitudeAdjustment = 0; } // Point to home if that is in our intent @@ -280,8 +304,10 @@ static void rescueAttainPosition() setBearing(rescueState.sensor.directionToHome); } - DEBUG_SET(DEBUG_RTH, 3, rescueState.failure); //Failure can change with no new GPS Data - + if (debugMode == DEBUG_RTH) { + DEBUG_SET(DEBUG_RTH, 3, rescueState.failure); //Failure can change with no new GPS Data + } + if (!newGPSData) { return; } @@ -305,26 +331,57 @@ static void rescueAttainPosition() /** Altitude controller */ - const int16_t altitudeError = (rescueState.intent.targetAltitudeCm - rescueState.sensor.currentAltitudeCm) / 100; // Error in meters - const int16_t altitudeDerivative = altitudeError - previousAltitudeError; - - // Only allow integral windup within +-15m absolute altitude error - if (ABS(altitudeError) < 25) { - altitudeIntegral = constrain(altitudeIntegral + altitudeError, -250, 250); + const int16_t altitudeError = rescueState.intent.targetAltitudeCm - rescueState.sensor.currentAltitudeCm; + + // P component + if (ABS(altitudeError) > 0 && ABS(altitudeError) < GPS_RESCUE_ZVELOCITY_THRESHOLD){ + scalingRate = (float)altitudeError / GPS_RESCUE_ZVELOCITY_THRESHOLD; } else { - altitudeIntegral = 0; + scalingRate = 1; } - previousAltitudeError = altitudeError; + if (altitudeError > 0) { + zVelocityError = gpsRescueConfig()->ascendRate * scalingRate - rescueState.sensor.zVelocity; + } else if (altitudeError < 0) { + if (rescueState.sensor.currentAltitudeCm <= GPS_RESCUE_SLOWDOWN_ALT) { + const int16_t rescueLandingDescendVel = MAX(GPS_RESCUE_LANDING_ZVELOCITY * rescueState.sensor.currentAltitudeCm / GPS_RESCUE_SLOWDOWN_ALT, GPS_RESCUE_MINIMUM_ZVELOCITY); + zVelocityError = -rescueLandingDescendVel - rescueState.sensor.zVelocity; + } else { + zVelocityError = -gpsRescueConfig()->descendRate * scalingRate - rescueState.sensor.zVelocity; + } + } else { + zVelocityError = 0; + } + + // I component + if (ABS(zVelocityError) < GPS_RESCUE_ITERM_WINDUP) { + zVelocityIntegral = constrainf(zVelocityIntegral + zVelocityError / 100.0f, -GPS_RESCUE_MAX_ITERM_ACC, GPS_RESCUE_MAX_ITERM_ACC); + } else { + zVelocityIntegral = 0; + } + + // D component + const int zVelocityDerivative = zVelocityError - previousZVelocityError; + previousZVelocityError = zVelocityError; - const int16_t altitudeAdjustment = (gpsRescueConfig()->throttleP * altitudeError + (gpsRescueConfig()->throttleI * altitudeIntegral) / 10 + gpsRescueConfig()->throttleD * altitudeDerivative) / ct / 20; const int16_t hoverAdjustment = (hoverThrottle - 1000) / ct; + altitudeAdjustment = constrain(altitudeAdjustment + (throttle.Kp * zVelocityError + throttle.Ki * zVelocityIntegral + throttle.Kd * zVelocityDerivative), + gpsRescueConfig()->throttleMin - 1000 - hoverAdjustment, gpsRescueConfig()->throttleMax - 1000 - hoverAdjustment); rescueThrottle = constrain(1000 + altitudeAdjustment + hoverAdjustment, gpsRescueConfig()->throttleMin, gpsRescueConfig()->throttleMax); - DEBUG_SET(DEBUG_RTH, 0, rescueThrottle); - DEBUG_SET(DEBUG_RTH, 1, gpsRescueAngle[AI_PITCH]); - DEBUG_SET(DEBUG_RTH, 2, altitudeAdjustment); + if (debugMode == DEBUG_RTH) { + DEBUG_SET(DEBUG_RTH, 0, rescueThrottle); + DEBUG_SET(DEBUG_RTH, 1, gpsRescueAngle[AI_PITCH]); + DEBUG_SET(DEBUG_RTH, 2, altitudeAdjustment); + } + + if (debugMode == DEBUG_GPS_RESCUE_THROTTLE_PID) { + DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 0, throttle.Kp * zVelocityError); + DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 1, throttle.Ki * zVelocityIntegral); + DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 2, throttle.Kd * zVelocityDerivative); + DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 3, rescueState.sensor.zVelocity); + } } static void performSanityChecks() @@ -495,7 +552,8 @@ void updateGPSRescueState(void) static float_t lineSlope; static float_t lineOffsetM; static int32_t newSpeed; - static uint32_t newAltitude; + static int32_t newAltitude; + float magnitudeTrigger; if (!FLIGHT_MODE(GPS_RESCUE_MODE)) { rescueStop(); @@ -520,6 +578,10 @@ void updateGPSRescueState(void) hoverThrottle = gpsRescueConfig()->throttleHover; } + throttle.Kp = gpsRescueConfig()->throttleP * GPS_RESCUE_THROTTLE_P_SCALE; + throttle.Ki = gpsRescueConfig()->throttleI * GPS_RESCUE_THROTTLE_I_SCALE; + throttle.Kd = gpsRescueConfig()->throttleD * GPS_RESCUE_THROTTLE_D_SCALE; + if (!STATE(GPS_FIX_HOME)) { setArmingDisabled(ARMING_DISABLED_ARM_SWITCH); disarm(); @@ -559,7 +621,7 @@ void updateGPSRescueState(void) } //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); + lineSlope = ((float)gpsRescueConfig()->initialAltitudeM - gpsRescueConfig()->targetLandingAltitudeM) / (newDescentDistanceM - gpsRescueConfig()->targetLandingDistanceM); lineOffsetM = gpsRescueConfig()->initialAltitudeM - lineSlope * newDescentDistanceM; rescueState.phase = RESCUE_ATTAIN_ALT; @@ -612,9 +674,14 @@ void updateGPSRescueState(void) 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. // At this point, do not let the target altitude go up anymore, so if we overshoot, we dont' move in a parabolic trajectory - // If we are over 150% of average magnitude, just disarm since we're pretty much home - if (rescueState.sensor.accMagnitude > rescueState.sensor.accMagnitudeAvg * 1.5) { + if (rescueState.sensor.currentAltitudeCm < GPS_RESCUE_ALMOST_LANDING_ALT) { + magnitudeTrigger = rescueState.sensor.accMagnitudeAvg * 1.2; + } else { + magnitudeTrigger = rescueState.sensor.accMagnitudeAvg * 1.5; + } + + if (rescueState.sensor.accMagnitude > magnitudeTrigger) { setArmingDisabled(ARMING_DISABLED_ARM_SWITCH); disarm(); rescueState.phase = RESCUE_COMPLETE; diff --git a/src/main/flight/gps_rescue.h b/src/main/flight/gps_rescue.h index f50149a348..31113287d0 100644 --- a/src/main/flight/gps_rescue.h +++ b/src/main/flight/gps_rescue.h @@ -40,6 +40,8 @@ typedef struct gpsRescue_s { uint16_t targetLandingAltitudeM; //meters uint16_t targetLandingDistanceM; //meters uint8_t altitudeMode; + uint16_t ascendRate; + uint16_t descendRate; } gpsRescueConfig_t; PG_DECLARE(gpsRescueConfig_t, gpsRescueConfig); From 716eea26ffb717d87ebf16b47589f8637d690466 Mon Sep 17 00:00:00 2001 From: Nicola De Pasquale Date: Sat, 29 Jun 2019 19:39:57 +0200 Subject: [PATCH 2/3] requested changes [ci skip] --- src/main/flight/gps_rescue.c | 22 ++++++++-------------- 1 file changed, 8 insertions(+), 14 deletions(-) diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c index 8bca3f84d2..cc9a95d363 100644 --- a/src/main/flight/gps_rescue.c +++ b/src/main/flight/gps_rescue.c @@ -304,9 +304,7 @@ static void rescueAttainPosition() setBearing(rescueState.sensor.directionToHome); } - if (debugMode == DEBUG_RTH) { - DEBUG_SET(DEBUG_RTH, 3, rescueState.failure); //Failure can change with no new GPS Data - } + DEBUG_SET(DEBUG_RTH, 3, rescueState.failure); //Failure can change with no new GPS Data if (!newGPSData) { return; @@ -370,18 +368,14 @@ static void rescueAttainPosition() rescueThrottle = constrain(1000 + altitudeAdjustment + hoverAdjustment, gpsRescueConfig()->throttleMin, gpsRescueConfig()->throttleMax); - if (debugMode == DEBUG_RTH) { - DEBUG_SET(DEBUG_RTH, 0, rescueThrottle); - DEBUG_SET(DEBUG_RTH, 1, gpsRescueAngle[AI_PITCH]); - DEBUG_SET(DEBUG_RTH, 2, altitudeAdjustment); - } + DEBUG_SET(DEBUG_RTH, 0, rescueThrottle); + DEBUG_SET(DEBUG_RTH, 1, gpsRescueAngle[AI_PITCH]); + DEBUG_SET(DEBUG_RTH, 2, altitudeAdjustment); - if (debugMode == DEBUG_GPS_RESCUE_THROTTLE_PID) { - DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 0, throttle.Kp * zVelocityError); - DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 1, throttle.Ki * zVelocityIntegral); - DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 2, throttle.Kd * zVelocityDerivative); - DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 3, rescueState.sensor.zVelocity); - } + DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 0, throttle.Kp * zVelocityError); + DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 1, throttle.Ki * zVelocityIntegral); + DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 2, throttle.Kd * zVelocityDerivative); + DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 3, rescueState.sensor.zVelocity); } static void performSanityChecks() From a3d43832f2d77b64814404a61abbcac17fda6f0a Mon Sep 17 00:00:00 2001 From: Nicola De Pasquale Date: Sun, 30 Jun 2019 11:24:11 +0200 Subject: [PATCH 3/3] remove whitespaces [ci skip] --- src/main/flight/gps_rescue.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c index cc9a95d363..df4f4be800 100644 --- a/src/main/flight/gps_rescue.c +++ b/src/main/flight/gps_rescue.c @@ -305,7 +305,7 @@ static void rescueAttainPosition() } DEBUG_SET(DEBUG_RTH, 3, rescueState.failure); //Failure can change with no new GPS Data - + if (!newGPSData) { return; }