diff --git a/docs/Settings.md b/docs/Settings.md index ea35813fbdd..6e8a065b7cb 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2618,7 +2618,7 @@ Exponential value used for the YAW axis by the `MANUAL` flight mode [0-100] | Default | Min | Max | | --- | --- | --- | -| 20 | 0 | 100 | +| 20 | -100 | 100 | --- @@ -5828,7 +5828,7 @@ Exponential value used for the YAW axis by all the stabilized flights modes (all | Default | Min | Max | | --- | --- | --- | -| 20 | 0 | 100 | +| 20 | -100 | 100 | --- diff --git a/src/main/common/streambuf.c b/src/main/common/streambuf.c index 5a766423421..4e7b192fc66 100644 --- a/src/main/common/streambuf.c +++ b/src/main/common/streambuf.c @@ -32,6 +32,11 @@ void sbufWriteU8(sbuf_t *dst, uint8_t val) *dst->ptr++ = val; } +void sbufWriteI8(sbuf_t *dst, int8_t val) +{ + *dst->ptr++ = val; +} + void sbufWriteU16(sbuf_t *dst, uint16_t val) { sbufWriteU8(dst, val >> 0); diff --git a/src/main/common/streambuf.h b/src/main/common/streambuf.h index a2ac1f681a6..590a81345e9 100644 --- a/src/main/common/streambuf.h +++ b/src/main/common/streambuf.h @@ -31,6 +31,7 @@ typedef struct sbuf_s { sbuf_t *sbufInit(sbuf_t *sbuf, uint8_t *ptr, uint8_t *end); void sbufWriteU8(sbuf_t *dst, uint8_t val); +void sbufWriteI8(sbuf_t *dst, int8_t val); void sbufWriteU16(sbuf_t *dst, uint16_t val); void sbufWriteU32(sbuf_t *dst, uint32_t val); void sbufFill(sbuf_t *dst, uint8_t data, int len); diff --git a/src/main/fc/controlrate_profile_config_struct.h b/src/main/fc/controlrate_profile_config_struct.h index e4038537603..3f4bcd9b1ef 100644 --- a/src/main/fc/controlrate_profile_config_struct.h +++ b/src/main/fc/controlrate_profile_config_struct.h @@ -36,13 +36,13 @@ typedef struct controlRateConfig_s { struct { uint8_t rcExpo8; - uint8_t rcYawExpo8; + int8_t rcYawExpo8; uint8_t rates[3]; } stabilized; struct { uint8_t rcExpo8; - uint8_t rcYawExpo8; + int8_t rcYawExpo8; uint8_t rates[3]; } manual; diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 2c0e63cbe8e..6679b358e46 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -177,7 +177,7 @@ bool areSensorsCalibrating(void) return false; } -int16_t getAxisRcCommand(int16_t rawData, int16_t rate, int16_t deadband) +int16_t getAxisRcCommand(int16_t rawData, int8_t expo, int16_t deadband) { int16_t stickDeflection = 0; @@ -195,7 +195,7 @@ int16_t getAxisRcCommand(int16_t rawData, int16_t rate, int16_t deadband) #endif stickDeflection = applyDeadbandRescaled(stickDeflection, deadband, -500, 500); - return rcLookup(stickDeflection, rate); + return rcLookup(stickDeflection, expo); } static void updateArmingStatus(void) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index d2f429d0e5c..876f06a53fa 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -690,7 +690,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, currentControlRateProfile->throttle.rcMid8); sbufWriteU8(dst, currentControlRateProfile->throttle.rcExpo8); sbufWriteU16(dst, currentControlRateProfile->throttle.pa_breakpoint); - sbufWriteU8(dst, currentControlRateProfile->stabilized.rcYawExpo8); + sbufWriteI8(dst, currentControlRateProfile->stabilized.rcYawExpo8); break; case MSP2_INAV_RATE_PROFILE: @@ -702,14 +702,14 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF // stabilized sbufWriteU8(dst, currentControlRateProfile->stabilized.rcExpo8); - sbufWriteU8(dst, currentControlRateProfile->stabilized.rcYawExpo8); + sbufWriteI8(dst, currentControlRateProfile->stabilized.rcYawExpo8); for (uint8_t i = 0 ; i < 3; ++i) { sbufWriteU8(dst, currentControlRateProfile->stabilized.rates[i]); // R,P,Y see flight_dynamics_index_t } // manual sbufWriteU8(dst, currentControlRateProfile->manual.rcExpo8); - sbufWriteU8(dst, currentControlRateProfile->manual.rcYawExpo8); + sbufWriteI8(dst, currentControlRateProfile->manual.rcYawExpo8); for (uint8_t i = 0 ; i < 3; ++i) { sbufWriteU8(dst, currentControlRateProfile->manual.rates[i]); // R,P,Y see flight_dynamics_index_t } @@ -2007,7 +2007,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) ((controlRateConfig_t*)currentControlRateProfile)->throttle.rcExpo8 = sbufReadU8(src); ((controlRateConfig_t*)currentControlRateProfile)->throttle.pa_breakpoint = sbufReadU16(src); if (dataSize > 10) { - ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rcYawExpo8 = sbufReadU8(src); + ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rcYawExpo8 = sbufReadI8(src); } schedulePidGainsUpdate(); @@ -2028,7 +2028,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) // stabilized currentControlRateProfile_p->stabilized.rcExpo8 = sbufReadU8(src); - currentControlRateProfile_p->stabilized.rcYawExpo8 = sbufReadU8(src); + currentControlRateProfile_p->stabilized.rcYawExpo8 = sbufReadI8(src); for (uint8_t i = 0; i < 3; ++i) { tmp_u8 = sbufReadU8(src); if (i == FD_YAW) { @@ -2040,7 +2040,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) // manual currentControlRateProfile_p->manual.rcExpo8 = sbufReadU8(src); - currentControlRateProfile_p->manual.rcYawExpo8 = sbufReadU8(src); + currentControlRateProfile_p->manual.rcYawExpo8 = sbufReadI8(src); for (uint8_t i = 0; i < 3; ++i) { tmp_u8 = sbufReadU8(src); if (i == FD_YAW) { diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index 47c359b00c2..7accff74079 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -351,6 +351,13 @@ static void applyAdjustmentU8(adjustmentFunction_e adjustmentFunction, uint8_t * blackboxLogInflightAdjustmentEvent(adjustmentFunction, newValue); } +static void applyAdjustmentI8(adjustmentFunction_e adjustmentFunction, int8_t *val, int delta, int low, int high) +{ + int newValue = constrain((int)(*val) + delta, low, high); + *val = newValue; + blackboxLogInflightAdjustmentEvent(adjustmentFunction, newValue); +} + static void applyAdjustmentU16(adjustmentFunction_e adjustmentFunction, uint16_t *val, int delta, int low, int high) { int newValue = constrain((int)(*val) + delta, low, high); @@ -358,9 +365,12 @@ static void applyAdjustmentU16(adjustmentFunction_e adjustmentFunction, uint16_t blackboxLogInflightAdjustmentEvent(adjustmentFunction, newValue); } -static void applyAdjustmentExpo(adjustmentFunction_e adjustmentFunction, uint8_t *val, int delta) +static void applyAdjustmentExpo(adjustmentFunction_e adjustmentFunction, int8_t *val, int delta, bool isYaw) { - applyAdjustmentU8(adjustmentFunction, val, delta, SETTING_RC_EXPO_MIN, SETTING_RC_EXPO_MAX); + if (isYaw) + applyAdjustmentI8(adjustmentFunction, val, delta, SETTING_RC_YAW_EXPO_MIN, SETTING_RC_YAW_EXPO_MAX); + else + applyAdjustmentU8(adjustmentFunction, (uint8_t *)val, delta, SETTING_RC_EXPO_MIN, SETTING_RC_EXPO_MAX); } static void applyAdjustmentManualRate(adjustmentFunction_e adjustmentFunction, uint8_t *val, int delta) @@ -382,19 +392,19 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t } switch (adjustmentFunction) { case ADJUSTMENT_RC_EXPO: - applyAdjustmentExpo(ADJUSTMENT_RC_EXPO, &controlRateConfig->stabilized.rcExpo8, delta); + applyAdjustmentExpo(ADJUSTMENT_RC_EXPO, (int8_t *)&controlRateConfig->stabilized.rcExpo8, delta, false); break; case ADJUSTMENT_RC_YAW_EXPO: - applyAdjustmentExpo(ADJUSTMENT_RC_YAW_EXPO, &controlRateConfig->stabilized.rcYawExpo8, delta); + applyAdjustmentExpo(ADJUSTMENT_RC_YAW_EXPO, &controlRateConfig->stabilized.rcYawExpo8, delta, true); break; case ADJUSTMENT_MANUAL_RC_EXPO: - applyAdjustmentExpo(ADJUSTMENT_MANUAL_RC_EXPO, &controlRateConfig->manual.rcExpo8, delta); + applyAdjustmentExpo(ADJUSTMENT_MANUAL_RC_EXPO, (int8_t *)&controlRateConfig->manual.rcExpo8, delta, false); break; case ADJUSTMENT_MANUAL_RC_YAW_EXPO: - applyAdjustmentExpo(ADJUSTMENT_MANUAL_RC_YAW_EXPO, &controlRateConfig->manual.rcYawExpo8, delta); + applyAdjustmentExpo(ADJUSTMENT_MANUAL_RC_YAW_EXPO, &controlRateConfig->manual.rcYawExpo8, delta, true); break; case ADJUSTMENT_THROTTLE_EXPO: - applyAdjustmentExpo(ADJUSTMENT_THROTTLE_EXPO, &controlRateConfig->throttle.rcExpo8, delta); + applyAdjustmentExpo(ADJUSTMENT_THROTTLE_EXPO, (int8_t *)&controlRateConfig->throttle.rcExpo8, delta, false); break; case ADJUSTMENT_PITCH_ROLL_RATE: case ADJUSTMENT_PITCH_RATE: diff --git a/src/main/fc/rc_curves.c b/src/main/fc/rc_curves.c index 5453083d279..15f2f5f15e3 100644 --- a/src/main/fc/rc_curves.c +++ b/src/main/fc/rc_curves.c @@ -53,10 +53,12 @@ void generateThrottleCurve(const controlRateConfig_t *controlRateConfig) } } -int16_t rcLookup(int32_t stickDeflection, uint8_t expo) +int16_t rcLookup(int32_t stickDeflection, int8_t expo) { - float tmpf = stickDeflection / 100.0f; - return lrintf((2500.0f + (float)expo * (tmpf * tmpf - 25.0f)) * tmpf / 25.0f); + float stk = stickDeflection / 500.0f; + float expoAdjusted = constrainf(stk * (1.0f + ((float)expo /100.0f) * (stk * stk - 1.0f)), -1.0f, 1.0f); + + return (int16_t)lrintf(expoAdjusted * 500.0f); } uint16_t rcLookupThrottle(uint16_t absoluteDeflection) diff --git a/src/main/fc/rc_curves.h b/src/main/fc/rc_curves.h index af7cd618e74..8795e711349 100644 --- a/src/main/fc/rc_curves.h +++ b/src/main/fc/rc_curves.h @@ -20,6 +20,6 @@ struct controlRateConfig_s; void generateThrottleCurve(const struct controlRateConfig_s *controlRateConfig); -int16_t rcLookup(int32_t stickDeflection, uint8_t expo); +int16_t rcLookup(int32_t stickDeflection, int8_t expo); uint16_t rcLookupThrottle(uint16_t tmp); int16_t rcLookupThrottleMid(void); diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 73077a2ec85..972f5ae7c18 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1412,7 +1412,7 @@ groups: description: "Exponential value used for the YAW axis by all the stabilized flights modes (all but `MANUAL`)" default_value: 20 field: stabilized.rcYawExpo8 - min: 0 + min: -100 max: 100 # New rates are in dps/10. That means, Rate of 20 means 200dps of rotation speed on given axis. # Rate 180 (1800dps) is max. value gyro can measure reliably @@ -1444,7 +1444,7 @@ groups: description: "Exponential value used for the YAW axis by the `MANUAL` flight mode [0-100]" default_value: 20 field: manual.rcYawExpo8 - min: 0 + min: -100 max: 100 - name: manual_roll_rate description: "Servo travel multiplier for the ROLL axis in `MANUAL` flight mode [0-100]%"