Skip to content

Commit 57135ce

Browse files
Quick-FlashnerdCopter
Quick-Flash
authored and
nerdCopter
committed
rc velocity filter boost
1 parent 2ff42a3 commit 57135ce

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;
@@ -333,32 +335,55 @@ FAST_CODE_NOINLINE int calcAutoSmoothingCutoff(int avgRxFrameTimeUs, uint8_t aut
333335

334336
// Initialize or update the filters base on either the manually selected cutoff, or
335337
// the auto-calculated cutoff frequency based on detected rx frame rate.
336-
FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothingData)
338+
FAST_CODE_NOINLINE void rcSmoothingAutoRxRateCutoffs(rcSmoothingFilter_t *smoothingData)
337339
{
338-
const float dT = targetPidLooptime * 1e-6f;
339-
uint16_t oldCutoff = smoothingData->setpointCutoffFrequency;
340-
341340
if (smoothingData->setpointCutoffSetting == 0) {
342341
smoothingData->setpointCutoffFrequency = MAX(RC_SMOOTHING_CUTOFF_MIN_HZ, calcAutoSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactorSetpoint));
343342
}
344343
if (smoothingData->throttleCutoffSetting == 0) {
345344
smoothingData->throttleCutoffFrequency = MAX(RC_SMOOTHING_CUTOFF_MIN_HZ, calcAutoSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactorThrottle));
346345
}
346+
if (rcSmoothingData.ffCutoffSetting == 0) {
347+
smoothingData->feedforwardCutoffFrequency = MAX(RC_SMOOTHING_CUTOFF_MIN_HZ, calcAutoSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactorSetpoint));
348+
}
349+
// todo add the rc velocity filter boost to ff
350+
if (!smoothingData->filterInitialized) {
351+
pidInitFeedforwardLpf(smoothingData->feedforwardCutoffFrequency, smoothingData->debugAxis);
352+
} else {
353+
pidUpdateFeedforwardLpf(smoothingData->feedforwardCutoffFrequency);
354+
}
355+
}
347356

357+
FAST_CODE_NOINLINE void rcSmoothingVelocityCutoffAdjustment (rcSmoothingFilter_t *smoothingData) {
358+
const float dT = targetPidLooptime * 1e-6f;
359+
float filterVelocityBoost[4];
348360
// initialize or update the Setpoint filter
349-
if ((smoothingData->setpointCutoffFrequency != oldCutoff) || !smoothingData->filterInitialized) {
361+
if (!smoothingData->filterInitialized) {
350362
for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
363+
// rcVelocity of 1 matches to moving your stick to the end of its travel in 1 second
364+
if (i != 3) {
365+
rcVelocity[i] = ABS(rcDeflection[i] - lastRcDeflection[i]) / (smoothingData->averageFrameTimeUs * 1e-6f);
366+
lastRcDeflection[i] = rcDeflection[i];
367+
368+
} else {
369+
float throttle = (rcCommand[i] / 1000.0);
370+
rcVelocity[i] = ABS(throttle - lastRcDeflection[i]) / (smoothingData->averageFrameTimeUs * 1e-6f);
371+
lastRcDeflection[i] = throttle;
372+
}
373+
rcVelocity[i] = pt1FilterApply(&smoothingData->rcVelocityFilter[i], rcVelocity[i]);
374+
filterVelocityBoost[i] = MAX(5.0, rcVelocity[i] * rxConfig()->rcVelocityCutoffBoost / 100.0f);
375+
351376
if (i < THROTTLE) { // Throttle handled by smoothing rcCommand
352377
if (!smoothingData->filterInitialized) {
353378
ptnFilterInit(&smoothingData->filter[i], rxConfig()->rc_smoothing_order, smoothingData->setpointCutoffFrequency, dT);
354379
} else {
355-
ptnFilterUpdate(&smoothingData->filter[i], smoothingData->setpointCutoffFrequency, dT);
380+
ptnFilterUpdate(&smoothingData->filter[i], smoothingData->setpointCutoffFrequency * filterVelocityBoost[i], dT);
356381
}
357382
} else {
358383
if (!smoothingData->filterInitialized) {
359384
ptnFilterInit(&smoothingData->filter[i], rxConfig()->rc_smoothing_order, smoothingData->throttleCutoffFrequency, dT);
360385
} else {
361-
ptnFilterUpdate(&smoothingData->filter[i], smoothingData->throttleCutoffFrequency, dT);
386+
ptnFilterUpdate(&smoothingData->filter[i], smoothingData->throttleCutoffFrequency * filterVelocityBoost[i], dT);
362387
}
363388
}
364389
}
@@ -368,21 +393,10 @@ FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothi
368393
if (!smoothingData->filterInitialized) {
369394
ptnFilterInit(&smoothingData->filterDeflection[i], rxConfig()->rc_smoothing_order, smoothingData->setpointCutoffFrequency, dT);
370395
} else {
371-
ptnFilterUpdate(&smoothingData->filterDeflection[i], smoothingData->setpointCutoffFrequency, dT);
396+
ptnFilterUpdate(&smoothingData->filterDeflection[i], smoothingData->setpointCutoffFrequency * filterVelocityBoost[i], dT);
372397
}
373398
}
374399
}
375-
376-
// update or initialize the FF filter
377-
oldCutoff = smoothingData->feedforwardCutoffFrequency;
378-
if (rcSmoothingData.ffCutoffSetting == 0) {
379-
smoothingData->feedforwardCutoffFrequency = MAX(RC_SMOOTHING_CUTOFF_MIN_HZ, calcAutoSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactorSetpoint));
380-
}
381-
if (!smoothingData->filterInitialized) {
382-
pidInitFeedforwardLpf(smoothingData->feedforwardCutoffFrequency, smoothingData->debugAxis);
383-
} else if (smoothingData->feedforwardCutoffFrequency != oldCutoff) {
384-
pidUpdateFeedforwardLpf(smoothingData->feedforwardCutoffFrequency);
385-
}
386400
}
387401

388402
FAST_CODE_NOINLINE void rcSmoothingResetAccumulation(rcSmoothingFilter_t *smoothingData)
@@ -431,11 +445,13 @@ static FAST_CODE void processRcSmoothingFilter(void)
431445
static FAST_DATA_ZERO_INIT timeMs_t validRxFrameTimeMs;
432446
static FAST_DATA_ZERO_INIT bool calculateCutoffs;
433447

448+
const float dT = targetPidLooptime * 1e-6f;
449+
434450
// first call initialization
435451
if (!initialized) {
436452
initialized = true;
437453
rcSmoothingData.filterInitialized = false;
438-
rcSmoothingData.averageFrameTimeUs = 0;
454+
rcSmoothingData.averageFrameTimeUs = 2000;
439455
rcSmoothingData.autoSmoothnessFactorSetpoint = rxConfig()->rc_smoothing_auto_factor_rpy;
440456
rcSmoothingData.autoSmoothnessFactorThrottle = rxConfig()->rc_smoothing_auto_factor_throttle;
441457
rcSmoothingData.debugAxis = rxConfig()->rc_smoothing_debug_axis;
@@ -445,6 +461,10 @@ static FAST_CODE void processRcSmoothingFilter(void)
445461
rcSmoothingResetAccumulation(&rcSmoothingData);
446462
rcSmoothingData.setpointCutoffFrequency = rcSmoothingData.setpointCutoffSetting;
447463
rcSmoothingData.throttleCutoffFrequency = rcSmoothingData.throttleCutoffSetting;
464+
for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
465+
pt1FilterInit(&rcSmoothingData.rcVelocityFilter[i], pt1FilterGain(rxConfig()->rcVelocityCutoff, dT));
466+
}
467+
448468
if (rcSmoothingData.ffCutoffSetting == 0) {
449469
// calculate and use an initial derivative cutoff until the RC interval is known
450470
const float cutoffFactor = 1.5f / (1.0f + (rcSmoothingData.autoSmoothnessFactorSetpoint / 10.0f));
@@ -459,7 +479,8 @@ static FAST_CODE void processRcSmoothingFilter(void)
459479

460480
// if we don't need to calculate cutoffs dynamically then the filters can be initialized now
461481
if (!calculateCutoffs) {
462-
rcSmoothingSetFilterCutoffs(&rcSmoothingData);
482+
rcSmoothingAutoRxRateCutoffs(&rcSmoothingData);
483+
rcSmoothingVelocityCutoffAdjustment(&rcSmoothingData);
463484
rcSmoothingData.filterInitialized = true;
464485
}
465486
}
@@ -505,12 +526,14 @@ static FAST_CODE void processRcSmoothingFilter(void)
505526
if (rcSmoothingAccumulateSample(&rcSmoothingData, currentRxRefreshRate)) {
506527
// the required number of samples were collected so set the filter cutoffs, but only if smoothing is active
507528
if (rxConfig()->rc_smoothing_mode) {
508-
rcSmoothingSetFilterCutoffs(&rcSmoothingData);
529+
rcSmoothingAutoRxRateCutoffs(&rcSmoothingData);
509530
rcSmoothingData.filterInitialized = true;
510531
}
511532
validRxFrameTimeMs = 0;
512533
}
513534
}
535+
// always update this as dynamically based on rc velocity
536+
rcSmoothingVelocityCutoffAdjustment(&rcSmoothingData);
514537

515538
}
516539
} 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)