Experimental biquad RC fitler instead of RC interpolation

docs/Cli.md

@@ -104,7 +104,7 @@ After restoring it's always a good idea to `dump` or `diff` the settings once ag
 |  rssi_channel  | 0 | RX channel containing the RSSI signal |
 |  rssi_min  | 0 | The minimum RSSI value sent by the receiver, in %. For example, if your receiver's minimum RSSI value shows as 42% in the configurator/OSD set this parameter to 42. See also rssi_max. Note that rssi_min can be set to a value bigger than rssi_max to invert the RSSI calculation (i.e. bigger values mean lower RSSI). |
 |  rssi_max  | 100 | The maximum RSSI value sent by the receiver, in %. For example, if your receiver's maximum RSSI value shows as 83% in the configurator/OSD set this parameter to 83. See also rssi_min. |
-|  rc_smoothing  | ON | Interpolation of Rc data during looptimes when there are no new updates. This gives smoother RC input to PID controller and cleaner PIDsum |
+|  rc_filter_frequency  | 50 | RC data biquad filter cutoff frequency. Lower cutoff frequencies result in smoother response at expense of command control delay. Practical values are 20-50. Set to zero to disable entirely and use unsmoothed RC stick values |
 |  input_filtering_mode  | OFF | Filter out noise from OpenLRS Telemetry RX |
 |  min_throttle  | 1150 | These are min/max values (in us) that are sent to esc when armed. Defaults of 1150/1850 are OK for everyone, for use with AfroESC, they could be set to 1064/1864. |
 |  max_throttle  | 1850 | These are min/max values (in us) that are sent to esc when armed. Defaults of 1150/1850 are OK for everyone, for use with AfroESC, they could be set to 1064/1864. If you have brushed motors, the value should be set to 2000. |

make/source.mk

@@ -64,6 +64,7 @@ COMMON_SRC = \
             fc/fc_hardfaults.c \
             fc/fc_msp.c \
             fc/fc_msp_box.c \
+            fc/rc_smoothing.c \
             fc/rc_adjustments.c \
             fc/rc_controls.c \
             fc/rc_curves.c \

src/main/common/filter.c

@@ -70,7 +70,7 @@ float pt1FilterApply3(pt1Filter_t *filter, float input, float dT)
     return filter->state;
 }
 
-float pt1FilterApply4(pt1Filter_t *filter, float input, uint16_t f_cut, float dT)
+float pt1FilterApply4(pt1Filter_t *filter, float input, float f_cut, float dT)
 {
     // Pre calculate and store RC
     if (!filter->RC) {

src/main/common/filter.h

@@ -61,7 +61,7 @@ void pt1FilterSetTimeConstant(pt1Filter_t *filter, float tau);
 float pt1FilterGetLastOutput(pt1Filter_t *filter);
 float pt1FilterApply(pt1Filter_t *filter, float input);
 float pt1FilterApply3(pt1Filter_t *filter, float input, float dT);
-float pt1FilterApply4(pt1Filter_t *filter, float input, uint16_t f_cut, float dt);
+float pt1FilterApply4(pt1Filter_t *filter, float input, float f_cut, float dt);
 void pt1FilterReset(pt1Filter_t *filter, float input);
 
 void rateLimitFilterInit(rateLimitFilter_t *filter);

src/main/fc/fc_core.c

@@ -53,6 +53,7 @@
 #include "fc/config.h"
 #include "fc/controlrate_profile.h"
 #include "fc/rc_adjustments.h"
+#include "fc/rc_smoothing.h"
 #include "fc/rc_controls.h"
 #include "fc/rc_curves.h"
 #include "fc/rc_modes.h"
@@ -419,6 +420,7 @@ void processRx(timeUs_t currentTimeUs)
 {
     static bool armedBeeperOn = false;
 
+    // Calculate RPY channel data
     calculateRxChannelsAndUpdateFailsafe(currentTimeUs);
 
     // in 3D mode, we need to be able to disarm by switch at any time
@@ -634,44 +636,6 @@ void processRx(timeUs_t currentTimeUs)
 
 }
 
-void filterRc(bool isRXDataNew)
-{
-    static int16_t lastCommand[4] = { 0, 0, 0, 0 };
-    static int16_t deltaRC[4] = { 0, 0, 0, 0 };
-    static int16_t factor, rcInterpolationFactor;
-    static biquadFilter_t filteredCycleTimeState;
-    static bool filterInitialised;
-
-    // Calculate average cycle time (1Hz LPF on cycle time)
-    if (!filterInitialised) {
-        biquadFilterInitLPF(&filteredCycleTimeState, 1, getLooptime());
-        filterInitialised = true;
-    }
-
-    const timeDelta_t filteredCycleTime = biquadFilterApply(&filteredCycleTimeState, (float) cycleTime);
-    rcInterpolationFactor = rxGetRefreshRate() / filteredCycleTime + 1;
-
-    if (isRXDataNew) {
-        for (int channel=0; channel < 4; channel++) {
-            deltaRC[channel] = rcCommand[channel] -  (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
-            lastCommand[channel] = rcCommand[channel];
-        }
-
-        factor = rcInterpolationFactor - 1;
-    } else {
-        factor--;
-    }
-
-    // Interpolate steps of rcCommand
-    if (factor > 0) {
-        for (int channel=0; channel < 4; channel++) {
-            rcCommand[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor;
-         }
-    } else {
-        factor = 0;
-    }
-}
-
 // Function for loop trigger
 void taskGyro(timeUs_t currentTimeUs) {
     // getTaskDeltaTime() returns delta time frozen at the moment of entering the scheduler. currentTime is frozen at the very same point.
@@ -723,8 +687,8 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
 
     annexCode();
 
-    if (rxConfig()->rcSmoothing) {
-        filterRc(isRXDataNew);
+    if (rxConfig()->rcFilterFrequency) {
+        rcInterpolationApply(isRXDataNew);
     }
 
 #if defined(USE_NAV)

src/main/fc/rc_smoothing.c

@@ -0,0 +1,110 @@
+/*
+ * This file is part of INAV.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#include <stdbool.h>
+#include <stdlib.h>
+#include <stdint.h>
+
+#include "platform.h"
+
+#include "blackbox/blackbox.h"
+
+#include "build/debug.h"
+
+#include "common/maths.h"
+#include "common/filter.h"
+
+#include "drivers/time.h"
+
+#include "rx/rx.h"
+
+#include "fc/config.h"
+#include "fc/fc_core.h"
+#include "fc/rc_controls.h"
+#include "fc/rc_smoothing.h"
+#include "fc/runtime_config.h"
+
+#include "flight/mixer.h"
+
+static biquadFilter_t rcSmoothFilter[4];
+static float rcStickUnfiltered[4];
+
+static void rcInterpolationInit(int rcFilterFreqency)
+{
+    for (int stick = 0; stick < 4; stick++) {
+        biquadFilterInitLPF(&rcSmoothFilter[stick], rcFilterFreqency, getLooptime());
+    }
+}
+
+void rcInterpolationApply(bool isRXDataNew)
+{
+    static bool initDone = false;
+    static float initFilterFreqency = 0;
+
+    if (isRXDataNew) {
+        if (!initDone || (initFilterFreqency != rxConfig()->rcFilterFrequency)) {
+            rcInterpolationInit(rxConfig()->rcFilterFrequency);
+            initFilterFreqency = rxConfig()->rcFilterFrequency;
+            initDone = true;
+        }
+
+        for (int stick = 0; stick < 4; stick++) {
+            rcStickUnfiltered[stick] = rcCommand[stick];
+        }
+    }
+
+    // Don't filter if not initialized
+    if (!initDone) {
+        return;
+    }
+
+    for (int stick = 0; stick < 4; stick++) {
+        rcCommand[stick] = biquadFilterApply(&rcSmoothFilter[stick], rcStickUnfiltered[stick]);
+    }
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

src/main/fc/rc_smoothing.h

@@ -0,0 +1,31 @@
+/*
+ * This file is part of INAV.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#pragma once
+
+#include <stdbool.h>
+#include <stdlib.h>
+#include <stdint.h>
+
+void rcInterpolationApply(bool isRXDataNew);

src/main/fc/settings.yaml

@@ -354,9 +354,10 @@ groups:
         field: sbusSyncInterval
         min: 500
         max: 10000
-      - name: rc_smoothing
-        field: rcSmoothing
-        type: bool
+      - name: rc_filter_frequency
+        field: rcFilterFrequency
+        min: 0
+        max: 100
       - name: serialrx_provider
         condition: USE_SERIAL_RX
         table: serial_rx

src/main/rx/rx.c

@@ -100,7 +100,7 @@ timeMs_t rcInvalidPulsPeriod[MAX_SUPPORTED_RC_CHANNEL_COUNT];
 rxRuntimeConfig_t rxRuntimeConfig;
 static uint8_t rcSampleIndex = 0;
 
-PG_REGISTER_WITH_RESET_TEMPLATE(rxConfig_t, rxConfig, PG_RX_CONFIG, 6);
+PG_REGISTER_WITH_RESET_TEMPLATE(rxConfig_t, rxConfig, PG_RX_CONFIG, 7);
 
 #ifndef RX_SPI_DEFAULT_PROTOCOL
 #define RX_SPI_DEFAULT_PROTOCOL 0
@@ -130,7 +130,7 @@ PG_RESET_TEMPLATE(rxConfig_t, rxConfig,
     .rssiMin = RSSI_VISIBLE_VALUE_MIN,
     .rssiMax = RSSI_VISIBLE_VALUE_MAX,
     .sbusSyncInterval = SBUS_DEFAULT_INTERFRAME_DELAY_US,
-    .rcSmoothing = 1,
+    .rcFilterFrequency = 50,
 );
 
 void resetAllRxChannelRangeConfigurations(void)

src/main/rx/rx.h

@@ -129,7 +129,7 @@ typedef struct rxConfig_s {
     uint16_t maxcheck;                      // maximum rc end
     uint16_t rx_min_usec;
     uint16_t rx_max_usec;
-    uint8_t rcSmoothing;                    // Enable/Disable RC filtering
+    uint8_t rcFilterFrequency;              // RC filter cutoff frequency (smoothness vs response sharpness)
 } rxConfig_t;
 
 PG_DECLARE(rxConfig_t, rxConfig);

src/main/target/SPRACINGF3NEO/target.h

@@ -114,6 +114,7 @@
 #undef USE_VTX_FFPV
 #undef USE_VTX_SMARTAUDIO           // Disabled due to flash size
 #undef USE_VTX_TRAMP                // Disabled due to flash size
+#undef USE_PWM_SERVO_DRIVER         // Disabled due to RAM size
 
 #define RTC6705_CS_PIN          PF4
 #define RTC6705_SPI_INSTANCE    SPI3

src/main/target/common.h

@@ -89,6 +89,8 @@
 #define USE_DASHBOARD
 #define DASHBOARD_ARMED_BITMAP
 #define USE_OLED_UG2864
+
+#define USE_PWM_DRIVER_PCA9685
 #endif
 
 #if (FLASH_SIZE > 128)
@@ -125,7 +127,6 @@
 #define USE_SERIALRX_CRSF
 #define USE_PWM_SERVO_DRIVER
 #define USE_SERIAL_PASSTHROUGH
-#define USE_PWM_DRIVER_PCA9685
 #define NAV_MAX_WAYPOINTS       60
 #define MAX_BOOTLOG_ENTRIES     64
 #define USE_RCDEVICE