Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-12 19:10:32 +03:00
Code Issues Wiki Activity

hover point throttle curve adjustment (#14229)

Browse source 
* hover point throttle curve adjustment

* Update src/main/fc/rc.c

Co-authored-by: Mark Haslinghuis <mark@numloq.nl>

* Update src/main/fc/rc.c

Co-authored-by: Petr Ledvina <ledvinap@gmail.com>

* Update src/main/fc/rc.c

Co-authored-by: Petr Ledvina <ledvinap@gmail.com>

* refactor lookup calculation and move hover to end of controlRateConfig_s struct

* moved sbufReadU8 to the bottom

* added sbufBytesRemaining check

* update controlRateProfile version from 6 to 7

* moved thrHover8 declaration position

* revert variable declaration position change

* fixed tests

---------

Co-authored-by: Mark Haslinghuis <mark@numloq.nl>
Co-authored-by: Petr Ledvina <ledvinap@gmail.com>
This commit is contained in:
Marc Frank 2025-04-10 14:39:36 +02:00 • committed by GitHub
parent fd3a131797
commit 4ae22e9f8e
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
9 changed files with 60 additions and 10 deletions
Download patch file Download diff file Expand all files Collapse all files

1
src/main/blackbox/blackbox.c
View file

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

1
src/main/cli/settings.c
View file

@ -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]) },

1
src/main/cms/cms_menu_imu.c
View file

@ -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} },

3
src/main/fc/controlrate_profile.c
View file

@ -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,
);
}
}

1
src/main/fc/controlrate_profile.h
View file

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

1
src/main/fc/parameter_names.h
View file

@ -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"

49
src/main/fc/rc.c
View file

@ -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) {

10
src/main/msp/msp.c
View file

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

3
src/test/unit/rc_controls_unittest.cc
View file

@ -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