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

Merge 96d8cfef02 into acbab53d13

Browse source
This commit is contained in:
Kevin Plaizier 2025-07-11 19:54:59 +02:00 • committed by GitHub
commit cd778609ff
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
15 changed files with 129 additions and 91 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1757,13 +1757,11 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_AUTO_FACTOR, "%d", rxConfig()->rc_smoothing_auto_factor_rpy);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_AUTO_FACTOR_THROTTLE, "%d", rxConfig()->rc_smoothing_auto_factor_throttle);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_FEEDFORWARD_CUTOFF, "%d", rcSmoothingData->feedforwardCutoffSetting);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_SETPOINT_CUTOFF, "%d", rcSmoothingData->setpointCutoffSetting);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_THROTTLE_CUTOFF, "%d", rcSmoothingData->throttleCutoffSetting);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_DEBUG_AXIS, "%d", rcSmoothingData->debugAxis);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_ACTIVE_CUTOFFS, "%d,%d,%d", rcSmoothingData->feedforwardCutoffFrequency,
rcSmoothingData->setpointCutoffFrequency,
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_ACTIVE_CUTOFFS, "%d,%d", rcSmoothingData->setpointCutoffFrequency,
rcSmoothingData->throttleCutoffFrequency);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_rx_smoothed", "%d", lrintf(rcSmoothingData->smoothedRxRateHz));
#endif // USE_RC_SMOOTHING_FILTER

8
src/main/cli/cli.c
View file

@ -5004,18 +5004,12 @@ static void cliRcSmoothing(const char *cmdName, char *cmdline)
cliPrintLine("NO SIGNAL");
}
}
cliPrintf("# Active setpoint cutoff: %dhz ", rcSmoothingData->setpointCutoffFrequency);
cliPrintf("# Active setpoint and FF cutoff: %dhz ", rcSmoothingData->setpointCutoffFrequency);
if (rcSmoothingData->setpointCutoffSetting) {
cliPrintLine("(manual)");
} else {
cliPrintLine("(auto)");
}
cliPrintf("# Active FF cutoff: %dhz ", rcSmoothingData->feedforwardCutoffFrequency);
if (rcSmoothingData->feedforwardCutoffSetting) {
cliPrintLine("(manual)");
} else {
cliPrintLine("(auto)");
}
cliPrintf("# Active throttle cutoff: %dhz ", rcSmoothingData->throttleCutoffFrequency);
if (rcSmoothingData->throttleCutoffSetting) {
cliPrintLine("(manual)");

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

@ -856,9 +856,9 @@ const clivalue_t valueTable[] = {
{ PARAM_NAME_RC_SMOOTHING_AUTO_FACTOR, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { RC_SMOOTHING_AUTO_FACTOR_MIN, RC_SMOOTHING_AUTO_FACTOR_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_auto_factor_rpy) },
{ PARAM_NAME_RC_SMOOTHING_AUTO_FACTOR_THROTTLE, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { RC_SMOOTHING_AUTO_FACTOR_MIN, RC_SMOOTHING_AUTO_FACTOR_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_auto_factor_throttle) },
{ PARAM_NAME_RC_SMOOTHING_SETPOINT_CUTOFF, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_setpoint_cutoff) },
{ PARAM_NAME_RC_SMOOTHING_FEEDFORWARD_CUTOFF, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_feedforward_cutoff) },
{ PARAM_NAME_RC_SMOOTHING_THROTTLE_CUTOFF, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_throttle_cutoff) },
{ PARAM_NAME_RC_SMOOTHING_DEBUG_AXIS, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RC_SMOOTHING_DEBUG }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_debug_axis) },
{ PARAM_NAME_RC_SMOOTHING_USE_TAU, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_use_tau) },
#endif // USE_RC_SMOOTHING_FILTER
{ "fpv_mix_degrees", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 90 }, PG_RX_CONFIG, offsetof(rxConfig_t, fpvCamAngleDegrees) },
@ -1099,6 +1099,9 @@ const clivalue_t valueTable[] = {
{ "roll_rate_limit", VAR_UINT16 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { CONTROL_RATE_CONFIG_RATE_LIMIT_MIN, CONTROL_RATE_CONFIG_RATE_LIMIT_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rate_limit[FD_ROLL]) },
{ "pitch_rate_limit", VAR_UINT16 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { CONTROL_RATE_CONFIG_RATE_LIMIT_MIN, CONTROL_RATE_CONFIG_RATE_LIMIT_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rate_limit[FD_PITCH]) },
{ "yaw_rate_limit", VAR_UINT16 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { CONTROL_RATE_CONFIG_RATE_LIMIT_MIN, CONTROL_RATE_CONFIG_RATE_LIMIT_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rate_limit[FD_YAW]) },
{ PARAM_NAME_RC_SMOOTHING_SETPOINT_TAU_CENTER,VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rc_smoothing_setpoint_tau_center) },
{ PARAM_NAME_RC_SMOOTHING_SETPOINT_TAU_END, VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rc_smoothing_setpoint_tau_end) },
{ PARAM_NAME_RC_SMOOTHING_THROTTLE_TAU, VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rc_smoothing_throttle_tau) },
// PG_SERIAL_CONFIG
{ "reboot_character", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 48, 126 }, PG_SERIAL_CONFIG, offsetof(serialConfig_t, reboot_character) },

15
src/main/common/filter.c
View file

@ -57,8 +57,9 @@ float pt1FilterGainFromDelay(float delay, float dT)
return 1.0f; // gain = 1 means no filtering
}
const float cutoffHz = 1.0f / (2.0f * M_PIf * delay);
return pt1FilterGain(cutoffHz, dT);
// cutoffHz = 1.0f / (2.0f * M_PIf * delay)
return dT / (dT + delay);
}
void pt1FilterInit(pt1Filter_t *filter, float k)
@ -93,8 +94,9 @@ float pt2FilterGainFromDelay(float delay, float dT)
return 1.0f; // gain = 1 means no filtering
}
const float cutoffHz = 1.0f / (M_PIf * delay * CUTOFF_CORRECTION_PT2);
return pt2FilterGain(cutoffHz, dT);
// cutoffHz = 1.0f / (2.0f * M_PIf * delay * CUTOFF_CORRECTION_PT2)
return dT / (dT + delay * CUTOFF_CORRECTION_PT2);
}
void pt2FilterInit(pt2Filter_t *filter, float k)
@ -131,8 +133,9 @@ float pt3FilterGainFromDelay(float delay, float dT)
return 1.0f; // gain = 1 means no filtering
}
const float cutoffHz = 1.0f / (M_PIf * delay * CUTOFF_CORRECTION_PT3);
return pt3FilterGain(cutoffHz, dT);
// cutoffHz = 1.0f / (2.0f * M_PIf * delay * CUTOFF_CORRECTION_PT3)
return dT / (dT + delay * CUTOFF_CORRECTION_PT3);
}
void pt3FilterInit(pt3Filter_t *filter, float k)

4
src/main/common/maths.h
View file

@ -164,3 +164,7 @@ static inline float constrainf(float amt, float low, float high)
else
return amt;
}
static inline float lerp(float t, float a, float b) {
return a + t * (b - a);
}

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

@ -63,6 +63,9 @@ void pgResetFn_controlRateProfiles(controlRateConfig_t *controlRateConfig)
.profileName = { 0 },
.quickRatesRcExpo = 0,
.thrHover8 = 50,
.rc_smoothing_setpoint_tau_center = 50,
.rc_smoothing_setpoint_tau_end = 15,
.rc_smoothing_throttle_tau = 20,
);
}
}

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

@ -61,6 +61,9 @@ typedef struct controlRateConfig_s {
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;
uint8_t rc_smoothing_setpoint_tau_center; // center stick tau for rc smoothing
uint8_t rc_smoothing_setpoint_tau_end; // end stick tau for rc smoothing
uint8_t rc_smoothing_throttle_tau; // throttle tau for rc smoothing
} controlRateConfig_t;
PG_DECLARE_ARRAY(controlRateConfig_t, CONTROL_RATE_PROFILE_COUNT, controlRateProfiles);

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

@ -38,9 +38,12 @@
#define PARAM_NAME_RC_SMOOTHING_AUTO_FACTOR "rc_smoothing_auto_factor"
#define PARAM_NAME_RC_SMOOTHING_AUTO_FACTOR_THROTTLE "rc_smoothing_auto_factor_throttle"
#define PARAM_NAME_RC_SMOOTHING_SETPOINT_CUTOFF "rc_smoothing_setpoint_cutoff"
#define PARAM_NAME_RC_SMOOTHING_FEEDFORWARD_CUTOFF "rc_smoothing_feedforward_cutoff"
#define PARAM_NAME_RC_SMOOTHING_THROTTLE_CUTOFF "rc_smoothing_throttle_cutoff"
#define PARAM_NAME_RC_SMOOTHING_DEBUG_AXIS "rc_smoothing_debug_axis"
#define PARAM_NAME_RC_SMOOTHING_USE_TAU "rc_smoothing_use_tau"
#define PARAM_NAME_RC_SMOOTHING_SETPOINT_TAU_CENTER "rc_smoothing_setpoint_tau_center"
#define PARAM_NAME_RC_SMOOTHING_SETPOINT_TAU_END "rc_smoothing_setpoint_tau_end"
#define PARAM_NAME_RC_SMOOTHING_THROTTLE_TAU "rc_smoothing_throttle_tau"
#define PARAM_NAME_RC_SMOOTHING_ACTIVE_CUTOFFS "rc_smoothing_active_cutoffs_ff_sp_thr"
#define PARAM_NAME_SERIAL_RX_PROVIDER "serialrx_provider"
#define PARAM_NAME_MOTOR_IDLE "motor_idle"

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

@ -335,77 +335,72 @@ bool getRxRateValid(void)
#ifdef USE_RC_SMOOTHING_FILTER
static FAST_CODE_NOINLINE void rcSmoothingUpdateFilterTau(rcSmoothingFilter_t *smoothingData)
{
const float cen_tau = smoothingData->setpointTauCenter;
const float end_tau = smoothingData->setpointTauEnd;
const float dT = targetPidLooptime * 1e-6f;
for (unsigned int i = FD_ROLL; i <= FD_YAW; i++) {
const float tau = lerp(rcDeflectionAbs[i], cen_tau, end_tau);
const float pt3K = pt3FilterGainFromDelay(tau, dT);
pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[i], pt3K);
pt3FilterUpdateCutoff(&smoothingData->filterFeedforward[i], pt3K);
if (i < ARRAYLEN(smoothingData->filterRcDeflection)) {
pt3FilterUpdateCutoff(&smoothingData->filterRcDeflection[i], pt3K);
}
}
}
// Initialize or update the filters base on either the manually selected cutoff, or
// the auto-calculated cutoff frequency based on detected rx frame rate.
static FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothingData)
{
// in auto mode, calculate the RC smoothing cutoff from the smoothed Rx link frequency
const uint16_t oldSetpointCutoff = smoothingData->setpointCutoffFrequency;
const uint16_t oldFeedforwardCutoff = smoothingData->feedforwardCutoffFrequency;
const uint16_t minCutoffHz = 15; // don't let any RC smoothing filter cutoff go below 15Hz
if (smoothingData->setpointCutoffSetting == 0) {
smoothingData->setpointCutoffFrequency = MAX(minCutoffHz, (uint16_t)(smoothingData->smoothedRxRateHz * smoothingData->autoSmoothnessFactorSetpoint));
}
if (smoothingData->throttleCutoffSetting == 0) {
smoothingData->throttleCutoffFrequency = MAX(minCutoffHz, (uint16_t)(smoothingData->smoothedRxRateHz * smoothingData->autoSmoothnessFactorThrottle));
const float minCutoffHz = 15.0f; // don't let any RC smoothing filter cutoff go below 15Hz
const bool autoSetpointSmoothing = smoothingData->setpointCutoffSetting == 0;
const bool autoThrottleSmoothing = smoothingData->throttleCutoffSetting == 0;
float setpointCutoffFrequency = smoothingData->setpointCutoffFrequency;
float throttleCutoffFrequency = smoothingData->throttleCutoffFrequency;
if (autoSetpointSmoothing) {
setpointCutoffFrequency = MAX(minCutoffHz, smoothingData->smoothedRxRateHz * smoothingData->autoSmoothnessFactorSetpoint);
smoothingData->setpointCutoffFrequency = setpointCutoffFrequency; // update for logging
}
if (smoothingData->feedforwardCutoffSetting == 0) {
smoothingData->feedforwardCutoffFrequency = MAX(minCutoffHz, (uint16_t)(smoothingData->smoothedRxRateHz * smoothingData->autoSmoothnessFactorFeedforward));
if (autoThrottleSmoothing) {
throttleCutoffFrequency = MAX(minCutoffHz, smoothingData->smoothedRxRateHz * smoothingData->autoSmoothnessFactorThrottle);
smoothingData->throttleCutoffFrequency = throttleCutoffFrequency; // update for logging
}
const float dT = targetPidLooptime * 1e-6f;
if ((smoothingData->setpointCutoffFrequency != oldSetpointCutoff) || !smoothingData->filterInitialized) {
// note that cutoff frequencies are integers, filter cutoffs won't re-calculate until there is > 1hz variation from previous cutoff
// initialize or update the setpoint cutoff based filters
const float setpointCutoffFrequency = smoothingData->setpointCutoffFrequency;
for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
if (i < THROTTLE) {
if (!smoothingData->filterInitialized) {
pt3FilterInit(&smoothingData->filterSetpoint[i], pt3FilterGain(setpointCutoffFrequency, dT));
} else {
pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[i], pt3FilterGain(setpointCutoffFrequency, dT));
}
} else {
const float throttleCutoffFrequency = smoothingData->throttleCutoffFrequency;
if (!smoothingData->filterInitialized) {
pt3FilterInit(&smoothingData->filterSetpoint[i], pt3FilterGain(throttleCutoffFrequency, dT));
} else {
pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[i], pt3FilterGain(throttleCutoffFrequency, dT));
}
}
}
// initialize or update the RC Deflection filter
for (int i = FD_ROLL; i < FD_YAW; i++) {
if (!smoothingData->filterInitialized) {
pt3FilterInit(&smoothingData->filterRcDeflection[i], pt3FilterGain(setpointCutoffFrequency, dT));
} else {
pt3FilterUpdateCutoff(&smoothingData->filterRcDeflection[i], pt3FilterGain(setpointCutoffFrequency, dT));
}
}
// Update the RC Setpoint/Deflection filter and FeedForward Filter
// all cutoffs will be the same, we can optimize :)
const float pt3K = pt3FilterGain(setpointCutoffFrequency, dT);
for (int i = FD_ROLL; i <= FD_YAW; i++) {
pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[i], pt3K);
pt3FilterUpdateCutoff(&smoothingData->filterFeedforward[i], pt3K);
}
// initialize or update the Feedforward filter
if ((smoothingData->feedforwardCutoffFrequency != oldFeedforwardCutoff) || !smoothingData->filterInitialized) {
for (int i = FD_ROLL; i <= FD_YAW; i++) {
const float feedforwardCutoffFrequency = smoothingData->feedforwardCutoffFrequency;
if (!smoothingData->filterInitialized) {
pt3FilterInit(&smoothingData->filterFeedforward[i], pt3FilterGain(feedforwardCutoffFrequency, dT));
} else {
pt3FilterUpdateCutoff(&smoothingData->filterFeedforward[i], pt3FilterGain(feedforwardCutoffFrequency, dT));
}
}
for (int i = FD_ROLL; i <= FD_PITCH; i++) {
pt3FilterUpdateCutoff(&smoothingData->filterRcDeflection[i], pt3K);
}
pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[THROTTLE], pt3FilterGain(throttleCutoffFrequency, dT));
DEBUG_SET(DEBUG_RC_SMOOTHING, 1, smoothingData->setpointCutoffFrequency);
DEBUG_SET(DEBUG_RC_SMOOTHING, 2, smoothingData->feedforwardCutoffFrequency);
DEBUG_SET(DEBUG_RC_SMOOTHING, 2, smoothingData->throttleCutoffFrequency);
}
// Determine if we need to caclulate filter cutoffs. If not then we can avoid
// Determine if we need to calculate filter cutoffs. If not then we can avoid
// examining the rx frame times completely
FAST_CODE_NOINLINE bool rcSmoothingAutoCalculate(void)
{
// if any rc smoothing cutoff is 0 (auto) then we need to calculate cutoffs
if ((rcSmoothingData.setpointCutoffSetting == 0) || (rcSmoothingData.feedforwardCutoffSetting == 0) || (rcSmoothingData.throttleCutoffSetting == 0)) {
if (((rcSmoothingData.setpointCutoffSetting == 0) || (rcSmoothingData.throttleCutoffSetting == 0)) && !rxConfig()->rc_smoothing_use_tau) {
return true;
}
return false;
@ -426,22 +421,21 @@ static FAST_CODE void processRcSmoothingFilter(void)
rcSmoothingData.debugAxis = rxConfig()->rc_smoothing_debug_axis;
rcSmoothingData.autoSmoothnessFactorSetpoint = 1.5f / (1.0f + (rxConfig()->rc_smoothing_auto_factor_rpy / 10.0f));
rcSmoothingData.autoSmoothnessFactorFeedforward = 1.5f / (1.0f + (rxConfig()->rc_smoothing_auto_factor_rpy / 10.0f));
rcSmoothingData.autoSmoothnessFactorThrottle = 1.5f / (1.0f + (rxConfig()->rc_smoothing_auto_factor_throttle / 10.0f));
rcSmoothingData.setpointCutoffSetting = rxConfig()->rc_smoothing_setpoint_cutoff;
rcSmoothingData.throttleCutoffSetting = rxConfig()->rc_smoothing_throttle_cutoff;
rcSmoothingData.feedforwardCutoffSetting = rxConfig()->rc_smoothing_feedforward_cutoff;
rcSmoothingData.setpointCutoffFrequency = rcSmoothingData.setpointCutoffSetting;
rcSmoothingData.feedforwardCutoffFrequency = rcSmoothingData.feedforwardCutoffSetting;
rcSmoothingData.throttleCutoffFrequency = rcSmoothingData.throttleCutoffSetting;
if (rxConfig()->rc_smoothing_mode) {
calculateCutoffs = rcSmoothingAutoCalculate();
// if we don't need to calculate cutoffs dynamically then the filters can be initialized now
if (!calculateCutoffs) {
rcSmoothingSetFilterCutoffs(&rcSmoothingData);
if (!rxConfig()->rc_smoothing_use_tau) {
rcSmoothingSetFilterCutoffs(&rcSmoothingData);
}
rcSmoothingData.filterInitialized = true;
}
}
@ -492,6 +486,8 @@ static FAST_CODE void processRcSmoothingFilter(void)
DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 1, rcSmoothingData.sampleCount);
DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 2, rcSmoothingData.smoothedRxRateHz); // value used by filters
DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 3, sampleState); // guard time = 1, guard time expired = 2
} else if (rxConfig()->rc_smoothing_use_tau) {
rcSmoothingUpdateFilterTau(&rcSmoothingData);
}
// Get new values to be smoothed
for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
@ -540,9 +536,7 @@ static FAST_CODE_NOINLINE void calculateFeedforward(const pidRuntime_t *pid, fli
static float prevRcCommand[3]; // for rcCommandDelta test
static float prevRcCommandDeltaAbs[3]; // for duplicate interpolation
static float prevSetpoint[3]; // equals raw unless extrapolated forward
static float prevSetpointSpeed[3]; // for setpointDelta calculation
static float prevSetpointSpeedDelta[3]; // for duplicate extrapolation
static bool isPrevPacketDuplicate[3]; // to identify multiple identical packets
static bool isPrevPacketDuplicate[3]; // to identify multiple identical packets
const float rcCommandDelta = rcCommand[axis] - prevRcCommand[axis];
prevRcCommand[axis] = rcCommand[axis];
@ -578,7 +572,9 @@ static FAST_CODE_NOINLINE void calculateFeedforward(const pidRuntime_t *pid, fli
if (fabsf(setpoint) < 0.90f * maxRcRate[axis]) {
// this is a single packet duplicate, and we assume that it is of approximately normal duration
// hence no multiplication of prevSetpointSpeedDelta by rxInterval / prevRxInterval
setpointSpeed = prevSetpointSpeed[axis] + prevSetpointSpeedDelta[axis];
float prevSetpointSpeed = rcSmoothingData.filterSetpointSpeed[axis].state;
float prevSetpointDelta = rcSmoothingData.filterSetpointDelta[axis].state;
setpointSpeed = prevSetpointSpeed + prevSetpointDelta;
// pretend that there was stick movement also, to hold the same jitter value
rcCommandDeltaAbs = prevRcCommandDeltaAbs[axis];
}
@ -588,7 +584,7 @@ static FAST_CODE_NOINLINE void calculateFeedforward(const pidRuntime_t *pid, fli
setpointSpeed = 0.0f;
// zero the acceleration by setting previous speed to zero
// feedforward will smoothly decay and be attenuated by the jitter reduction value for zero rcCommandDelta
prevSetpointSpeed[axis] = 0.0f; // zero acceleration later on
rcSmoothingData.filterSetpointSpeed[axis].state = 0.0f; // zero acceleration later on
}
isPrevPacketDuplicate[axis] = isDuplicate;
}
@ -621,15 +617,18 @@ static FAST_CODE_NOINLINE void calculateFeedforward(const pidRuntime_t *pid, fli
prevRcCommandDeltaAbs[axis] = rcCommandDeltaAbs;
// smooth the setpointSpeed value
setpointSpeed = prevSetpointSpeed[axis] + pid->feedforwardSmoothFactor * (setpointSpeed - prevSetpointSpeed[axis]);
const float pt1K = pt1FilterGainFromDelay(pid->feedforwardSmoothFactor, 1.0f / rxRate);
pt1FilterUpdateCutoff(&rcSmoothingData.filterSetpointSpeed[axis], pt1K);
// grab the previous output from the filter
float prevSetpointSpeed = rcSmoothingData.filterSetpointSpeed[axis].state;
setpointSpeed = pt1FilterApply(&rcSmoothingData.filterSetpointSpeed[axis], setpointSpeed);
// calculate setpointDelta from smoothed setpoint speed
setpointSpeedDelta = setpointSpeed - prevSetpointSpeed[axis];
prevSetpointSpeed[axis] = setpointSpeed;
setpointSpeedDelta = setpointSpeed - prevSetpointSpeed;
// smooth the setpointDelta element (effectively a second order filter since incoming setpoint was already smoothed)
setpointSpeedDelta = prevSetpointSpeedDelta[axis] + pid->feedforwardSmoothFactor * (setpointSpeedDelta - prevSetpointSpeedDelta[axis]);
prevSetpointSpeedDelta[axis] = setpointSpeedDelta;
pt1FilterUpdateCutoff(&rcSmoothingData.filterSetpointDelta[axis], pt1K);
setpointSpeedDelta = pt1FilterApply(&rcSmoothingData.filterSetpointDelta[axis], setpointSpeedDelta);
// apply gain factor to delta and adjust for rxRate
const float feedforwardBoost = setpointSpeedDelta * rxRate * pid->feedforwardBoostFactor;
@ -967,6 +966,22 @@ void initRcProcessing(void)
const int maxYawRate = (int)maxRcRate[FD_YAW];
initYawSpinRecovery(maxYawRate);
#endif
if (rxConfig()->rc_smoothing_use_tau) {
const float cen_tau = currentControlRateProfile->rc_smoothing_setpoint_tau_center;
const float end_tau = currentControlRateProfile->rc_smoothing_setpoint_tau_end;
const float throttle_tau = currentControlRateProfile->rc_smoothing_throttle_tau;
// unit is 10th of a millisecond, sort of
rcSmoothingData.setpointTauCenter = cen_tau / 10000.0f + cen_tau * cen_tau / 1250000.0f;
rcSmoothingData.setpointTauEnd = end_tau / 10000.0f + end_tau * end_tau / 1250000.0f;
const float throttleTau = throttle_tau / 10000.0f + throttle_tau * throttle_tau / 1250000.0f;
const float dT = targetPidLooptime * 1e-6f;
rcSmoothingUpdateFilterTau(&rcSmoothingData);
pt3FilterUpdateCutoff(&rcSmoothingData.filterSetpoint[THROTTLE], pt3FilterGainFromDelay(throttleTau, dT));
}
}
// send rc smoothing details to blackbox

7
src/main/fc/rc_controls.h
View file

@ -90,22 +90,23 @@ typedef struct rcSmoothingFilter_s {
pt3Filter_t filterSetpoint[4];
pt3Filter_t filterRcDeflection[RP_AXIS_COUNT];
pt3Filter_t filterFeedforward[3];
pt1Filter_t filterSetpointSpeed[3];
pt1Filter_t filterSetpointDelta[3];
uint8_t setpointCutoffSetting;
uint8_t throttleCutoffSetting;
uint8_t feedforwardCutoffSetting;
uint16_t setpointCutoffFrequency;
uint16_t throttleCutoffFrequency;
uint16_t feedforwardCutoffFrequency;
float smoothedRxRateHz;
uint8_t sampleCount;
uint8_t debugAxis;
float autoSmoothnessFactorSetpoint;
float autoSmoothnessFactorFeedforward;
float autoSmoothnessFactorThrottle;
float setpointTauCenter;
float setpointTauEnd;
} rcSmoothingFilter_t;
typedef struct rcControlsConfig_s {

7
src/main/flight/pid_init.c
View file

@ -549,7 +549,12 @@ void pidInitConfig(const pidProfile_t *pidProfile)
pidRuntime.feedforwardTransition = pidProfile->feedforward_transition / 100.0f;
pidRuntime.feedforwardTransitionInv = (pidProfile->feedforward_transition == 0) ? 0.0f : 100.0f / pidProfile->feedforward_transition;
pidRuntime.feedforwardAveraging = pidProfile->feedforward_averaging;
pidRuntime.feedforwardSmoothFactor = 1.0f - (0.01f * pidProfile->feedforward_smooth_factor);
// feedforward_smooth_factor effect previously would change based on packet looprate
// normalizing to 250hz packet rate as that is the most commonly used ELRS packet rate
float scaledSmoothFactor = 0.01f * pidProfile->feedforward_smooth_factor;
float rxDt = 1.0f / 250.0f;
float feedforwardSmoothingTau = (rxDt * scaledSmoothFactor) / (1.0f - scaledSmoothFactor);
pidRuntime.feedforwardSmoothFactor = feedforwardSmoothingTau;
pidRuntime.feedforwardJitterFactor = pidProfile->feedforward_jitter_factor;
pidRuntime.feedforwardJitterFactorInv = 1.0f / (1.0f + pidProfile->feedforward_jitter_factor);
pidRuntime.feedforwardBoostFactor = 0.001f * pidProfile->feedforward_boost;

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

@ -1605,7 +1605,7 @@ case MSP_NAME:
#if defined(USE_RC_SMOOTHING_FILTER)
sbufWriteU8(dst, 0); // not required in API 1.44, was rxConfig()->rc_smoothing_type
sbufWriteU8(dst, rxConfig()->rc_smoothing_setpoint_cutoff);
sbufWriteU8(dst, rxConfig()->rc_smoothing_feedforward_cutoff);
sbufWriteU8(dst, 0); // was rxConfig()->rc_smoothing_feedforward_cutoff, now always combined with rc_smoothing_setpoint_cutoff
sbufWriteU8(dst, 0); // not required in API 1.44, was rxConfig()->rc_smoothing_input_type
sbufWriteU8(dst, 0); // not required in API 1.44, was rxConfig()->rc_smoothing_derivative_type
#else
@ -3798,7 +3798,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
#if defined(USE_RC_SMOOTHING_FILTER)
sbufReadU8(src); // not required in API 1.44, was rc_smoothing_type
configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_setpoint_cutoff, sbufReadU8(src));
configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_feedforward_cutoff, sbufReadU8(src));
sbufReadU8(src); // was rc_smoothing_feedforward_cutoff
sbufReadU8(src); // not required in API 1.44, was rc_smoothing_input_type
sbufReadU8(src); // not required in API 1.44, was rc_smoothing_derivative_type
#else

2
src/main/pg/rx.c
View file

@ -104,7 +104,6 @@ void pgResetFn_rxConfig(rxConfig_t *rxConfig)
.max_aux_channel = DEFAULT_AUX_CHANNEL_COUNT,
.rc_smoothing_mode = 1,
.rc_smoothing_setpoint_cutoff = 0,
.rc_smoothing_feedforward_cutoff = 0,
.rc_smoothing_throttle_cutoff = 0,
.rc_smoothing_debug_axis = ROLL,
.rc_smoothing_auto_factor_rpy = 30,
@ -114,6 +113,7 @@ void pgResetFn_rxConfig(rxConfig_t *rxConfig)
.sbus_baud_fast = false,
.msp_override_channels_mask = 0,
.crsf_use_negotiated_baud = false,
.rc_smoothing_use_tau = false,
);
#ifdef RX_CHANNELS_TAER

2
src/main/pg/rx.h
View file

@ -51,7 +51,6 @@ typedef struct rxConfig_s {
int8_t rssi_offset; // offset applied to the RSSI value before it is returned
uint8_t rc_smoothing_mode; // Whether filter based rc smoothing is on or off
uint8_t rc_smoothing_setpoint_cutoff; // Filter cutoff frequency for the setpoint filter (0 = auto)
uint8_t rc_smoothing_feedforward_cutoff; // Filter cutoff frequency for the feedforward filter (0 = auto)
uint8_t rc_smoothing_throttle_cutoff; // Filter cutoff frequency for the setpoint filter (0 = auto)
uint8_t rc_smoothing_debug_axis; // Axis to log as debug values when debug_mode = RC_SMOOTHING
uint8_t rc_smoothing_auto_factor_rpy; // Used to adjust the "smoothness" determined by the auto cutoff calculations
@ -64,6 +63,7 @@ typedef struct rxConfig_s {
uint32_t msp_override_channels_mask; // Channels to override when the MSP override mode is enabled
uint8_t msp_override_failsafe; // if false then extra RC link is always required in msp_override mode, true - allows control via msp_override without extra RC link (autonomous use case)
uint8_t crsf_use_negotiated_baud; // Use negotiated baud rate for CRSF V3
uint8_t rc_smoothing_use_tau; // if set to 1 it will use the tau values for filtering
} rxConfig_t;
PG_DECLARE(rxConfig_t, rxConfig);

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

@ -272,6 +272,9 @@ protected:
.profileName = "default",
.quickRatesRcExpo = 0,
.thrHover8 = 0,
.rc_smoothing_setpoint_tau_center = 0,
.rc_smoothing_setpoint_tau_end = 0,
.rc_smoothing_throttle_tau = 0,
};
channelRange_t fullRange = {
@ -379,6 +382,9 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp
.profileName = "default",
.quickRatesRcExpo = 0,
.thrHover8 = 0,
.rc_smoothing_setpoint_tau_center = 0,
.rc_smoothing_setpoint_tau_end = 0,
.rc_smoothing_throttle_tau = 0,
};
// and