From 2e9bf6b6e7038896d4bd6ad29dfa4767a8cd581d Mon Sep 17 00:00:00 2001
From: Darren Lines <darren@darrenlines.uk>
Date: Wed, 11 May 2022 18:13:13 +0100
Subject: [PATCH] Added FW level trim to in flight adjustments

---
 docs/Inflight Adjustments.md |  1 +
 src/main/fc/rc_adjustments.c | 13 +++++++++++++
 src/main/fc/rc_adjustments.h |  1 +
 src/main/io/osd.c            | 20 +++++---------------
 src/main/io/osd.h            |  1 +
 5 files changed, 21 insertions(+), 15 deletions(-)

diff --git a/docs/Inflight Adjustments.md b/docs/Inflight Adjustments.md
index 0c216f8fc5..37102cebf0 100644
--- a/docs/Inflight Adjustments.md	
+++ b/docs/Inflight Adjustments.md	
@@ -159,6 +159,7 @@ this reason ensure that you define enough ranges to cover the range channel's us
 | 55    | TPA_BREAKPOINT |
 | 56    | NAV_FW_CONTROL_SMOOTHNESS |
 | 57    | FW_TPA_TIME_CONSTANT |
+| 58    | FW_LEVEL_TRIM |
 
 ## Examples
 
diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c
index 70e37a0651..0081953bc8 100644
--- a/src/main/fc/rc_adjustments.c
+++ b/src/main/fc/rc_adjustments.c
@@ -293,6 +293,10 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU
         .adjustmentFunction = ADJUSTMENT_FW_TPA_TIME_CONSTANT,
         .mode = ADJUSTMENT_MODE_STEP,
         .data = { .stepConfig = { .step = 5 }}
+    }, {
+        .adjustmentFunction = ADJUSTMENT_FW_LEVEL_TRIM,
+        .mode = ADJUSTMENT_MODE_STEP,
+        .data = { .stepConfig = { .step = 1 }}
     }
 };
 
@@ -607,6 +611,15 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
         case ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS:
             applyAdjustmentU8(ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS, &navConfigMutable()->fw.control_smoothness, delta, SETTING_NAV_FW_CONTROL_SMOOTHNESS_MIN, SETTING_NAV_FW_CONTROL_SMOOTHNESS_MAX);
             break;
+        case ADJUSTMENT_FW_LEVEL_TRIM:
+        {
+            float newValue = pidProfileMutable()->fixedWingLevelTrim + (delta / 10.0f);
+            if (newValue > SETTING_FW_LEVEL_PITCH_TRIM_MAX) {newValue = (float)SETTING_FW_LEVEL_PITCH_TRIM_MAX;}
+            else if (newValue < SETTING_FW_LEVEL_PITCH_TRIM_MIN) {newValue = (float)SETTING_FW_LEVEL_PITCH_TRIM_MIN;}
+            pidProfileMutable()->fixedWingLevelTrim = newValue;
+            blackboxLogInflightAdjustmentEvent(ADJUSTMENT_FW_LEVEL_TRIM, (int)(newValue * 10.0f));
+            break;
+        }
         default:
             break;
     };
diff --git a/src/main/fc/rc_adjustments.h b/src/main/fc/rc_adjustments.h
index 21bd1b8c97..30f25340bd 100644
--- a/src/main/fc/rc_adjustments.h
+++ b/src/main/fc/rc_adjustments.h
@@ -83,6 +83,7 @@ typedef enum {
     ADJUSTMENT_TPA_BREAKPOINT                   = 55,
     ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS        = 56,
     ADJUSTMENT_FW_TPA_TIME_CONSTANT             = 57,
+    ADJUSTMENT_FW_LEVEL_TRIM                    = 58,
     ADJUSTMENT_FUNCTION_COUNT // must be last
 } adjustmentFunction_e;
 
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 8dd32d4b6c..9ddd7d1ee0 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -3019,24 +3019,14 @@ static bool osdDrawSingleElement(uint8_t item)
         }
     case OSD_TPA_TIME_CONSTANT:
         {
-           /* char buff[7];
-            textAttributes_t attr;
-
- if (isAdjustmentFunctionSelected(ADJUSTMENT_FW_TPA_TIME_CONSTANT)) {
-            displayWrite(osdDisplayPort, elemPosX, elemPosY, "TC OK");
- } else {
-     displayWrite(osdDisplayPort, elemPosX, elemPosY, "TPA TC");
- }
-            
-            attr = TEXT_ATTRIBUTES_NONE;
-            tfp_sprintf(buff, "%4d", currentControlRateProfile->throttle.fixedWingTauMs);
-            if (isAdjustmentFunctionSelected(ADJUSTMENT_FW_TPA_TIME_CONSTANT)) {
-                TEXT_ATTRIBUTES_ADD_BLINK(attr);
-            }
-            displayWriteWithAttr(osdDisplayPort, elemPosX + 7, elemPosY, buff, attr);*/
             osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "TPA TC", 0, currentControlRateProfile->throttle.fixedWingTauMs, 4, 0, ADJUSTMENT_FW_TPA_TIME_CONSTANT);
             return true;
         }
+    case OSD_FW_LEVEL_TRIM:
+        {
+            osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "LEVEL", 0, pidProfileMutable()->fixedWingLevelTrim, 3, 1, ADJUSTMENT_FW_LEVEL_TRIM);
+            return true;
+        }
 
     case OSD_NAV_FW_CONTROL_SMOOTHNESS:
         osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "CTL S", 0, navConfig()->fw.control_smoothness, 1, 0, ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS);
diff --git a/src/main/io/osd.h b/src/main/io/osd.h
index 5021496d09..cbe64969aa 100644
--- a/src/main/io/osd.h
+++ b/src/main/io/osd.h
@@ -258,6 +258,7 @@ typedef enum {
     OSD_SWITCH_INDICATOR_2,
     OSD_SWITCH_INDICATOR_3,
     OSD_TPA_TIME_CONSTANT,
+    OSD_FW_LEVEL_TRIM,
     OSD_ITEM_COUNT // MUST BE LAST
 } osd_items_e;