Further serial gimbal/headtracker improvements

docs/Settings.md

@@ -1472,6 +1472,16 @@ Gimbal pan rc channel index. 0 is no channel.
 
 ---
 
+### gimbal_pan_trim
+
+Trim gimbal pan center position.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | -500 | 500 |
+
+---
+
 ### gimbal_roll_channel
 
 Gimbal roll rc channel index. 0 is no channel.
@@ -1482,6 +1492,16 @@ Gimbal roll rc channel index. 0 is no channel.
 
 ---
 
+### gimbal_roll_trim
+
+Trim gimbal roll center position.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | -500 | 500 |
+
+---
+
 ### gimbal_sensitivity
 
 Gimbal sensitivity is similar to gain and will affect how quickly the gimbal will react.
@@ -1512,6 +1532,16 @@ Gimbal tilt rc channel index. 0 is no channel.
 
 ---
 
+### gimbal_tilt_trim
+
+Trim gimbal tilt center position.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | -500 | 500 |
+
+---
+
 ### gps_auto_baud
 
 Automatic configuration of GPS baudrate(The specified baudrate in configured in ports will be used) when used with UBLOX GPS

src/main/common/log.c

@@ -20,11 +20,12 @@
 #include <stdarg.h>
 #include <ctype.h>
 
-#if defined(SEMIHOSTING)
+#if defined(SEMIHOSTING) || defined(SITL_BUILD)
 #include <stdio.h>
 #endif
 
 #include "build/version.h"
+#include "build/debug.h"
 
 #include "drivers/serial.h"
 #include "drivers/time.h"
@@ -125,6 +126,7 @@ static void logPrint(const char *buf, size_t size)
         fputc(buf[ii], stdout);
     }
 #endif
+    SD(printf("%s\n", buf));
     if (logPort) {
         // Send data via UART (if configured & connected - a safeguard against zombie VCP)
         if (serialIsConnected(logPort)) {

src/main/drivers/gimbal_common.c

@@ -31,10 +31,22 @@
 #include "fc/cli.h"
 
 #include "drivers/gimbal_common.h"
+#include "rx/rx.h"
 
+#include "settings_generated.h"
 
-PG_REGISTER(gimbalConfig_t, gimbalConfig, PG_GIMBAL_CONFIG, 0);
 
+PG_REGISTER_WITH_RESET_TEMPLATE(gimbalConfig_t, gimbalConfig, PG_GIMBAL_CONFIG, 1);
+
+PG_RESET_TEMPLATE(gimbalConfig_t, gimbalConfig, 
+    .panChannel = SETTING_GIMBAL_PAN_CHANNEL_DEFAULT,
+    .tiltChannel = SETTING_GIMBAL_TILT_CHANNEL_DEFAULT,
+    .rollChannel = SETTING_GIMBAL_ROLL_CHANNEL_DEFAULT,
+    .sensitivity = SETTING_GIMBAL_SENSITIVITY_DEFAULT,
+    .panTrim = SETTING_GIMBAL_PAN_TRIM_DEFAULT,
+    .tiltTrim = SETTING_GIMBAL_TILT_TRIM_DEFAULT,
+    .rollTrim = SETTING_GIMBAL_ROLL_TRIM_DEFAULT
+);
 
 static gimbalDevice_t *commonGimbalDevice = NULL;
 
@@ -114,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 + gimbalConfig()->panTrim;
+}
+
 #endif

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;
 
 
@@ -60,6 +62,9 @@ typedef struct gimbalConfig_s {
     uint8_t tiltChannel;
     uint8_t rollChannel;
     uint8_t sensitivity;
+    uint16_t panTrim;
+    uint16_t tiltTrim;
+    uint16_t rollTrim;
 } gimbalConfig_t;
 
 PG_DECLARE(gimbalConfig_t, gimbalConfig);
@@ -90,6 +95,8 @@ void taskUpdateGimbal(timeUs_t currentTimeUs);
 bool gimbalCommonIsEnabled(void);
 bool gimbalCommonHtrkIsEnabled(void);
 
+int16_t gimbalCommonGetPanPwm(const gimbalDevice_t *gimbalDevice);
+
 #ifdef __cplusplus
 }
 #endif

src/main/drivers/headtracker_common.h

@@ -19,6 +19,10 @@
 
 #include "platform.h"
 
+#define MAX_HEADTRACKER_DATA_AGE_US HZ2US(25)
+#define HEADTRACKER_RANGE_MIN   -2048
+#define HEADTRACKER_RANGE_MAX   2047
+
 #ifdef USE_HEADTRACKER
 
 #include <stdint.h>
@@ -29,9 +33,6 @@
 
 #include "config/feature.h"
 
-#define MAX_HEADTRACKER_DATA_AGE_US HZ2US(25)
-#define HEADTRACKER_RANGE_MIN   -2048
-#define HEADTRACKER_RANGE_MAX   2047
 
 #ifdef __cplusplus
 extern "C" {

src/main/fc/settings.yaml

@@ -4236,6 +4236,24 @@ groups:
         field: sensitivity
         min: -16
         max: 15
+      - name: gimbal_pan_trim
+        field: panTrim
+        description: "Trim gimbal pan center position."
+        default_value: 0
+        min: -500
+        max: 500
+      - name: gimbal_tilt_trim
+        field: tiltTrim
+        description: "Trim gimbal tilt center position."
+        default_value: 0
+        min: -500
+        max: 500
+      - name: gimbal_roll_trim
+        field: rollTrim
+        description: "Trim gimbal roll center position."
+        default_value: 0
+        min: -500
+        max: 500
   - name: PG_GIMBAL_SERIAL_CONFIG
     type: gimbalSerialConfig_t
     headers: ["io/gimbal_serial.h"]

src/main/io/gimbal_serial.c

@@ -60,6 +60,7 @@ static volatile uint8_t txBuffer[GIMBAL_SERIAL_BUFFER_SIZE];
 #if defined(USE_HEADTRACKER) && defined(USE_HEADTRACKER_SERIAL)
 static gimbalSerialHtrkState_t headTrackerState = { 
     .payloadSize = 0,
+    .attitude = {},
     .state = WAITING_HDR1,
 };
 #endif
@@ -78,7 +79,8 @@ gimbalVTable_t gimbalSerialVTable = {
 };
 
 static gimbalDevice_t serialGimbalDevice = {
-    .vTable = &gimbalSerialVTable
+    .vTable = &gimbalSerialVTable,
+    .currentPanPWM = PWM_RANGE_MIDDLE
 };
 
 #if (defined(USE_HEADTRACKER) && defined(USE_HEADTRACKER_SERIAL))
@@ -188,14 +190,17 @@ void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
 
     gimbalHtkAttitudePkt_t attitude = {
         .sync = {HTKATTITUDE_SYNC0, HTKATTITUDE_SYNC1},
-        .mode = GIMBAL_MODE_DEFAULT
+        .mode = GIMBAL_MODE_DEFAULT,
+        .pan = 0,
+        .tilt = 0,
+        .roll = 0
     };
 
     const gimbalConfig_t *cfg = gimbalConfig();
 
-    int pan = PWM_RANGE_MIDDLE;
-    int tilt = PWM_RANGE_MIDDLE;
-    int roll = PWM_RANGE_MIDDLE;
+    int panPWM = PWM_RANGE_MIDDLE + cfg->panTrim;
+    int tiltPWM = PWM_RANGE_MIDDLE + cfg->tiltTrim;
+    int rollPWM = PWM_RANGE_MIDDLE + cfg->rollTrim;
 
     if (IS_RC_MODE_ACTIVE(BOXGIMBALTLOCK)) {
         attitude.mode |= GIMBAL_MODE_TILT_LOCK;
@@ -212,18 +217,18 @@ void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
 
     if (rxAreFlightChannelsValid() && !IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)) {
         if (cfg->panChannel > 0) {
-            pan = rxGetChannelValue(cfg->panChannel - 1);
-            pan = constrain(pan, PWM_RANGE_MIN, PWM_RANGE_MAX);
+            panPWM = rxGetChannelValue(cfg->panChannel - 1) + cfg->panTrim;
+            panPWM = constrain(panPWM, PWM_RANGE_MIN, PWM_RANGE_MAX);
         }
 
         if (cfg->tiltChannel > 0) {
-            tilt = rxGetChannelValue(cfg->tiltChannel - 1);
-            tilt = constrain(tilt, PWM_RANGE_MIN, PWM_RANGE_MAX);
+            tiltPWM = rxGetChannelValue(cfg->tiltChannel - 1) + cfg->tiltTrim;
+            tiltPWM = constrain(tiltPWM, PWM_RANGE_MIN, PWM_RANGE_MAX);
         }
 
         if (cfg->rollChannel > 0) {
-            roll = rxGetChannelValue(cfg->rollChannel - 1);
-            roll = constrain(roll, PWM_RANGE_MIN, PWM_RANGE_MAX);
+            rollPWM = rxGetChannelValue(cfg->rollChannel - 1) + cfg->rollTrim;
+            rollPWM = constrain(rollPWM, PWM_RANGE_MIN, PWM_RANGE_MAX);
         }
     }
 
@@ -234,11 +239,12 @@ 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 = 0;
-            attitude.tilt = 0;
-            attitude.roll = 0;
+            attitude.pan = constrain(gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, PWM_RANGE_MIDDLE + cfg->panTrim), HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX);
+            attitude.tilt = constrain(gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, PWM_RANGE_MIDDLE + cfg->tiltTrim), HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX);
+            attitude.roll = constrain(gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, PWM_RANGE_MIDDLE + cfg->rollTrim), HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX);
             DEBUG_SET(DEBUG_HEADTRACKING, 4, -1);
         }
     } else {
@@ -248,9 +254,9 @@ void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
         DEBUG_SET(DEBUG_HEADTRACKING, 4, 0);
         // Radio endpoints may need to be adjusted, as it seems ot go a bit
         // bananas at the extremes
-        attitude.pan = gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, pan);
-        attitude.tilt = gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, tilt);
-        attitude.roll = gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, roll);
+        attitude.pan = gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, panPWM);
+        attitude.tilt = gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, tiltPWM);
+        attitude.roll = gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, rollPWM);
     }
 
     DEBUG_SET(DEBUG_HEADTRACKING, 5, attitude.pan);
@@ -267,16 +273,26 @@ 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;
-    ret = scaleRange(value, inputMin, inputMax, -2048, 2047);
+    ret = scaleRange(value, inputMin, inputMax, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX);
     return ret;
 }
 

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);

src/main/io/headtracker_msp.c

@@ -30,18 +30,6 @@
 
 #include "io/headtracker_msp.h"
 
-/*
-typedef struct headTrackerVTable_s {
-    void (*process)(headTrackerDevice_t *headTrackerDevice, timeUs_t currentTimeUs);
-    headTrackerDevType_e (*getDeviceType)(const headTrackerDevice_t *headTrackerDevice);
-    bool (*isReady)(const headTrackerDevice_t *headTrackerDevice);
-    bool (*isValid)(const headTrackerDevice_t *headTrackerDevice);
-    int (*getPanPWM)(const headTrackerDevice_t *headTrackerDevice);
-    int (*getTiltPWM)(const headTrackerDevice_t *headTrackerDevice);
-    int (*getRollPWM)(const headTrackerDevice_t *headTrackerDevice);
-} headtrackerVTable_t;
-*/
-
 static headTrackerVTable_t headTrackerMspVTable = {
     .process = NULL,
     .getDeviceType = heatTrackerMspGetDeviceType,
@@ -78,9 +66,6 @@ void mspHeadTrackerReceiverNewData(uint8_t *data, int dataSize)
     headTrackerMspDevice.roll = constrain(status->roll, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX);
     headTrackerMspDevice.expires = micros() + MAX_HEADTRACKER_DATA_AGE_US;
 
-    SD(fprintf(stderr, "[headTracker]: pan: %d tilt: %d roll: %d\n", status->pan, status->tilt, status->roll));
-    SD(fprintf(stderr, "[headTracker]: scaled pan: %d tilt: %d roll: %d\n", headTrackerMspDevice.pan, headTrackerMspDevice.tilt, headTrackerMspDevice.roll));
-
     UNUSED(status);
 }
 

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 + gimbalConfig()->panTrim;
+    }
+
     return (int16_t)CENTIDEGREES_TO_DEGREES((servoPosition - servoMiddle) * osdConfig()->pan_servo_pwm2centideg);
 }
 

src/test/unit/gimbal_serial_unittest.cc

@@ -24,6 +24,7 @@
 #include "unittest_macros.h"
 
 #include "io/gimbal_serial.h"
+#include "drivers/headtracker_common.h"
 
 void dumpMemory(uint8_t *mem, int size)
 {