diff --git a/docs/Settings.md b/docs/Settings.md index 102f520b80..cd58bb0cfb 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1822,6 +1822,36 @@ This setting limits yaw rotation rate that HEADING_HOLD controller can request f --- +### headtracker_pan_ratio + +Head pan movement vs camera movement ratio + +| Default | Min | Max | +| --- | --- | --- | +| 1 | 0 | 3 | + +--- + +### headtracker_roll_ratio + +Head roll movement vs camera movement ratio + +| Default | Min | Max | +| --- | --- | --- | +| 1 | 0 | 3 | + +--- + +### headtracker_tilt_ratio + +Head tilt movement vs camera movement ratio + +| Default | Min | Max | +| --- | --- | --- | +| 1 | 0 | 3 | + +--- + ### headtracker_type Type of headtrackr dervice diff --git a/src/main/drivers/headtracker_common.c b/src/main/drivers/headtracker_common.c index 670a412c50..84484cfb00 100644 --- a/src/main/drivers/headtracker_common.c +++ b/src/main/drivers/headtracker_common.c @@ -40,7 +40,7 @@ #include "drivers/headtracker_common.h" -PG_REGISTER_WITH_RESET_TEMPLATE(headTrackerConfig_t, headTrackerConfig, PG_HEADTRACKER_CONFIG, 0); +PG_REGISTER_WITH_RESET_TEMPLATE(headTrackerConfig_t, headTrackerConfig, PG_HEADTRACKER_CONFIG, 1); PG_RESET_TEMPLATE(headTrackerConfig_t, headTrackerConfig, .devType = SETTING_HEADTRACKER_TYPE_DEFAULT diff --git a/src/main/drivers/headtracker_common.h b/src/main/drivers/headtracker_common.h index 092a00ab0a..f8b894f912 100644 --- a/src/main/drivers/headtracker_common.h +++ b/src/main/drivers/headtracker_common.h @@ -70,6 +70,9 @@ typedef struct headTrackerVTable_s { typedef struct headTrackerConfig_s { headTrackerDevType_e devType; + float pan_ratio; + float tilt_ratio; + float roll_ratio; } headTrackerConfig_t; PG_DECLARE(headTrackerConfig_t, headTrackerConfig); diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 66d2b098f4..0cd859462e 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4251,3 +4251,24 @@ groups: field: devType type: uint8_t table: headtracker_dev_type + - name: headtracker_pan_ratio + description: "Head pan movement vs camera movement ratio" + type: float + default_value: 1 + field: tilt_ratio + min: 0 + max: 3 + - name: headtracker_tilt_ratio + description: "Head tilt movement vs camera movement ratio" + type: float + default_value: 1 + field: tilt_ratio + min: 0 + max: 3 + - name: headtracker_roll_ratio + description: "Head roll movement vs camera movement ratio" + type: float + default_value: 1 + field: tilt_ratio + min: 0 + max: 3 \ No newline at end of file diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c index 90216ea8c6..f73f6759bf 100644 --- a/src/main/io/gimbal_serial.c +++ b/src/main/io/gimbal_serial.c @@ -317,9 +317,9 @@ void gimbalSerialHeadTrackerReceive(uint16_t c, void *data) state->attitude.crcl = c; if(checkCrc(&(state->attitude))) { state->expires = micros() + MAX_HEADTRACKER_DATA_AGE_US; - state->pan = state->attitude.pan; - state->tilt = state->attitude.tilt; - state->roll = state->attitude.roll; + state->pan = constrain(state->attitude.pan * headTrackerConfig()->pan_ratio) + 0.5, -2048, 2047); + state->tilt = constrain(state->attitude.tilt * headTrackerConfig()->tilt_ratio) + 0.5, -2048, 2047); + state->roll = constrain(state->attitude.roll * headTrackerConfig()->roll_ratio) + 0.5, -2048, 2047); DEBUG_SET(DEBUG_HEADTRACKING, 2, pktCount++); } else { DEBUG_SET(DEBUG_HEADTRACKING, 3, errorCount++); diff --git a/src/main/io/headtracker_msp.c b/src/main/io/headtracker_msp.c index 35757665d5..a1d85bc1bb 100644 --- a/src/main/io/headtracker_msp.c +++ b/src/main/io/headtracker_msp.c @@ -73,9 +73,9 @@ void mspHeadTrackerReceiverNewData(uint8_t *data, int dataSize) headtrackerMspMessage_t *status = (headtrackerMspMessage_t *)data; - headTrackerMspDevice.pan = status->pan; - headTrackerMspDevice.tilt = status->tilt; - headTrackerMspDevice.roll = status->roll; + headTrackerMspDevice.pan = constrain((status->pan * headTrackerConfig()->pan_ratio) + 0.5, -2048, 2047); + headTrackerMspDevice.tilt = constrain((status->tilt * headTrackerConfig()->tilt_ratio) + 0.5, -2048, 2047); + headTrackerMspDevice.roll = constrain((status->roll * headTrackerConfig()->roll_ratio) + 0.5, -2048, 2047); headTrackerMspDevice.expires = micros() + MAX_HEADTRACKER_DATA_AGE_US; UNUSED(status);