From 792a32febb258e77ea2ab767cd8a3d3e8b44793c Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Sat, 26 May 2018 00:21:42 +0200 Subject: [PATCH] FW: Allow manual increase of throttle in auto controlled modes --- src/main/fc/settings.yaml | 3 +++ src/main/io/osd.c | 8 +++++--- src/main/navigation/navigation.c | 1 + src/main/navigation/navigation.h | 4 ++++ src/main/navigation/navigation_fixedwing.c | 21 +++++++++++++++++++++ 5 files changed, 34 insertions(+), 3 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index df6513a93d..d26e80ba7c 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1384,6 +1384,9 @@ groups: field: fw.cruise_yaw_rate min: 0 max: 60 + - name: nav_fw_allow_manual_thr_increase + field: fw.allow_manual_thr_increase + type: bool - name: PG_TELEMETRY_CONFIG type: telemetryConfig_t diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 9295b87940..2f5e2c2caa 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -750,7 +750,7 @@ static void osdCrosshairsBounds(uint8_t *x, uint8_t *y, uint8_t *length) * uses the THR value applied by the system rather than the * input value received by the sticks. **/ -static void osdFormatThrottlePosition(char *buff, bool autoThr) +static void osdFormatThrottlePosition(char *buff, bool autoThr, textAttributes_t *elemAttr) { buff[0] = SYM_THR; buff[1] = SYM_THR1; @@ -759,6 +759,8 @@ static void osdFormatThrottlePosition(char *buff, bool autoThr) buff[0] = SYM_AUTO_THR0; buff[1] = SYM_AUTO_THR1; thr = rcCommand[THROTTLE]; + if (isFixedWingAutoThrottleManuallyIncreased()) + TEXT_ATTRIBUTES_ADD_BLINK(*elemAttr); } tfp_sprintf(buff + 2, "%3d", (constrain(thr, PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN)); } @@ -1344,7 +1346,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_THROTTLE_POS: { - osdFormatThrottlePosition(buff, false); + osdFormatThrottlePosition(buff, false, NULL); break; } @@ -1736,7 +1738,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_THROTTLE_POS_AUTO_THR: { - osdFormatThrottlePosition(buff, true); + osdFormatThrottlePosition(buff, true, &elemAttr); break; } diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 181d66e7b1..bd26ec6125 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -147,6 +147,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .launch_climb_angle = 18, // 18 degrees .launch_max_angle = 45, // 45 deg .cruise_yaw_rate = 20, // 20dps + .allow_manual_thr_increase = false } ); diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 33f83e8bbd..511cb46e7b 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -31,6 +31,8 @@ extern gpsLocation_t GPS_home; extern uint16_t GPS_distanceToHome; // distance to home point in meters extern int16_t GPS_directionToHome; // direction to home point in degrees +extern bool autoThrottleManuallyIncreased; + /* Navigation system updates */ void onNewGPSData(void); @@ -171,6 +173,7 @@ typedef struct navConfig_s { uint8_t launch_climb_angle; // Target climb angle for launch (deg) uint8_t launch_max_angle; // Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg] uint8_t cruise_yaw_rate; // Max yaw rate (dps) when CRUISE MODE is enabled + bool allow_manual_thr_increase; } fw; } navConfig_t; @@ -335,6 +338,7 @@ rthState_e getStateOfForcedRTH(void); /* Getter functions which return data about the state of the navigation system */ bool navigationIsControllingThrottle(void); +bool isFixedWingAutoThrottleManuallyIncreased(void); bool navigationIsFlyingAutonomousMode(void); /* Returns true iff navConfig()->general.flags.rth_allow_landing is NAV_RTH_ALLOW_LANDING_ALWAYS * or if it's NAV_RTH_ALLOW_LANDING_FAILSAFE and failsafe mode is active. diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 67330e6c38..e82913975b 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -48,6 +48,9 @@ #include "navigation/navigation.h" #include "navigation/navigation_private.h" +#include "rx/rx.h" + + // If we are going slower than NAV_FW_MIN_VEL_SPEED_BOOST - boost throttle to fight against the wind #define NAV_FW_THROTTLE_SPEED_BOOST_GAIN 1.5f #define NAV_FW_MIN_VEL_SPEED_BOOST 700.0f // 7 m/s @@ -58,6 +61,7 @@ static bool isPitchAdjustmentValid = false; static bool isRollAdjustmentValid = false; static float throttleSpeedAdjustment = 0; +static bool isAutoThrottleManuallyIncreased = false; /*----------------------------------------------------------- @@ -450,6 +454,18 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat } uint16_t correctedThrottleValue = constrain(navConfig()->fw.cruise_throttle + throttleCorrection, navConfig()->fw.min_throttle, navConfig()->fw.max_throttle); + + // Manual throttle increase + if (navConfig()->fw.allow_manual_thr_increase && !FLIGHT_MODE(FAILSAFE_MODE)) { + if (rcCommand[THROTTLE] < PWM_RANGE_MIN + (PWM_RANGE_MAX - PWM_RANGE_MIN) * 0.95) + correctedThrottleValue += MAX(0, rcCommand[THROTTLE] - navConfig()->fw.cruise_throttle); + else + correctedThrottleValue = motorConfig()->maxthrottle; + isAutoThrottleManuallyIncreased = (rcCommand[THROTTLE] > navConfig()->fw.cruise_throttle); + } else { + isAutoThrottleManuallyIncreased = false; + } + rcCommand[THROTTLE] = constrain(correctedThrottleValue, motorConfig()->minthrottle, motorConfig()->maxthrottle); } @@ -476,6 +492,11 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat #endif } +bool isFixedWingAutoThrottleManuallyIncreased() +{ + return isAutoThrottleManuallyIncreased; +} + /*----------------------------------------------------------- * FixedWing land detector *-----------------------------------------------------------*/