Merge pull request #3278 from shellixyz/manual_throttle_increase_in_a…

…uto_throttle_modes_clean

FW: Allow manual throttle increase in auto controlled modes

src/main/fc/settings.yaml

@@ -1388,6 +1388,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

src/main/io/osd.c

@@ -751,7 +751,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;
@@ -760,6 +760,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));
 }
@@ -1345,7 +1347,7 @@ static bool osdDrawSingleElement(uint8_t item)
 
  case OSD_THROTTLE_POS:
  {
- osdFormatThrottlePosition(buff, false);
+ osdFormatThrottlePosition(buff, false, NULL);
  break;
  }
 
@@ -1794,7 +1796,7 @@ static bool osdDrawSingleElement(uint8_t item)
 
  case OSD_THROTTLE_POS_AUTO_THR:
  {
- osdFormatThrottlePosition(buff, true);
+ osdFormatThrottlePosition(buff, true, &elemAttr);
  break;
  }
 

src/main/navigation/navigation.c

@@ -148,6 +148,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
  }
 );
 

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.

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
  *-----------------------------------------------------------*/