From 5ed1bbb2a9c8fb163475094e24e401d57d1c8876 Mon Sep 17 00:00:00 2001
From: Bruce Luckcuck <github@etracer.net>
Date: Sat, 8 Dec 2018 12:58:06 -0500
Subject: [PATCH] Fix OSD throttle position element calculation and add 3D
 support Previously the Throttle Position element calculation was based solely
 on the raw rcCommand value and didn't take into account the min_check
 deadzone so the resulting displayed percentage was incorrect. Also 3D trottle
 handling was not considered.

Corrected the calculation and added support for 3D throttle modes. Reversed thrust will be represented with negative throttle percentages.
---
 src/main/fc/core.c            | 34 ++++++++++++++++++++++------------
 src/main/fc/core.h            |  4 ++--
 src/main/io/osd.c             |  2 +-
 src/test/unit/osd_unittest.cc |  1 +
 4 files changed, 26 insertions(+), 15 deletions(-)

diff --git a/src/main/fc/core.c b/src/main/fc/core.c
index 02236a25b6..7fd4ea661c 100644
--- a/src/main/fc/core.c
+++ b/src/main/fc/core.c
@@ -570,28 +570,38 @@ void runawayTakeoffTemporaryDisable(uint8_t disableFlag)
 
 
 // calculate the throttle stick percent - integer math is good enough here.
-uint8_t calculateThrottlePercent(void)
+// returns negative values for reversed thrust in 3D mode
+int8_t calculateThrottlePercent(void)
 {
     uint8_t ret = 0;
+    int channelData = constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX);
+
     if (featureIsEnabled(FEATURE_3D)
         && !IS_RC_MODE_ACTIVE(BOX3D)
         && !flight3DConfig()->switched_mode3d) {
-
-        if ((rcData[THROTTLE] >= PWM_RANGE_MAX) || (rcData[THROTTLE] <= PWM_RANGE_MIN)) {
-            ret = 100;
-        } else {
-            if (rcData[THROTTLE] > (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle)) {
-                ret = ((rcData[THROTTLE] - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle) * 100) / (PWM_RANGE_MAX - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle);
-            } else if (rcData[THROTTLE] < (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle)) {
-                ret = ((rxConfig()->midrc - flight3DConfig()->deadband3d_throttle - rcData[THROTTLE]) * 100) / (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle - PWM_RANGE_MIN);
-            }
+        
+        if (channelData > (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle)) {
+            ret = ((channelData - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle) * 100) / (PWM_RANGE_MAX - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle);
+        } else if (channelData < (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle)) {
+            ret = -((rxConfig()->midrc - flight3DConfig()->deadband3d_throttle - channelData) * 100) / (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle - PWM_RANGE_MIN);
         }
     } else {
-        ret = constrain(((rcData[THROTTLE] - rxConfig()->mincheck) * 100) / (PWM_RANGE_MAX - rxConfig()->mincheck), 0, 100);
+        ret = constrain(((channelData - rxConfig()->mincheck) * 100) / (PWM_RANGE_MAX - rxConfig()->mincheck), 0, 100);
+        if (featureIsEnabled(FEATURE_3D)
+            && IS_RC_MODE_ACTIVE(BOX3D)
+            && flight3DConfig()->switched_mode3d) {
+
+            ret = -ret;  // 3D on a switch is active
+        }
     }
     return ret;
 }
 
+uint8_t calculateThrottlePercentAbs(void)
+{
+    return ABS(calculateThrottlePercent());
+}
+
 static bool airmodeIsActivated;
 
 bool isAirmodeActivated()
@@ -629,7 +639,7 @@ bool processRx(timeUs_t currentTimeUs)
     failsafeUpdateState();
 
     const throttleStatus_e throttleStatus = calculateThrottleStatus();
-    const uint8_t throttlePercent = calculateThrottlePercent();
+    const uint8_t throttlePercent = calculateThrottlePercentAbs();
     
     const bool launchControlActive = isLaunchControlActive();
 
diff --git a/src/main/fc/core.h b/src/main/fc/core.h
index 6d3aa646f7..2a1f1913d2 100644
--- a/src/main/fc/core.h
+++ b/src/main/fc/core.h
@@ -68,7 +68,8 @@ void updateArmingStatus(void);
 void taskMainPidLoop(timeUs_t currentTimeUs);
 
 bool isFlipOverAfterCrashActive(void);
-
+int8_t calculateThrottlePercent(void);
+uint8_t calculateThrottlePercentAbs(void);
 void runawayTakeoffTemporaryDisable(uint8_t disableFlag);
 bool isAirmodeActivated();
 timeUs_t getLastDisarmTimeUs(void);
@@ -78,4 +79,3 @@ void resetTryingToArm();
 void subTaskTelemetryPollSensors(timeUs_t currentTimeUs);
 
 bool isLaunchControlActive(void);
-
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index fb6b4a9923..977b4d4b6e 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -764,7 +764,7 @@ static bool osdDrawSingleElement(uint8_t item)
     case OSD_THROTTLE_POS:
         buff[0] = SYM_THR;
         buff[1] = SYM_THR1;
-        tfp_sprintf(buff + 2, "%3d", (constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN));
+        tfp_sprintf(buff + 2, "%3d", calculateThrottlePercent());
         break;
 
 #if defined(USE_VTX_COMMON)
diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc
index 7faf227c54..1161c2bae1 100644
--- a/src/test/unit/osd_unittest.cc
+++ b/src/test/unit/osd_unittest.cc
@@ -1079,4 +1079,5 @@ extern "C" {
     bool pidOsdAntiGravityActive(void) { return false; }
     bool failsafeIsActive(void) { return false; }
     bool gpsRescueIsConfigured(void) { return false; }
+    int8_t calculateThrottlePercent(void) { return 0; }
 }