diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 1e4b2725fc..b4d7f02e7e 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1530,6 +1530,7 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_PID_PROCESS_DENOM, "%d", activePidLoopDenom); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THR_MID, "%d", currentControlRateProfile->thrMid8); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THR_EXPO, "%d", currentControlRateProfile->thrExpo8); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THR_HOVER, "%d", currentControlRateProfile->thrHover8); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_MODE, "%d", currentPidProfile->tpa_mode); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_RATE, "%d", currentPidProfile->tpa_rate); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_BREAKPOINT, "%d", currentPidProfile->tpa_breakpoint); diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 96df532868..b0ad031be9 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1062,6 +1062,7 @@ const clivalue_t valueTable[] = { #endif { PARAM_NAME_THR_MID, VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, thrMid8) }, { PARAM_NAME_THR_EXPO, VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, thrExpo8) }, + { PARAM_NAME_THR_HOVER, VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, thrHover8) }, { PARAM_NAME_RATES_TYPE, VAR_UINT8 | PROFILE_RATE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RATES_TYPE }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rates_type) }, { "quickrates_rc_expo", VAR_UINT8 | PROFILE_RATE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, quickRatesRcExpo) }, { "roll_rc_rate", VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { 1, CONTROL_RATE_CONFIG_RC_RATES_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rcRates[FD_ROLL]) }, diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index d10e386652..2c48e6106d 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -429,6 +429,7 @@ static const OSD_Entry cmsx_menuRateProfileEntries[] = { "THR MID", OME_UINT8, NULL, &(OSD_UINT8_t) { &rateProfile.thrMid8, 0, 100, 1} }, { "THR EXPO", OME_UINT8, NULL, &(OSD_UINT8_t) { &rateProfile.thrExpo8, 0, 100, 1} }, + { "THR HOVER", OME_UINT8, NULL, &(OSD_UINT8_t) { &rateProfile.thrHover8, 0, 100, 1} }, { "THR LIM TYPE",OME_TAB, NULL, &(OSD_TAB_t) { &rateProfile.throttle_limit_type, THROTTLE_LIMIT_TYPE_COUNT - 1, lookupTableThrottleLimitType} }, { "THR LIM %", OME_UINT8, NULL, &(OSD_UINT8_t) { &rateProfile.throttle_limit_percent, 25, 100, 1} }, diff --git a/src/main/fc/controlrate_profile.c b/src/main/fc/controlrate_profile.c index 0add5622b5..6fb219fbb5 100644 --- a/src/main/fc/controlrate_profile.c +++ b/src/main/fc/controlrate_profile.c @@ -37,7 +37,7 @@ controlRateConfig_t *currentControlRateProfile; -PG_REGISTER_ARRAY_WITH_RESET_FN(controlRateConfig_t, CONTROL_RATE_PROFILE_COUNT, controlRateProfiles, PG_CONTROL_RATE_PROFILES, 6); +PG_REGISTER_ARRAY_WITH_RESET_FN(controlRateConfig_t, CONTROL_RATE_PROFILE_COUNT, controlRateProfiles, PG_CONTROL_RATE_PROFILES, 7); void pgResetFn_controlRateProfiles(controlRateConfig_t *controlRateConfig) { @@ -62,6 +62,7 @@ void pgResetFn_controlRateProfiles(controlRateConfig_t *controlRateConfig) .rate_limit[FD_YAW] = CONTROL_RATE_CONFIG_RATE_LIMIT_MAX, .profileName = { 0 }, .quickRatesRcExpo = 0, + .thrHover8 = 50, ); } } diff --git a/src/main/fc/controlrate_profile.h b/src/main/fc/controlrate_profile.h index 6a2a05d415..61607f4b83 100644 --- a/src/main/fc/controlrate_profile.h +++ b/src/main/fc/controlrate_profile.h @@ -60,6 +60,7 @@ typedef struct controlRateConfig_s { uint16_t rate_limit[3]; // Sets the maximum rate for the axes char profileName[MAX_RATE_PROFILE_NAME_LENGTH + 1]; // Descriptive name for rate profile uint8_t quickRatesRcExpo; // Sets expo on rc command for quick rates + uint8_t thrHover8; } controlRateConfig_t; PG_DECLARE_ARRAY(controlRateConfig_t, CONTROL_RATE_PROFILE_COUNT, controlRateProfiles); diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h index 070a516d6e..8d9c812183 100644 --- a/src/main/fc/parameter_names.h +++ b/src/main/fc/parameter_names.h @@ -51,6 +51,7 @@ #define PARAM_NAME_MOTOR_POLES "motor_poles" #define PARAM_NAME_THR_MID "thr_mid" #define PARAM_NAME_THR_EXPO "thr_expo" +#define PARAM_NAME_THR_HOVER "thr_hover" #define PARAM_NAME_RATES_TYPE "rates_type" #define PARAM_NAME_TPA_RATE "tpa_rate" #define PARAM_NAME_TPA_BREAKPOINT "tpa_breakpoint" diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c index 80c284191a..f10752fecc 100644 --- a/src/main/fc/rc.c +++ b/src/main/fc/rc.c @@ -823,20 +823,53 @@ bool isMotorsReversed(void) return reverseMotors; } +static float quadraticBezier(float t, float p0, float p1, float p2) { + return (1.0f - t) * (1.0f - t) * p0 + 2.0f * (1.0f - t) * t * p1 + t * t * p2; +} + void initRcProcessing(void) { rcCommandDivider = 500.0f - rcControlsConfig()->deadband; rcCommandYawDivider = 500.0f - rcControlsConfig()->yaw_deadband; + float thrMid = currentControlRateProfile->thrMid8 / 100.0f; // normalized x coordinate for hover point + float expo = currentControlRateProfile->thrExpo8 / 100.0f; // normalized expo (0.0 .. 1.0) + float thrHover = currentControlRateProfile->thrHover8 / 100.0f; // normalized y coordinate for hover point + + /* + Algorithm Overview: + - thrMid and thrHover define a key point (hover point) in the throttle curve. + • thrMid is the normalized x-coordinate at which the curve reaches the hover point. + • thrHover is the normalized y-coordinate at that point. + - The curve is built in two segments using quadratic Bezier interpolation: + Segment 1: from (0, 0) to (thrMid, thrHover) + • ymin = 0, ymid = thrHover. + • The control point cp1y blends between (thrHover/2) for expo=0 and thrHover for expo=1. + Segment 2: from (thrMid, thrHover) to (1, 1) + • ymid = thrHover, ymax = 1. + • The control point cp2y blends between [thrHover + (1 - thrHover) / 2] for expo=0 and thrHover for expo=1. + - The output y is mapped from [0,1] to the PWM range. + */ + for (int i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) { - const int16_t tmp = 10 * i - currentControlRateProfile->thrMid8; - uint8_t y = 1; - if (tmp > 0) - y = 100 - currentControlRateProfile->thrMid8; - if (tmp < 0) - y = currentControlRateProfile->thrMid8; - lookupThrottleRC[i] = 10 * currentControlRateProfile->thrMid8 + tmp * (100 - currentControlRateProfile->thrExpo8 + (int32_t) currentControlRateProfile->thrExpo8 * (tmp * tmp) / (y * y)) / 10; - lookupThrottleRC[i] = PWM_RANGE_MIN + PWM_RANGE * lookupThrottleRC[i] / 1000; // [MINTHROTTLE;MAXTHROTTLE] + float x = (float)i / (THROTTLE_LOOKUP_LENGTH - 1); + float y = 0.0f; + + if (x <= thrMid) { + float t = (thrMid > 0.0f) ? x / thrMid : 0.0f; + float ymin = 0.0f; + float ymid = thrHover; + float cp1y = (thrHover / 2.0f) * (1.0f - expo) + thrHover * expo; + y = quadraticBezier(t, ymin, cp1y, ymid); + } else { + float t = ((1.0f - thrMid) > 0.0f) ? (x - thrMid) / (1.0f - thrMid) : 0.0f; + float ymid = thrHover; + float ymax = 1.0f; + float cp2y = (thrHover + (1.0f - thrHover) / 2.0f) * (1.0f - expo) + thrHover * expo; + y = quadraticBezier(t, ymid, cp2y, ymax); + } + + lookupThrottleRC[i] = lrintf(scaleRangef(y, 0.0f, 1.0f, PWM_RANGE_MIN, PWM_RANGE_MAX)); } switch (currentControlRateProfile->rates_type) { diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 025df2fc7b..3f6aeca7b2 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -1372,7 +1372,10 @@ case MSP_NAME: // added in 1.43 sbufWriteU8(dst, currentControlRateProfile->rates_type); - + + // added in 1.47 + sbufWriteU8(dst, currentControlRateProfile->thrHover8); + break; case MSP_PID: @@ -2851,6 +2854,11 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, currentControlRateProfile->rates_type = sbufReadU8(src); } + // version 1.47 + if (sbufBytesRemaining(src) >= 1) { + currentControlRateProfile->thrHover8 = sbufReadU8(src); + } + initRcProcessing(); } else { return MSP_RESULT_ERROR; diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc index 7120aa97d8..cf16c68aee 100644 --- a/src/test/unit/rc_controls_unittest.cc +++ b/src/test/unit/rc_controls_unittest.cc @@ -271,6 +271,7 @@ protected: .rate_limit = {0, 0, 0}, .profileName = "default", .quickRatesRcExpo = 0, + .thrHover8 = 0, }; channelRange_t fullRange = { @@ -292,6 +293,7 @@ protected: controlRateConfig.rcExpo[FD_PITCH] = 0; controlRateConfig.thrMid8 = 0; controlRateConfig.thrExpo8 = 0; + controlRateConfig.thrHover8 = 0; controlRateConfig.rcExpo[FD_YAW] = 0; controlRateConfig.rates[0] = 0; controlRateConfig.rates[1] = 0; @@ -376,6 +378,7 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp .rate_limit = {0, 0, 0}, .profileName = "default", .quickRatesRcExpo = 0, + .thrHover8 = 0, }; // and