Take gimbal input into account for pan servo compensation and center

indication
iNavFlight · Jun 21, 2024 · 6b574df · 6b574df
1 parent ea1c941
commit 6b574df
Show file tree

Hide file tree

Showing 5 changed files with 39 additions and 3 deletions.
diff --git a/src/main/drivers/gimbal_common.c b/src/main/drivers/gimbal_common.c
@@ -31,6 +31,7 @@
 #include "fc/cli.h"
 
 #include "drivers/gimbal_common.h"
+#include "rx/rx.h"
 
 #include "settings_generated.h"
 
@@ -125,4 +126,14 @@ bool gimbalCommonHtrkIsEnabled(void)
     return false;
 }
 
+
+int16_t gimbalCommonGetPanPwm(const gimbalDevice_t *gimbalDevice)
+{
+    if (gimbalDevice && gimbalDevice->vTable->getGimbalPanPWM) {
+        return gimbalDevice->vTable->getGimbalPanPWM(gimbalDevice);
+    }
+
+    return gimbalDevice ? gimbalDevice->currentPanPWM : PWM_RANGE_MIDDLE;
+}
+
 #endif
diff --git a/src/main/drivers/gimbal_common.h b/src/main/drivers/gimbal_common.h
@@ -41,6 +41,7 @@ struct gimbalVTable_s;
 
 typedef struct gimbalDevice_s {
     const struct gimbalVTable_s *vTable;
+    int16_t currentPanPWM;
 } gimbalDevice_t;
 
 // {set,get}BandAndChannel: band and channel are 1 origin
@@ -52,6 +53,7 @@ typedef struct gimbalVTable_s {
     gimbalDevType_e (*getDeviceType)(const gimbalDevice_t *gimbalDevice);
     bool (*isReady)(const gimbalDevice_t *gimbalDevice);
     bool (*hasHeadTracker)(const gimbalDevice_t *gimbalDevice);
+    int16_t (*getGimbalPanPWM)(const gimbalDevice_t *gimbalDevice);
 } gimbalVTable_t;
 
 
@@ -93,6 +95,8 @@ void taskUpdateGimbal(timeUs_t currentTimeUs);
 bool gimbalCommonIsEnabled(void);
 bool gimbalCommonHtrkIsEnabled(void);
 
+int16_t gimbalCommonGetPanPwm(const gimbalDevice_t *gimbalDevice);
+
 #ifdef __cplusplus
 }
 #endif

diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c
@@ -78,7 +78,8 @@ gimbalVTable_t gimbalSerialVTable = {
 };
 
 static gimbalDevice_t serialGimbalDevice = {
-    .vTable = &gimbalSerialVTable
+    .vTable = &gimbalSerialVTable,
+    .currentPanPWM = PWM_RANGE_MIDDLE
 };
 
 #if (defined(USE_HEADTRACKER) && defined(USE_HEADTRACKER_SERIAL))
@@ -234,6 +235,7 @@ void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
             attitude.pan = headTrackerCommonGetPan(dev);
             attitude.tilt = headTrackerCommonGetTilt(dev);
             attitude.roll = headTrackerCommonGetRoll(dev);
+
             DEBUG_SET(DEBUG_HEADTRACKING, 4, 1);
         } else {
             attitude.pan = constrain(gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, PWM_RANGE_MIDDLE + cfg->panTrim), PWM_RANGE_MIN, PWM_RANGE_MAX);
@@ -267,12 +269,22 @@ void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
     attitude.crch = (crc16 >> 8) & 0xFF;
     attitude.crcl = crc16 & 0xFF;
 
+    serialGimbalDevice.currentPanPWM = gimbal2pwm(attitude.pan);
+
     serialBeginWrite(gimbalPort);
     serialWriteBuf(gimbalPort, (uint8_t *)&attitude, sizeof(gimbalHtkAttitudePkt_t));
     serialEndWrite(gimbalPort);
 }
 #endif
 
+int16_t gimbal2pwm(int16_t value)
+{
+    int16_t ret = 0;
+    ret = scaleRange(value, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX, PWM_RANGE_MIN, PWM_RANGE_MAX);
+    return ret;
+}
+
+
 int16_t gimbal_scale12(int16_t inputMin, int16_t inputMax, int16_t value)
 {
     int16_t ret = 0;

diff --git a/src/main/io/gimbal_serial.h b/src/main/io/gimbal_serial.h
@@ -70,9 +70,10 @@ typedef struct gimbalSerialConfig_s {
 
 PG_DECLARE(gimbalSerialConfig_t, gimbalSerialConfig);
 
-
 int16_t gimbal_scale12(int16_t inputMin, int16_t inputMax, int16_t value);
 
+int16_t gimbal2pwm(int16_t value);
+
 bool gimbalSerialInit(void);
 bool gimbalSerialDetect(void);
 void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime);

diff --git a/src/main/io/osd.c b/src/main/io/osd.c
@@ -62,6 +62,7 @@
 #include "drivers/osd_symbols.h"
 #include "drivers/time.h"
 #include "drivers/vtx_common.h"
+#include "drivers/gimbal_common.h"
 
 #include "io/adsb.h"
 #include "io/flashfs.h"
@@ -1203,8 +1204,15 @@ int16_t osdGetHeading(void)
 int16_t osdGetPanServoOffset(void)
 {
     int8_t servoIndex = osdConfig()->pan_servo_index;
-    int16_t servoPosition = servo[servoIndex];
     int16_t servoMiddle = servoParams(servoIndex)->middle;
+    int16_t servoPosition = servo[servoIndex];
+
+    gimbalDevice_t *dev = gimbalCommonDevice();
+    if (dev && gimbalCommonIsReady(dev)) {
+        servoPosition = gimbalCommonGetPanPwm(dev);
+        servoMiddle = PWM_RANGE_MIDDLE;
+    }
+
     return (int16_t)CENTIDEGREES_TO_DEGREES((servoPosition - servoMiddle) * osdConfig()->pan_servo_pwm2centideg);
 }