From bf5baf9f3128c367b1e586be8088a7ff0b2b6bde Mon Sep 17 00:00:00 2001
From: error414 <error414@error414.com>
Date: Mon, 7 Oct 2024 19:55:50 +0200
Subject: [PATCH] change gimbal sensitivity from programming framework

---
 docs/Programming Framework.md          |  1 +
 src/main/drivers/gimbal_common.c       |  5 +++++
 src/main/drivers/gimbal_common.h       |  7 ++++---
 src/main/fc/settings.yaml              |  3 +++
 src/main/programming/logic_condition.c | 11 +++++++++++
 src/main/programming/logic_condition.h |  3 ++-
 6 files changed, 26 insertions(+), 4 deletions(-)

diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md
index c2314dc627e..2eeb3fb3ad5 100644
--- a/docs/Programming Framework.md	
+++ b/docs/Programming Framework.md	
@@ -101,6 +101,7 @@ IPF can be edited using INAV Configurator user interface, or via CLI. To use COn
 | 52            | LED Pin PWM                   | Value `Operand A` from [`0` : `100`] starts PWM generation on LED Pin. See [LED pin PWM](LED%20pin%20PWM.md). Any other value stops PWM generation (stop to allow ws2812 LEDs updates in shared modes). |
 | 53            | Disable GPS Sensor Fix        | Disables the GNSS sensor fix. For testing GNSS failure. |
 | 54            | Mag calibration               | Trigger a magnetometer calibration. |
+| 55            | Set Gimbal Sensitivity        | Scales `Operand A` from [`-16` : `15`]
 
 ### Operands
 
diff --git a/src/main/drivers/gimbal_common.c b/src/main/drivers/gimbal_common.c
index e03e7fd9ddd..7660f82203c 100644
--- a/src/main/drivers/gimbal_common.c
+++ b/src/main/drivers/gimbal_common.c
@@ -89,6 +89,11 @@ bool gimbalCommonIsReady(gimbalDevice_t *gimbalDevice)
     return false;
 }
 
+void setGimbalSensitivity(int16_t sensitivity)
+{
+    gimbalConfigMutable()->sensitivity = sensitivity;
+}
+
 #ifdef GIMBAL_UNIT_TEST
 void taskUpdateGimbal(timeUs_t currentTimeUs)
 {
diff --git a/src/main/drivers/gimbal_common.h b/src/main/drivers/gimbal_common.h
index f79a0e8374c..495d0f752b0 100644
--- a/src/main/drivers/gimbal_common.h
+++ b/src/main/drivers/gimbal_common.h
@@ -62,9 +62,9 @@ typedef struct gimbalConfig_s {
     uint8_t tiltChannel;
     uint8_t rollChannel;
     int8_t sensitivity;
-    uint16_t panTrim;
-    uint16_t tiltTrim;
-    uint16_t rollTrim;
+    int16_t panTrim;
+    int16_t tiltTrim;
+    int16_t rollTrim;
 } gimbalConfig_t;
 
 PG_DECLARE(gimbalConfig_t, gimbalConfig);
@@ -96,6 +96,7 @@ bool gimbalCommonIsEnabled(void);
 bool gimbalCommonHtrkIsEnabled(void);
 
 int16_t gimbalCommonGetPanPwm(const gimbalDevice_t *gimbalDevice);
+void setGimbalSensitivity(int16_t sensitivity);
 
 #ifdef __cplusplus
 }
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 67331ae6a5b..1c50b5b0a86 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -4307,18 +4307,21 @@ groups:
         field: sensitivity
         min: -16
         max: 15
+        type: int8_t
       - name: gimbal_pan_trim
         field: panTrim
         description: "Trim gimbal pan center position."
         default_value: 0
         min: -500
         max: 500
+        type: int16_t
       - name: gimbal_tilt_trim
         field: tiltTrim
         description: "Trim gimbal tilt center position."
         default_value: 0
         min: -500
         max: 500
+        type: int16_t
       - name: gimbal_roll_trim
         field: rollTrim
         description: "Trim gimbal roll center position."
diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c
index 4143664616f..e95acf2c4b5 100644
--- a/src/main/programming/logic_condition.c
+++ b/src/main/programming/logic_condition.c
@@ -48,6 +48,7 @@
 #include "flight/pid.h"
 #include "flight/mixer_profile.h"
 #include "drivers/io_port_expander.h"
+#include "drivers/gimbal_common.h"
 #include "io/osd_common.h"
 #include "sensors/diagnostics.h"
 
@@ -342,6 +343,16 @@ static int logicConditionCompute(
             break;
         }
 #endif
+
+        case LOGIC_CONDITION_SET_GIMBAL_SENSITIVITY:
+#ifdef USE_SERIAL_GIMBAL
+            setGimbalSensitivity(constrain(operandA, SETTING_GIMBAL_SENSITIVITY_MIN, SETTING_GIMBAL_SENSITIVITY_MAX));
+            return true;
+            break;
+#else
+            return false;
+#endif
+
         case LOGIC_CONDITION_INVERT_ROLL:
             LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_ROLL);
             return true;
diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h
index f5653bb68cc..ad35ae9ec6a 100644
--- a/src/main/programming/logic_condition.h
+++ b/src/main/programming/logic_condition.h
@@ -84,7 +84,8 @@ typedef enum {
     LOGIC_CONDITION_LED_PIN_PWM                 = 52,
     LOGIC_CONDITION_DISABLE_GPS_FIX             = 53,
     LOGIC_CONDITION_RESET_MAG_CALIBRATION       = 54,
-    LOGIC_CONDITION_LAST                        = 55,
+    LOGIC_CONDITION_SET_GIMBAL_SENSITIVITY      = 55,
+    LOGIC_CONDITION_LAST                        = 56,
 } logicOperation_e;
 
 typedef enum logicOperandType_s {