From 9868557c115415a23e28f7ce5e22249d06c2cbff Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Wed, 27 Jun 2018 10:00:28 -0400 Subject: [PATCH 1/3] Revise GPS Rescue throttle and yaw handling to not modify rcCommand Injecting new values directly into rcCommand to override pilot inputs does not work correctly when rc interpolation or smoothing is enabled. This is because the smoothing functions maintain an internal state that is used to produce the final rcCommand values. So in effect it re-overrides the values set by GPS Rescue. Additionally, modifying rcCommand directly is undesirable because: - It happens before rates are applied. So the pilot's rates will effect the control commanded by GPS Rescue. - rcCommand values are used for more than input in the flight control. They also are used for stick commands, etc. - In the case of throttle, various modifications can be additionally applied like throttle boost and throttle limit that may negatively effect GPS Rescue. These changes revise the logic to only modify the commanded values used in the PID controller (yaw) and mixer (throttle) rather than attempting to override rcCommand. --- src/main/fc/fc_core.c | 5 ---- src/main/fc/fc_rc.c | 32 +++++++++++++++++--- src/main/flight/gps_rescue.c | 57 ++++++++++++++++++++++++++++-------- src/main/flight/gps_rescue.h | 3 +- src/main/flight/mixer.c | 9 ++++++ 5 files changed, 83 insertions(+), 23 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 8e378fe125..3e28f8a4a4 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -992,11 +992,6 @@ static FAST_CODE_NOINLINE void subTaskRcCommand(timeUs_t currentTimeUs) processRcCommand(); -#if defined(USE_GPS_RESCUE) - if (FLIGHT_MODE(GPS_RESCUE_MODE)) { - gpsRescueInjectRcCommands(); - } -#endif } // Function for loop trigger diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index d634c8982f..c908ab3a8a 100644 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -43,6 +43,7 @@ #include "flight/failsafe.h" #include "flight/imu.h" +#include "flight/gps_rescue.h" #include "flight/pid.h" #include "pg/rx.h" #include "rx/rx.h" @@ -144,10 +145,13 @@ float applyRaceFlightRates(const int axis, float rcCommandf, const float rcComma return angleRate; } -static void calculateSetpointRate(int axis) +static void calculateSetpointRate(int axis, bool zeroAxis) { - // scale rcCommandf to range [-1.0, 1.0] - float rcCommandf = rcCommand[axis] / 500.0f; + float rcCommandf = 0; + if (!zeroAxis) { + // scale rcCommandf to range [-1.0, 1.0] + rcCommandf = rcCommand[axis] / 500.0f; + } rcDeflection[axis] = rcCommandf; const float rcCommandfAbs = ABS(rcCommandf); rcDeflectionAbs[axis] = rcCommandfAbs; @@ -548,7 +552,27 @@ FAST_CODE void processRcCommand(void) #if defined(SIMULATOR_BUILD) #pragma GCC diagnostic pop #endif - calculateSetpointRate(axis); + bool useGpsRescueYaw = false; +#ifdef USE_GPS_RESCUE + // Logic improvements to not override rcCommand when GPS Rescue is active. + // Instead only modify the flight control values derived from rcCommand + // like setpointRate[] and rcDeflection[]. + if ((axis == FD_YAW) && FLIGHT_MODE(GPS_RESCUE_MODE)) { + // If GPS Rescue is active then override the yaw commanded input + // with a zero (centered) stick input. Otherwise the pilot's yaw inputs + // will get calculated into the rcDeflection values affecting downstream logic + useGpsRescueYaw = true; + } +#endif + calculateSetpointRate(axis, useGpsRescueYaw); + +#ifdef USE_GPS_RESCUE + if (useGpsRescueYaw) { + // If GPS Rescue is active then override the setpointRate used in the + // pid controller with the value calculated from the desired heading logic. + setpointRate[axis] = gpsRescueGetYawRate(); + } +#endif } DEBUG_SET(DEBUG_RC_INTERPOLATION, 3, setpointRate[0]); diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c index 792344f4b6..8c47bbaeec 100644 --- a/src/main/flight/gps_rescue.c +++ b/src/main/flight/gps_rescue.c @@ -50,6 +50,9 @@ #include "gps_rescue.h" +#define GPS_RESCUE_MAX_YAW_RATE 180 // 180 deg/sec +#define GPS_RESCUE_RATE_SCALE_DEGREES 20 // Scale the commanded yaw rate when the error is less then this angle + PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 0); PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, @@ -71,8 +74,8 @@ PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, .minSats = 8 ); -static uint16_t rescueThrottle; -static int16_t rescueYaw; +static uint16_t rescueThrottle; +static float rescueYaw; int32_t gpsRescueAngle[ANGLE_INDEX_COUNT] = { 0, 0 }; uint16_t hoverThrottle = 0; @@ -319,6 +322,11 @@ void idleTasks() rescueThrottle = rcCommand[THROTTLE]; //to do: have a default value for hoverThrottle + + // FIXME: GPS Rescue throttle handling should take into account min_check as the + // active throttle is from min_check through PWM_RANGE_MAX. Currently adjusting for this + // in gpsRescueGetThrottle() but it would be better handled here. + float ct = getCosTiltAngle(); if (ct > 0.5 && ct < 0.96 && throttleSamples < 1E6 && rescueThrottle > 1070) { //5 to 45 degrees tilt //TO DO: only sample when acceleration is low @@ -395,25 +403,48 @@ void rescueAttainPosition() } // Very similar to maghold function on betaflight/cleanflight -void setBearing(int16_t deg) +void setBearing(int16_t desiredHeading) { - int16_t dif = DECIDEGREES_TO_DEGREES(attitude.values.yaw) - deg; + float errorAngle = DECIDEGREES_TO_DEGREES(attitude.values.yaw) - desiredHeading; - if (dif <= -180) { - dif += 360; - } else if (dif > 180) { - dif -= 360; + // Determine the most efficient direction to rotate + if (errorAngle <= -180) { + errorAngle += 360; + } else if (errorAngle > 180) { + errorAngle -= 360; } - dif *= -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed); + errorAngle *= -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed); + const int errorAngleSign = (errorAngle < 0) ? -1 : 1; + const float errorAngleAbs = ABS(errorAngle); + + // Calculate a desired yaw rate based on a maximum limit beyond + // an error window and then scale the requested rate down inside + // the window as error approaches 0. + rescueYaw = (errorAngleAbs > GPS_RESCUE_RATE_SCALE_DEGREES) ? GPS_RESCUE_MAX_YAW_RATE : errorAngleAbs; - rescueYaw = - (dif * gpsRescueConfig()->yawP / 20); + rescueYaw *= errorAngleSign; } -void gpsRescueInjectRcCommands(void) +float gpsRescueGetYawRate(void) { - rcCommand[THROTTLE] = rescueThrottle; - rcCommand[YAW] = rescueYaw; + return rescueYaw; +} + +float gpsRescueGetThrottle(void) +{ + float commandedThrottle = 0; + const int minCheck = MAX(rxConfig()->mincheck, 1000); + + // We need to compensate for min_check since the throttle value set by gps rescue + // is based on the raw rcCommand value commanded by the pilot. + if (rescueThrottle > minCheck) { + // Calculated a desired commanded throttle scaled from 0.0 to 1.0 for use in the mixer. + commandedThrottle = (float)(rescueThrottle - minCheck) / (PWM_RANGE_MAX - minCheck); + commandedThrottle = constrainf(commandedThrottle, 0.0f, 1.0f); + } + + return commandedThrottle; } #endif diff --git a/src/main/flight/gps_rescue.h b/src/main/flight/gps_rescue.h index cfce5bd41c..afcb5bf0a2 100644 --- a/src/main/flight/gps_rescue.h +++ b/src/main/flight/gps_rescue.h @@ -109,4 +109,5 @@ void sensorUpdate(void); void rescueAttainPosition(void); -void gpsRescueInjectRcCommands(void); +float gpsRescueGetYawRate(void); +float gpsRescueGetThrottle(void); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 70b4d5841f..e8b5f13755 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -54,6 +54,7 @@ #include "flight/failsafe.h" #include "flight/imu.h" +#include "flight/gps_rescue.h" #include "flight/mixer.h" #include "flight/mixer_tricopter.h" #include "flight/pid.h" @@ -810,6 +811,14 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensa } #endif +#ifdef USE_GPS_RESCUE + // If gps rescue is active then override the throttle. This prevents things + // like throttle boost or throttle limit from negatively affecting the throttle. + if (FLIGHT_MODE(GPS_RESCUE_MODE)) { + throttle = gpsRescueGetThrottle(); + } +#endif + motorMixRange = motorMixMax - motorMixMin; if (motorMixRange > 1.0f) { for (int i = 0; i < motorCount; i++) { From 0cd61c3a0ff12f124e395afe1f961020650b234c Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Wed, 27 Jun 2018 11:10:52 -0400 Subject: [PATCH 2/3] Revise yaw rate calculation and defaults --- src/main/flight/gps_rescue.c | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c index 8c47bbaeec..ea148a5705 100644 --- a/src/main/flight/gps_rescue.c +++ b/src/main/flight/gps_rescue.c @@ -50,8 +50,8 @@ #include "gps_rescue.h" -#define GPS_RESCUE_MAX_YAW_RATE 180 // 180 deg/sec -#define GPS_RESCUE_RATE_SCALE_DEGREES 20 // Scale the commanded yaw rate when the error is less then this angle +#define GPS_RESCUE_MAX_YAW_RATE 100 // 100 deg/sec max yaw +#define GPS_RESCUE_RATE_SCALE_DEGREES 45 // Scale the commanded yaw rate when the error is less then this angle PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 0); @@ -421,6 +421,11 @@ void setBearing(int16_t desiredHeading) // Calculate a desired yaw rate based on a maximum limit beyond // an error window and then scale the requested rate down inside // the window as error approaches 0. + if (errorAngleAbs > GPS_RESCUE_RATE_SCALE_DEGREES) { + rescueYaw = GPS_RESCUE_MAX_YAW_RATE; + } else { + rescueYaw = (errorAngleAbs / GPS_RESCUE_RATE_SCALE_DEGREES) * GPS_RESCUE_MAX_YAW_RATE; + } rescueYaw = (errorAngleAbs > GPS_RESCUE_RATE_SCALE_DEGREES) ? GPS_RESCUE_MAX_YAW_RATE : errorAngleAbs; rescueYaw *= errorAngleSign; From 9b49d481e3dadbd12d30588631bf5669521288eb Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Wed, 27 Jun 2018 19:27:22 -0400 Subject: [PATCH 3/3] Fix setpoint rate sign, optimize code, cleanup from review Also adds a `scaleRangef()` function to math.c --- src/main/common/maths.c | 6 ++++ src/main/common/maths.h | 1 + src/main/fc/fc_rc.c | 57 +++++++++++++++--------------------- src/main/flight/gps_rescue.c | 26 ++++------------ 4 files changed, 37 insertions(+), 53 deletions(-) diff --git a/src/main/common/maths.c b/src/main/common/maths.c index 565684be82..00a0c51ecd 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -176,6 +176,12 @@ int scaleRange(int x, int srcFrom, int srcTo, int destFrom, int destTo) { return (a / b) + destFrom; } +float scaleRangef(float x, float srcFrom, float srcTo, float destFrom, float destTo) { + float a = (destTo - destFrom) * (x - srcFrom); + float b = srcTo - srcFrom; + return (a / b) + destFrom; +} + // Normalize a vector void normalizeV(struct fp_vector *src, struct fp_vector *dest) { diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 957f85d81d..2cdae7ec17 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -102,6 +102,7 @@ float devStandardDeviation(stdev_t *dev); float degreesToRadians(int16_t degrees); int scaleRange(int x, int srcFrom, int srcTo, int destFrom, int destTo); +float scaleRangef(float x, float srcFrom, float srcTo, float destFrom, float destTo); void normalizeV(struct fp_vector *src, struct fp_vector *dest); diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index c908ab3a8a..4472d246b5 100644 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -145,22 +145,33 @@ float applyRaceFlightRates(const int axis, float rcCommandf, const float rcComma return angleRate; } -static void calculateSetpointRate(int axis, bool zeroAxis) +static void calculateSetpointRate(int axis) { - float rcCommandf = 0; - if (!zeroAxis) { - // scale rcCommandf to range [-1.0, 1.0] - rcCommandf = rcCommand[axis] / 500.0f; - } - rcDeflection[axis] = rcCommandf; - const float rcCommandfAbs = ABS(rcCommandf); - rcDeflectionAbs[axis] = rcCommandfAbs; + float angleRate; + +#ifdef USE_GPS_RESCUE + if ((axis == FD_YAW) && FLIGHT_MODE(GPS_RESCUE_MODE)) { + // If GPS Rescue is active then override the setpointRate used in the + // pid controller with the value calculated from the desired heading logic. + angleRate = gpsRescueGetYawRate(); - float angleRate = applyRates(axis, rcCommandf, rcCommandfAbs); + // Treat the stick input as centered to avoid any stick deflection base modifications (like acceleration limit) + rcDeflection[axis] = 0; + rcDeflectionAbs[axis] = 0; + } else +#endif + { + // scale rcCommandf to range [-1.0, 1.0] + float rcCommandf = rcCommand[axis] / 500.0f; + rcDeflection[axis] = rcCommandf; + const float rcCommandfAbs = ABS(rcCommandf); + rcDeflectionAbs[axis] = rcCommandfAbs; + + angleRate = applyRates(axis, rcCommandf, rcCommandfAbs); + } + setpointRate[axis] = constrainf(angleRate, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); // Rate limit protection (deg/sec) DEBUG_SET(DEBUG_ANGLERATE, axis, angleRate); - - setpointRate[axis] = constrainf(angleRate, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); // Rate limit protection (deg/sec) } static void scaleRcCommandToFpvCamAngle(void) @@ -552,27 +563,7 @@ FAST_CODE void processRcCommand(void) #if defined(SIMULATOR_BUILD) #pragma GCC diagnostic pop #endif - bool useGpsRescueYaw = false; -#ifdef USE_GPS_RESCUE - // Logic improvements to not override rcCommand when GPS Rescue is active. - // Instead only modify the flight control values derived from rcCommand - // like setpointRate[] and rcDeflection[]. - if ((axis == FD_YAW) && FLIGHT_MODE(GPS_RESCUE_MODE)) { - // If GPS Rescue is active then override the yaw commanded input - // with a zero (centered) stick input. Otherwise the pilot's yaw inputs - // will get calculated into the rcDeflection values affecting downstream logic - useGpsRescueYaw = true; - } -#endif - calculateSetpointRate(axis, useGpsRescueYaw); - -#ifdef USE_GPS_RESCUE - if (useGpsRescueYaw) { - // If GPS Rescue is active then override the setpointRate used in the - // pid controller with the value calculated from the desired heading logic. - setpointRate[axis] = gpsRescueGetYawRate(); - } -#endif + calculateSetpointRate(axis); } DEBUG_SET(DEBUG_RC_INTERPOLATION, 3, setpointRate[0]); diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c index ea148a5705..d9f6eccdb4 100644 --- a/src/main/flight/gps_rescue.c +++ b/src/main/flight/gps_rescue.c @@ -50,7 +50,7 @@ #include "gps_rescue.h" -#define GPS_RESCUE_MAX_YAW_RATE 100 // 100 deg/sec max yaw +#define GPS_RESCUE_MAX_YAW_RATE 360 // 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 PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 0); @@ -405,7 +405,7 @@ void rescueAttainPosition() // Very similar to maghold function on betaflight/cleanflight void setBearing(int16_t desiredHeading) { - float errorAngle = DECIDEGREES_TO_DEGREES(attitude.values.yaw) - desiredHeading; + float errorAngle = (attitude.values.yaw / 10.0f) - desiredHeading; // Determine the most efficient direction to rotate if (errorAngle <= -180) { @@ -415,20 +415,11 @@ void setBearing(int16_t desiredHeading) } errorAngle *= -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed); - const int errorAngleSign = (errorAngle < 0) ? -1 : 1; - const float errorAngleAbs = ABS(errorAngle); // Calculate a desired yaw rate based on a maximum limit beyond // an error window and then scale the requested rate down inside // the window as error approaches 0. - if (errorAngleAbs > GPS_RESCUE_RATE_SCALE_DEGREES) { - rescueYaw = GPS_RESCUE_MAX_YAW_RATE; - } else { - rescueYaw = (errorAngleAbs / GPS_RESCUE_RATE_SCALE_DEGREES) * GPS_RESCUE_MAX_YAW_RATE; - } - rescueYaw = (errorAngleAbs > GPS_RESCUE_RATE_SCALE_DEGREES) ? GPS_RESCUE_MAX_YAW_RATE : errorAngleAbs; - - rescueYaw *= errorAngleSign; + rescueYaw = -constrainf(errorAngle / GPS_RESCUE_RATE_SCALE_DEGREES * GPS_RESCUE_MAX_YAW_RATE, -GPS_RESCUE_MAX_YAW_RATE, GPS_RESCUE_MAX_YAW_RATE); } float gpsRescueGetYawRate(void) @@ -438,16 +429,11 @@ float gpsRescueGetYawRate(void) float gpsRescueGetThrottle(void) { - float commandedThrottle = 0; - const int minCheck = MAX(rxConfig()->mincheck, 1000); - + // Calculated a desired commanded throttle scaled from 0.0 to 1.0 for use in the mixer. // We need to compensate for min_check since the throttle value set by gps rescue // is based on the raw rcCommand value commanded by the pilot. - if (rescueThrottle > minCheck) { - // Calculated a desired commanded throttle scaled from 0.0 to 1.0 for use in the mixer. - commandedThrottle = (float)(rescueThrottle - minCheck) / (PWM_RANGE_MAX - minCheck); - commandedThrottle = constrainf(commandedThrottle, 0.0f, 1.0f); - } + float commandedThrottle = scaleRangef(rescueThrottle, MAX(rxConfig()->mincheck, PWM_RANGE_MIN), PWM_RANGE_MAX, 0.0f, 1.0f); + commandedThrottle = constrainf(commandedThrottle, 0.0f, 1.0f); return commandedThrottle; }