Skip to content

Commit 0526878

Browse files
Quick-FlashnerdCopter
Quick-Flash
authored and
nerdCopter
committed
rc velocity filter boost
1 parent d3481a3 commit 0526878

File tree

5 files changed

+52
-22
lines changed

5 files changed

+52
-22
lines changed

‎src/main/cli/settings.c‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -763,6 +763,8 @@ const clivalue_t valueTable[] = {
763763
{ 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) },
764764
{ 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) },
765765
{ 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) },
766+
{ "rc_velocity_boost_cutoff_hz", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 1, 255 }, PG_RX_CONFIG, offsetof(rxConfig_t, rcVelocityCutoff) },
767+
{ "rc_velocity_boost", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_RX_CONFIG, offsetof(rxConfig_t, rcVelocityCutoffBoost) },
766768
#endif // USE_RC_SMOOTHING_FILTER
767769

768770
{ "fpv_mix_degrees", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 90 }, PG_RX_CONFIG, offsetof(rxConfig_t, fpvCamAngleDegrees) },

‎src/main/fc/rc.c‎

Lines changed: 45 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -64,6 +64,8 @@ float rcCommandDelta[XYZ_AXIS_COUNT];
6464
#endif
6565
static float rawSetpoint[XYZ_AXIS_COUNT];
6666
static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3];
67+
FAST_DATA_ZERO_INIT float lastRcDeflection[4], rcVelocity[4];
68+
static float rcDeflectionSmoothed[3];
6769
static bool reverseMotors = false;
6870
static applyRatesFn *applyRates;
6971
static uint16_t currentRxRefreshRate;
@@ -328,32 +330,55 @@ FAST_CODE_NOINLINE int calcAutoSmoothingCutoff(int avgRxFrameTimeUs, uint8_t aut
328330

329331
// Initialize or update the filters base on either the manually selected cutoff, or
330332
// the auto-calculated cutoff frequency based on detected rx frame rate.
331-
FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothingData)
333+
FAST_CODE_NOINLINE void rcSmoothingAutoRxRateCutoffs(rcSmoothingFilter_t *smoothingData)
332334
{
333-
const float dT = targetPidLooptime * 1e-6f;
334-
uint16_t oldCutoff = smoothingData->setpointCutoffFrequency;
335-
336335
if (smoothingData->setpointCutoffSetting == 0) {
337336
smoothingData->setpointCutoffFrequency = MAX(RC_SMOOTHING_CUTOFF_MIN_HZ, calcAutoSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactorSetpoint));
338337
}
339338
if (smoothingData->throttleCutoffSetting == 0) {
340339
smoothingData->throttleCutoffFrequency = MAX(RC_SMOOTHING_CUTOFF_MIN_HZ, calcAutoSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactorThrottle));
341340
}
341+
if (rcSmoothingData.ffCutoffSetting == 0) {
342+
smoothingData->feedforwardCutoffFrequency = MAX(RC_SMOOTHING_CUTOFF_MIN_HZ, calcAutoSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactorSetpoint));
343+
}
344+
// todo add the rc velocity filter boost to ff
345+
if (!smoothingData->filterInitialized) {
346+
pidInitFeedforwardLpf(smoothingData->feedforwardCutoffFrequency, smoothingData->debugAxis);
347+
} else {
348+
pidUpdateFeedforwardLpf(smoothingData->feedforwardCutoffFrequency);
349+
}
350+
}
342351

352+
FAST_CODE_NOINLINE void rcSmoothingVelocityCutoffAdjustment (rcSmoothingFilter_t *smoothingData) {
353+
const float dT = targetPidLooptime * 1e-6f;
354+
float filterVelocityBoost[4];
343355
// initialize or update the Setpoint filter
344-
if ((smoothingData->setpointCutoffFrequency != oldCutoff) || !smoothingData->filterInitialized) {
356+
if (!smoothingData->filterInitialized) {
345357
for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
358+
// rcVelocity of 1 matches to moving your stick to the end of its travel in 1 second
359+
if (i != 3) {
360+
rcVelocity[i] = ABS(rcDeflection[i] - lastRcDeflection[i]) / (smoothingData->averageFrameTimeUs * 1e-6f);
361+
lastRcDeflection[i] = rcDeflection[i];
362+
363+
} else {
364+
float throttle = (rcCommand[i] / 1000.0);
365+
rcVelocity[i] = ABS(throttle - lastRcDeflection[i]) / (smoothingData->averageFrameTimeUs * 1e-6f);
366+
lastRcDeflection[i] = throttle;
367+
}
368+
rcVelocity[i] = pt1FilterApply(&smoothingData->rcVelocityFilter[i], rcVelocity[i]);
369+
filterVelocityBoost[i] = MAX(5.0, rcVelocity[i] * rxConfig()->rcVelocityCutoffBoost / 100.0f);
370+
346371
if (i < THROTTLE) { // Throttle handled by smoothing rcCommand
347372
if (!smoothingData->filterInitialized) {
348373
ptnFilterInit(&smoothingData->filter[i], rxConfig()->rc_smoothing_order, smoothingData->setpointCutoffFrequency, dT);
349374
} else {
350-
ptnFilterUpdate(&smoothingData->filter[i], smoothingData->setpointCutoffFrequency, dT);
375+
ptnFilterUpdate(&smoothingData->filter[i], smoothingData->setpointCutoffFrequency * filterVelocityBoost[i], dT);
351376
}
352377
} else {
353378
if (!smoothingData->filterInitialized) {
354379
ptnFilterInit(&smoothingData->filter[i], rxConfig()->rc_smoothing_order, smoothingData->throttleCutoffFrequency, dT);
355380
} else {
356-
ptnFilterUpdate(&smoothingData->filter[i], smoothingData->throttleCutoffFrequency, dT);
381+
ptnFilterUpdate(&smoothingData->filter[i], smoothingData->throttleCutoffFrequency * filterVelocityBoost[i], dT);
357382
}
358383
}
359384
}
@@ -363,21 +388,10 @@ FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothi
363388
if (!smoothingData->filterInitialized) {
364389
ptnFilterInit(&smoothingData->filterDeflection[i], rxConfig()->rc_smoothing_order, smoothingData->setpointCutoffFrequency, dT);
365390
} else {
366-
ptnFilterUpdate(&smoothingData->filterDeflection[i], smoothingData->setpointCutoffFrequency, dT);
391+
ptnFilterUpdate(&smoothingData->filterDeflection[i], smoothingData->setpointCutoffFrequency * filterVelocityBoost[i], dT);
367392
}
368393
}
369394
}
370-
371-
// update or initialize the FF filter
372-
oldCutoff = smoothingData->feedforwardCutoffFrequency;
373-
if (rcSmoothingData.ffCutoffSetting == 0) {
374-
smoothingData->feedforwardCutoffFrequency = MAX(RC_SMOOTHING_CUTOFF_MIN_HZ, calcAutoSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactorSetpoint));
375-
}
376-
if (!smoothingData->filterInitialized) {
377-
pidInitFeedforwardLpf(smoothingData->feedforwardCutoffFrequency, smoothingData->debugAxis);
378-
} else if (smoothingData->feedforwardCutoffFrequency != oldCutoff) {
379-
pidUpdateFeedforwardLpf(smoothingData->feedforwardCutoffFrequency);
380-
}
381395
}
382396

383397
FAST_CODE_NOINLINE void rcSmoothingResetAccumulation(rcSmoothingFilter_t *smoothingData)
@@ -426,11 +440,13 @@ static FAST_CODE void processRcSmoothingFilter(void)
426440
static FAST_DATA_ZERO_INIT timeMs_t validRxFrameTimeMs;
427441
static FAST_DATA_ZERO_INIT bool calculateCutoffs;
428442

443+
const float dT = targetPidLooptime * 1e-6f;
444+
429445
// first call initialization
430446
if (!initialized) {
431447
initialized = true;
432448
rcSmoothingData.filterInitialized = false;
433-
rcSmoothingData.averageFrameTimeUs = 0;
449+
rcSmoothingData.averageFrameTimeUs = 2000;
434450
rcSmoothingData.autoSmoothnessFactorSetpoint = rxConfig()->rc_smoothing_auto_factor_rpy;
435451
rcSmoothingData.autoSmoothnessFactorThrottle = rxConfig()->rc_smoothing_auto_factor_throttle;
436452
rcSmoothingData.debugAxis = rxConfig()->rc_smoothing_debug_axis;
@@ -440,6 +456,10 @@ static FAST_CODE void processRcSmoothingFilter(void)
440456
rcSmoothingResetAccumulation(&rcSmoothingData);
441457
rcSmoothingData.setpointCutoffFrequency = rcSmoothingData.setpointCutoffSetting;
442458
rcSmoothingData.throttleCutoffFrequency = rcSmoothingData.throttleCutoffSetting;
459+
for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
460+
pt1FilterInit(&rcSmoothingData.rcVelocityFilter[i], pt1FilterGain(rxConfig()->rcVelocityCutoff, dT));
461+
}
462+
443463
if (rcSmoothingData.ffCutoffSetting == 0) {
444464
// calculate and use an initial derivative cutoff until the RC interval is known
445465
const float cutoffFactor = 1.5f / (1.0f + (rcSmoothingData.autoSmoothnessFactorSetpoint / 10.0f));
@@ -454,7 +474,8 @@ static FAST_CODE void processRcSmoothingFilter(void)
454474

455475
// if we don't need to calculate cutoffs dynamically then the filters can be initialized now
456476
if (!calculateCutoffs) {
457-
rcSmoothingSetFilterCutoffs(&rcSmoothingData);
477+
rcSmoothingAutoRxRateCutoffs(&rcSmoothingData);
478+
rcSmoothingVelocityCutoffAdjustment(&rcSmoothingData);
458479
rcSmoothingData.filterInitialized = true;
459480
}
460481
}
@@ -500,12 +521,14 @@ static FAST_CODE void processRcSmoothingFilter(void)
500521
if (rcSmoothingAccumulateSample(&rcSmoothingData, currentRxRefreshRate)) {
501522
// the required number of samples were collected so set the filter cutoffs, but only if smoothing is active
502523
if (rxConfig()->rc_smoothing_mode) {
503-
rcSmoothingSetFilterCutoffs(&rcSmoothingData);
524+
rcSmoothingAutoRxRateCutoffs(&rcSmoothingData);
504525
rcSmoothingData.filterInitialized = true;
505526
}
506527
validRxFrameTimeMs = 0;
507528
}
508529
}
530+
// always update this as dynamically based on rc velocity
531+
rcSmoothingVelocityCutoffAdjustment(&rcSmoothingData);
509532

510533
}
511534
} else {

‎src/main/fc/rc_controls.h‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -108,6 +108,7 @@ typedef struct rcSmoothingFilter_s {
108108
uint8_t debugAxis;
109109
uint8_t autoSmoothnessFactorSetpoint;
110110
uint8_t autoSmoothnessFactorThrottle;
111+
pt1Filter_t rcVelocityFilter[4];
111112
} rcSmoothingFilter_t;
112113

113114
typedef struct rcControlsConfig_s {

‎src/main/pg/rx.c‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -67,6 +67,8 @@ void pgResetFn_rxConfig(rxConfig_t *rxConfig)
6767
.rc_smoothing_debug_axis = ROLL,
6868
.rc_smoothing_auto_factor_rpy = 30,
6969
.rc_smoothing_auto_factor_throttle = 30,
70+
.rcVelocityCutoffBoost = 75,
71+
.rcVelocityCutoff = 15,
7072
.srxl2_unit_id = 1,
7173
.srxl2_baud_fast = true,
7274
.sbus_baud_fast = false,

‎src/main/pg/rx.h‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -57,6 +57,8 @@ typedef struct rxConfig_s {
5757
uint8_t rc_smoothing_debug_axis; // Axis to log as debug values when debug_mode = RC_SMOOTHING
5858
uint8_t rc_smoothing_auto_factor_rpy; // Used to adjust the "smoothness" determined by the auto cutoff calculations
5959
uint8_t rc_smoothing_auto_factor_throttle; // Used to adjust the "smoothness" determined by the auto cutoff calculations
60+
uint8_t rcVelocityCutoffBoost;
61+
uint8_t rcVelocityCutoff;
6062
uint8_t rssi_src_frame_lpf_period; // Period of the cutoff frequency for the source frame RSSI filter (in 0.1 s)
6163
uint8_t srxl2_unit_id; // Spektrum SRXL2 RX unit id
6264
uint8_t srxl2_baud_fast; // Select Spektrum SRXL2 fast baud rate

0 commit comments

Comments
 (0)