choose ptn filter order for rc filtering

emuflight · May 24, 2022 · fd40b1f · fd40b1f
1 parent 7015291
commit fd40b1f
Show file tree

Hide file tree

Showing 7 changed files with 64 additions and 17 deletions.
diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c
@@ -756,6 +756,7 @@ const clivalue_t valueTable[] = {
 
 #ifdef USE_RC_SMOOTHING_FILTER
     { PARAM_NAME_RC_SMOOTHING,                   VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_mode) },
+    { "rc_smoothing_order",                      VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 1, 3 }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_mode) },
     { 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) },

diff --git a/src/main/common/filter.c b/src/main/common/filter.c
@@ -309,3 +309,37 @@ void simpleLPFilterInit(simpleLowpassFilter_t *filter, int32_t beta, int32_t fpS
     filter->beta = beta;
     filter->fpShift = fpShift;
 }
+
+const float PTN_SCALE[3] = { 1.0f, 1.553773974f, 1.961459177f };
+
+void ptnFilterInit(ptnFilter_t *filter, uint8_t order, uint16_t f_cut, float dT) {
+
+	  // AdjCutHz = CutHz /(sqrtf(powf(2, 1/Order) -1))
+    const float ScaleF[] = { 1.0f, 1.553773974f, 1.961459177f };
+    float Adj_f_cut;
+
+	  filter->order = (order > 3) ? 3 : order;
+	  for (int n = 1; n <= filter->order; n++) {
+		    filter->state[n] = 0.0f;
+    }
+
+	  Adj_f_cut = f_cut * PTN_SCALE[filter->order - 1];
+
+	  filter->k = dT / ((1.0f / (2.0f * M_PIf * Adj_f_cut)) + dT);
+} // ptnFilterInit
+
+FAST_CODE void ptnFilterUpdate(ptnFilter_t *filter, float f_cut, float dT) {
+    float Adj_f_cut;
+    Adj_f_cut = f_cut * PTN_SCALE[filter->order - 1];
+    filter->k = dT / ((1.0f / (2.0f * M_PIf * Adj_f_cut)) + dT);
+}
+
+FAST_CODE float ptnFilterApply(ptnFilter_t *filter, float input) {
+    filter->state[0] = input;
+
+	  for (int n = 1; n <= filter->order; n++) {
+		    filter->state[n] += (filter->state[n - 1] - filter->state[n]) * filter->k;
+    }
+
+	  return filter->state[filter->order];
+} // ptnFilterApply
diff --git a/src/main/common/filter.h b/src/main/common/filter.h
@@ -63,6 +63,18 @@ typedef struct laggedMovingAverage_s {
     bool primed;
 } laggedMovingAverage_t;
 
+typedef struct simpleLowpassFilter_s {
+    int32_t fp;
+    int32_t beta;
+    int32_t fpShift;
+} simpleLowpassFilter_t;
+
+typedef struct ptnFilter_s {
+    float state[4];
+    float k;
+    uint8_t order;
+} ptnFilter_t;
+
 typedef enum {
     FILTER_PT1 = 0,
     FILTER_BIQUAD,
@@ -111,11 +123,9 @@ float pt3FilterApply(pt3Filter_t *filter, float input);
 void slewFilterInit(slewFilter_t *filter, float slewLimit, float threshold);
 float slewFilterApply(slewFilter_t *filter, float input);
 
-typedef struct simpleLowpassFilter_s {
-    int32_t fp;
-    int32_t beta;
-    int32_t fpShift;
-} simpleLowpassFilter_t;
-
 int32_t simpleLPFilterUpdate(simpleLowpassFilter_t *filter, int32_t newVal);
 void simpleLPFilterInit(simpleLowpassFilter_t *filter, int32_t beta, int32_t fpShift);
+
+void ptnFilterInit(ptnFilter_t *filter, uint8_t order, uint16_t f_cut, float dT);
+void ptnFilterUpdate(ptnFilter_t *filter, float f_cut, float dt);
+float ptnFilterApply(ptnFilter_t *filter, float input);
diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c
@@ -350,25 +350,25 @@ FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothi
         for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
             if (i < THROTTLE) { // Throttle handled by smoothing rcCommand
                 if (!smoothingData->filterInitialized) {
-                    pt3FilterInit(&smoothingData->filter[i], pt3FilterGain(smoothingData->setpointCutoffFrequency, dT));
+                    ptnFilterInit(&smoothingData->filter[i], rxConfig()->rc_smoothing_order, smoothingData->setpointCutoffFrequency, dT);
                 } else {
-                    pt3FilterUpdateCutoff(&smoothingData->filter[i], pt3FilterGain(smoothingData->setpointCutoffFrequency, dT));
+                    ptnFilterUpdate(&smoothingData->filter[i], smoothingData->setpointCutoffFrequency, dT);
                 }
             } else {
                 if (!smoothingData->filterInitialized) {
-                    pt3FilterInit(&smoothingData->filter[i], pt3FilterGain(smoothingData->throttleCutoffFrequency, dT));
+                    ptnFilterInit(&smoothingData->filter[i], rxConfig()->rc_smoothing_order, smoothingData->throttleCutoffFrequency, dT);
                 } else {
-                    pt3FilterUpdateCutoff(&smoothingData->filter[i], pt3FilterGain(smoothingData->throttleCutoffFrequency, dT));
+                    ptnFilterUpdate(&smoothingData->filter[i], smoothingData->throttleCutoffFrequency, dT);
                 }
             }
         }
 
         // initialize or update the Level filter
         for (int i = FD_ROLL; i < FD_YAW; i++) {
             if (!smoothingData->filterInitialized) {
-                pt3FilterInit(&smoothingData->filterDeflection[i], pt3FilterGain(smoothingData->setpointCutoffFrequency, dT));
+                ptnFilterInit(&smoothingData->filterDeflection[i], rxConfig()->rc_smoothing_order, smoothingData->setpointCutoffFrequency, dT);
             } else {
-                pt3FilterUpdateCutoff(&smoothingData->filterDeflection[i], pt3FilterGain(smoothingData->setpointCutoffFrequency, dT));
+                ptnFilterUpdate(&smoothingData->filterDeflection[i], smoothingData->setpointCutoffFrequency, dT);
             }
         }
     }
@@ -546,7 +546,7 @@ static FAST_CODE void processRcSmoothingFilter(void)
     for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
         float *dst = i == THROTTLE ? &rcCommand[i] : &setpointRate[i];
         if (rcSmoothingData.filterInitialized) {
-            *dst = pt3FilterApply(&rcSmoothingData.filter[i], rxDataToSmooth[i]);
+            *dst = ptnFilterApply(&rcSmoothingData.filter[i], rxDataToSmooth[i]);
         } else {
             // If filter isn't initialized yet, as in smoothing off, use the actual unsmoothed rx channel data
             *dst = rxDataToSmooth[i];
@@ -557,7 +557,7 @@ static FAST_CODE void processRcSmoothingFilter(void)
     bool smoothingNeeded = (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && rcSmoothingData.filterInitialized;
     for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
         if (smoothingNeeded && axis < FD_YAW) {
-            rcDeflectionSmoothed[axis] = pt3FilterApply(&rcSmoothingData.filterDeflection[axis], rcDeflection[axis]);
+            rcDeflectionSmoothed[axis] = ptnFilterApply(&rcSmoothingData.filterDeflection[axis], rcDeflection[axis]);
         } else {
             rcDeflectionSmoothed[axis] = rcDeflection[axis];
         }
@@ -586,7 +586,7 @@ FAST_CODE void processRcCommand(void)
 #endif
 
             float angleRate;
-            
+
 #ifdef USE_GPS_RESCUE
             if ((axis == FD_YAW) && FLIGHT_MODE(GPS_RESCUE_MODE)) {
                 // If GPS Rescue is active then override the setpointRate used in the

diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h
@@ -95,8 +95,8 @@ typedef struct rcSmoothingFilterTraining_s {
 
 typedef struct rcSmoothingFilter_s {
     bool filterInitialized;
-    pt3Filter_t filter[4];
-    pt3Filter_t filterDeflection[2];
+    ptnFilter_t filter[4];
+    ptnFilter_t filterDeflection[2];
     uint8_t setpointCutoffSetting;
     uint8_t throttleCutoffSetting;
     uint16_t setpointCutoffFrequency;

diff --git a/src/main/pg/rx.c b/src/main/pg/rx.c
@@ -60,6 +60,7 @@ void pgResetFn_rxConfig(rxConfig_t *rxConfig)
         .airModeActivateThreshold = 25,
         .max_aux_channel = DEFAULT_AUX_CHANNEL_COUNT,
         .rc_smoothing_mode = 1,
+        .rc_smoothing_order = 3,
         .rc_smoothing_setpoint_cutoff = 0,
         .rc_smoothing_feedforward_cutoff = 0,
         .rc_smoothing_throttle_cutoff = 0,

diff --git a/src/main/pg/rx.h b/src/main/pg/rx.h
@@ -50,6 +50,7 @@ typedef struct rxConfig_s {
     uint8_t rssi_src_frame_errors;             // true to use frame drop flags in the rx protocol
     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_order;                // choose from pt1-pt4 for rc smoothing
     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)