1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-25 17:25:20 +03:00

Setpoint and FF always use the same cutoff.

This commit is contained in:
Quick-Flash 2025-06-02 20:33:31 -06:00
parent cf14059642
commit 29fbc0af2c
9 changed files with 10 additions and 60 deletions

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

View file

@ -5005,18 +5005,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)");

View file

@ -840,7 +840,6 @@ 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) },
#endif // USE_RC_SMOOTHING_FILTER

View file

@ -38,7 +38,6 @@
#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_ACTIVE_CUTOFFS "rc_smoothing_active_cutoffs_ff_sp_thr"

View file

@ -342,26 +342,13 @@ static FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *
// in auto mode, calculate the RC smoothing cutoff from the smoothed Rx link frequency
const uint16_t oldSetpointCutoff = smoothingData->setpointCutoffFrequency;
const uint16_t oldThrottleCutoff = smoothingData->throttleCutoffFrequency;
const uint16_t oldFeedforwardCutoff = smoothingData->feedforwardCutoffFrequency;
const uint16_t minCutoffHz = 15; // don't let any RC smoothing filter cutoff go below 15Hz
const bool autoSetpointSmoothing = smoothingData->setpointCutoffSetting == 0;
const bool autoThrottleSmoothing = smoothingData->throttleCutoffSetting == 0;
const bool autoFeedforwardSmoothing = smoothingData->feedforwardCutoffSetting == 0;
const bool autoSetpointAndFeedforward = autoSetpointSmoothing && autoFeedforwardSmoothing;
if (autoSetpointAndFeedforward) {
// if both are using auto smoothing, both will have the same cutoff
if (autoSetpointSmoothing) {
smoothingData->setpointCutoffFrequency = MAX(minCutoffHz, (uint16_t)(smoothingData->smoothedRxRateHz * smoothingData->autoSmoothnessFactorSetpoint));
smoothingData->feedforwardCutoffFrequency = smoothingData->setpointCutoffFrequency;
} else {
if (autoSetpointSmoothing) {
smoothingData->setpointCutoffFrequency = MAX(minCutoffHz, (uint16_t)(smoothingData->smoothedRxRateHz * smoothingData->autoSmoothnessFactorSetpoint));
}
if (autoFeedforwardSmoothing) {
smoothingData->feedforwardCutoffFrequency = MAX(minCutoffHz, (uint16_t)(smoothingData->smoothedRxRateHz * smoothingData->autoSmoothnessFactorSetpoint));
}
}
if (autoThrottleSmoothing) {
@ -370,13 +357,12 @@ static FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *
const float dT = targetPidLooptime * 1e-6f;
const float setpointCutoffFrequency = smoothingData->setpointCutoffFrequency;
const float feedforwardCutoffFrequency = smoothingData->feedforwardCutoffFrequency;
const float throttleCutoffFrequency = smoothingData->throttleCutoffFrequency;
if (!smoothingData->filterInitialized) {
for (int i = FD_ROLL; i <= FD_YAW; i++) {
pt3FilterInit(&smoothingData->filterSetpoint[i], pt3FilterGain(setpointCutoffFrequency, dT));
pt3FilterInit(&smoothingData->filterFeedforward[i], pt3FilterGain(feedforwardCutoffFrequency, dT));
pt3FilterInit(&smoothingData->filterFeedforward[i], pt3FilterGain(setpointCutoffFrequency, dT));
}
for (int i = FD_ROLL; i < FD_YAW; i++) {
pt3FilterInit(&smoothingData->filterRcDeflection[i], pt3FilterGain(setpointCutoffFrequency, dT));
@ -385,7 +371,7 @@ static FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *
}
// note that cutoff frequencies are integers, filter cutoffs won't re-calculate until there is > 1hz variation from previous cutoff
if (autoSetpointAndFeedforward && (smoothingData->setpointCutoffFrequency != oldSetpointCutoff)) {
if (smoothingData->setpointCutoffFrequency != oldSetpointCutoff) {
// Update the RC Setpoint/Deflection filter and FeedForward Filter
// all cutoffs will be the same, we can optimize :)
pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[0], pt3FilterGain(setpointCutoffFrequency, dT));
@ -397,26 +383,6 @@ static FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *
for (int i = FD_ROLL; i < FD_YAW; i++) {
pt3FilterUpdateCutoff(&smoothingData->filterRcDeflection[i], pt3K);
}
} else {
if (smoothingData->setpointCutoffFrequency != oldSetpointCutoff) {
pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[0], pt3FilterGain(setpointCutoffFrequency, dT));
const float pt3K = smoothingData->filterSetpoint[0].k;
for (int i = FD_ROLL; i <= FD_YAW; i++) {
pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[i], pt3K);
}
for (int i = FD_ROLL; i < FD_YAW; i++) {
pt3FilterUpdateCutoff(&smoothingData->filterRcDeflection[i], pt3K);
}
}
// Update the Feedforward filter
if (smoothingData->feedforwardCutoffFrequency != oldFeedforwardCutoff) {
pt3FilterUpdateCutoff(&smoothingData->filterFeedforward[0], pt3FilterGain(setpointCutoffFrequency, dT));
const float pt3K = smoothingData->filterSetpoint[0].k;
for (int i = FD_ROLL; i <= FD_YAW; i++) {
pt3FilterUpdateCutoff(&smoothingData->filterFeedforward[i], pt3K);
}
}
}
// Update the throttle filter
@ -425,15 +391,15 @@ static FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *
}
DEBUG_SET(DEBUG_RC_SMOOTHING, 1, smoothingData->setpointCutoffFrequency);
DEBUG_SET(DEBUG_RC_SMOOTHING, 2, smoothingData->feedforwardCutoffFrequency);
DEBUG_SET(DEBUG_RC_SMOOTHING, 2, 0);
}
// 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)) {
return true;
}
return false;
@ -458,10 +424,8 @@ static FAST_CODE void processRcSmoothingFilter(void)
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) {

View file

@ -93,11 +93,9 @@ typedef struct rcSmoothingFilter_s {
uint8_t setpointCutoffSetting;
uint8_t throttleCutoffSetting;
uint8_t feedforwardCutoffSetting;
uint16_t setpointCutoffFrequency;
uint16_t throttleCutoffFrequency;
uint16_t feedforwardCutoffFrequency;
float smoothedRxRateHz;
uint8_t sampleCount;

View file

@ -1598,7 +1598,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
@ -3840,7 +3840,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

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,

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