rc velocity filter boost

emuflight · May 24, 2022 · c47d0e9 · c47d0e9
1 parent f220ccf
commit c47d0e9
Show file tree

Hide file tree

Showing 5 changed files with 52 additions and 22 deletions.
diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c
@@ -763,6 +763,8 @@ const clivalue_t valueTable[] = {
     { 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) },
+    { "rc_velocity_boost_cutoff_hz",              VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 1, 255 }, PG_RX_CONFIG, offsetof(rxConfig_t, rcVelocityCutoff) },
+    { "rc_velocity_boost",                        VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_RX_CONFIG, offsetof(rxConfig_t, rcVelocityCutoffBoost) },
 #endif // USE_RC_SMOOTHING_FILTER
 
     { "fpv_mix_degrees",             VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 90 }, PG_RX_CONFIG, offsetof(rxConfig_t, fpvCamAngleDegrees) },

diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c
@@ -64,6 +64,8 @@ float rcCommandDelta[XYZ_AXIS_COUNT];
 #endif
 static float rawSetpoint[XYZ_AXIS_COUNT];
 static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3];
+FAST_DATA_ZERO_INIT float lastRcDeflection[4], rcVelocity[4];
+static float rcDeflectionSmoothed[3];
 static bool reverseMotors = false;
 static applyRatesFn *applyRates;
 static uint16_t currentRxRefreshRate;
@@ -333,32 +335,55 @@ FAST_CODE_NOINLINE int calcAutoSmoothingCutoff(int avgRxFrameTimeUs, uint8_t aut
 
 // Initialize or update the filters base on either the manually selected cutoff, or
 // the auto-calculated cutoff frequency based on detected rx frame rate.
-FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothingData)
+FAST_CODE_NOINLINE void rcSmoothingAutoRxRateCutoffs(rcSmoothingFilter_t *smoothingData)
 {
-    const float dT = targetPidLooptime * 1e-6f;
-    uint16_t oldCutoff = smoothingData->setpointCutoffFrequency;
-
     if (smoothingData->setpointCutoffSetting == 0) {
         smoothingData->setpointCutoffFrequency = MAX(RC_SMOOTHING_CUTOFF_MIN_HZ, calcAutoSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactorSetpoint));
     }
     if (smoothingData->throttleCutoffSetting == 0) {
         smoothingData->throttleCutoffFrequency = MAX(RC_SMOOTHING_CUTOFF_MIN_HZ, calcAutoSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactorThrottle));
     }
+    if (rcSmoothingData.ffCutoffSetting == 0) {
+        smoothingData->feedforwardCutoffFrequency = MAX(RC_SMOOTHING_CUTOFF_MIN_HZ, calcAutoSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactorSetpoint));
+    }
+    // todo add the rc velocity filter boost to ff
+    if (!smoothingData->filterInitialized) {
+        pidInitFeedforwardLpf(smoothingData->feedforwardCutoffFrequency, smoothingData->debugAxis);
+    } else {
+        pidUpdateFeedforwardLpf(smoothingData->feedforwardCutoffFrequency);
+    }
+}
 
+FAST_CODE_NOINLINE void rcSmoothingVelocityCutoffAdjustment (rcSmoothingFilter_t *smoothingData) {
+    const float dT = targetPidLooptime * 1e-6f;
+    float filterVelocityBoost[4];
     // initialize or update the Setpoint filter
-    if ((smoothingData->setpointCutoffFrequency != oldCutoff) || !smoothingData->filterInitialized) {
+    if (!smoothingData->filterInitialized) {
         for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
+          // rcVelocity of 1 matches to moving your stick to the end of its travel in 1 second
+            if (i != 3) {
+            rcVelocity[i] = ABS(rcDeflection[i] - lastRcDeflection[i]) / (smoothingData->averageFrameTimeUs * 1e-6f);
+            lastRcDeflection[i] = rcDeflection[i];
+
+          } else {
+            float throttle = (rcCommand[i] / 1000.0);
+            rcVelocity[i] = ABS(throttle - lastRcDeflection[i]) / (smoothingData->averageFrameTimeUs * 1e-6f);
+            lastRcDeflection[i] = throttle;
+          }
+            rcVelocity[i] = pt1FilterApply(&smoothingData->rcVelocityFilter[i], rcVelocity[i]);
+            filterVelocityBoost[i] = MAX(5.0, rcVelocity[i] * rxConfig()->rcVelocityCutoffBoost / 100.0f);
+
             if (i < THROTTLE) { // Throttle handled by smoothing rcCommand
                 if (!smoothingData->filterInitialized) {
                     ptnFilterInit(&smoothingData->filter[i], rxConfig()->rc_smoothing_order, smoothingData->setpointCutoffFrequency, dT);
                 } else {
-                    ptnFilterUpdate(&smoothingData->filter[i], smoothingData->setpointCutoffFrequency, dT);
+                    ptnFilterUpdate(&smoothingData->filter[i], smoothingData->setpointCutoffFrequency * filterVelocityBoost[i], dT);
                 }
             } else {
                 if (!smoothingData->filterInitialized) {
                     ptnFilterInit(&smoothingData->filter[i], rxConfig()->rc_smoothing_order, smoothingData->throttleCutoffFrequency, dT);
                 } else {
-                    ptnFilterUpdate(&smoothingData->filter[i], smoothingData->throttleCutoffFrequency, dT);
+                    ptnFilterUpdate(&smoothingData->filter[i], smoothingData->throttleCutoffFrequency * filterVelocityBoost[i], dT);
                 }
             }
         }
@@ -368,21 +393,10 @@ FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothi
             if (!smoothingData->filterInitialized) {
                 ptnFilterInit(&smoothingData->filterDeflection[i], rxConfig()->rc_smoothing_order, smoothingData->setpointCutoffFrequency, dT);
             } else {
-                ptnFilterUpdate(&smoothingData->filterDeflection[i], smoothingData->setpointCutoffFrequency, dT);
+                ptnFilterUpdate(&smoothingData->filterDeflection[i], smoothingData->setpointCutoffFrequency * filterVelocityBoost[i], dT);
             }
         }
     }
-
-    // update or initialize the FF filter
-    oldCutoff = smoothingData->feedforwardCutoffFrequency;
-    if (rcSmoothingData.ffCutoffSetting == 0) {
-        smoothingData->feedforwardCutoffFrequency = MAX(RC_SMOOTHING_CUTOFF_MIN_HZ, calcAutoSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactorSetpoint));
-    }
-    if (!smoothingData->filterInitialized) {
-        pidInitFeedforwardLpf(smoothingData->feedforwardCutoffFrequency, smoothingData->debugAxis);
-    } else if (smoothingData->feedforwardCutoffFrequency != oldCutoff) {
-        pidUpdateFeedforwardLpf(smoothingData->feedforwardCutoffFrequency);
-    }
 }
 
 FAST_CODE_NOINLINE void rcSmoothingResetAccumulation(rcSmoothingFilter_t *smoothingData)
@@ -431,11 +445,13 @@ static FAST_CODE void processRcSmoothingFilter(void)
     static FAST_DATA_ZERO_INIT timeMs_t validRxFrameTimeMs;
     static FAST_DATA_ZERO_INIT bool calculateCutoffs;
 
+    const float dT = targetPidLooptime * 1e-6f;
+
     // first call initialization
     if (!initialized) {
         initialized = true;
         rcSmoothingData.filterInitialized = false;
-        rcSmoothingData.averageFrameTimeUs = 0;
+        rcSmoothingData.averageFrameTimeUs = 2000;
         rcSmoothingData.autoSmoothnessFactorSetpoint = rxConfig()->rc_smoothing_auto_factor_rpy;
         rcSmoothingData.autoSmoothnessFactorThrottle = rxConfig()->rc_smoothing_auto_factor_throttle;
         rcSmoothingData.debugAxis = rxConfig()->rc_smoothing_debug_axis;
@@ -445,6 +461,10 @@ static FAST_CODE void processRcSmoothingFilter(void)
         rcSmoothingResetAccumulation(&rcSmoothingData);
         rcSmoothingData.setpointCutoffFrequency = rcSmoothingData.setpointCutoffSetting;
         rcSmoothingData.throttleCutoffFrequency = rcSmoothingData.throttleCutoffSetting;
+        for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
+            pt1FilterInit(&rcSmoothingData.rcVelocityFilter[i], pt1FilterGain(rxConfig()->rcVelocityCutoff, dT));
+        }
+
         if (rcSmoothingData.ffCutoffSetting == 0) {
             // calculate and use an initial derivative cutoff until the RC interval is known
             const float cutoffFactor = 1.5f / (1.0f + (rcSmoothingData.autoSmoothnessFactorSetpoint / 10.0f));
@@ -459,7 +479,8 @@ static FAST_CODE void processRcSmoothingFilter(void)
 
             // if we don't need to calculate cutoffs dynamically then the filters can be initialized now
             if (!calculateCutoffs) {
-                rcSmoothingSetFilterCutoffs(&rcSmoothingData);
+                rcSmoothingAutoRxRateCutoffs(&rcSmoothingData);
+                rcSmoothingVelocityCutoffAdjustment(&rcSmoothingData);
                 rcSmoothingData.filterInitialized = true;
             }
         }
@@ -505,12 +526,14 @@ static FAST_CODE void processRcSmoothingFilter(void)
                             if (rcSmoothingAccumulateSample(&rcSmoothingData, currentRxRefreshRate)) {
                                 // the required number of samples were collected so set the filter cutoffs, but only if smoothing is active
                                 if (rxConfig()->rc_smoothing_mode) {
-                                    rcSmoothingSetFilterCutoffs(&rcSmoothingData);
+                                    rcSmoothingAutoRxRateCutoffs(&rcSmoothingData);
                                     rcSmoothingData.filterInitialized = true;
                                 }
                                 validRxFrameTimeMs = 0;
                             }
                         }
+                        // always update this as dynamically based on rc velocity
+                        rcSmoothingVelocityCutoffAdjustment(&rcSmoothingData);
 
                     }
                 } else {

diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h
@@ -108,6 +108,7 @@ typedef struct rcSmoothingFilter_s {
     uint8_t debugAxis;
     uint8_t autoSmoothnessFactorSetpoint;
     uint8_t autoSmoothnessFactorThrottle;
+    pt1Filter_t rcVelocityFilter[4];
 } rcSmoothingFilter_t;
 
 typedef struct rcControlsConfig_s {

diff --git a/src/main/pg/rx.c b/src/main/pg/rx.c
@@ -67,6 +67,8 @@ void pgResetFn_rxConfig(rxConfig_t *rxConfig)
         .rc_smoothing_debug_axis = ROLL,
         .rc_smoothing_auto_factor_rpy = 30,
         .rc_smoothing_auto_factor_throttle = 30,
+        .rcVelocityCutoffBoost = 75,
+        .rcVelocityCutoff = 15,
         .srxl2_unit_id = 1,
         .srxl2_baud_fast = true,
         .sbus_baud_fast = false,

diff --git a/src/main/pg/rx.h b/src/main/pg/rx.h
@@ -57,6 +57,8 @@ typedef struct rxConfig_s {
     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
     uint8_t rc_smoothing_auto_factor_throttle; // Used to adjust the "smoothness" determined by the auto cutoff calculations
+    uint8_t rcVelocityCutoffBoost;
+    uint8_t rcVelocityCutoff;
     uint8_t rssi_src_frame_lpf_period;         // Period of the cutoff frequency for the source frame RSSI filter (in 0.1 s)
     uint8_t srxl2_unit_id;                     // Spektrum SRXL2 RX unit id
     uint8_t srxl2_baud_fast;                   // Select Spektrum SRXL2 fast baud rate