Basic approach to adaptive filter.

src/main/flight/adaptive_filter.c

@@ -35,6 +35,7 @@
 #include "common/filter.h"
 #include "build/debug.h"
 #include "fc/config.h"
+#include "sensors/gyro.h"
 
 STATIC_FASTRAM float32_t adaptiveFilterSamples[XYZ_AXIS_COUNT][ADAPTIVE_FILTER_BUFFER_SIZE];
 STATIC_FASTRAM uint8_t adaptiveFilterSampleIndex = 0;
@@ -52,6 +53,12 @@ STATIC_FASTRAM float axisAttenuationFactor[XYZ_AXIS_COUNT];
 STATIC_FASTRAM uint8_t adaptiveFilterInitialized = 0;
 STATIC_FASTRAM uint8_t hpfFilterInitialized = 0;
 
+//Defines if current, min and max  values for the filter were set and filter is ready to work
+STATIC_FASTRAM uint8_t targetsSet = 0;
+STATIC_FASTRAM float currentLpf;
+STATIC_FASTRAM float minLpf;
+STATIC_FASTRAM float maxLpf; 
+
 STATIC_FASTRAM float adaptiveFilterIntegrator;
 STATIC_FASTRAM float adaptiveIntegratorTarget;
 
@@ -82,7 +89,24 @@ void adaptiveFilterPushRate(const flight_dynamics_index_t index, const float rat
     axisAttenuationFactor[index] = constrainf(axisAttenuationFactor[index], 0.0f, 1.0f);
 }
 
+void adaptiveFilterResetIntegrator(void) {
+    adaptiveFilterIntegrator = 0.0f;
+}
+
+void adaptiveFilterSetDefaultFrequency(int lpf, int min, int max) {
+    currentLpf = lpf;
+    minLpf = min;
+    maxLpf = max;
+    targetsSet = 1;
+}
+
 void adaptiveFilterTask(timeUs_t currentTimeUs) {
+
+    //If we don't have current, min and max values for the filter, we can't run it yet
+    if (!targetsSet) {
+        return;
+    }
+
     static timeUs_t previousUpdateTimeUs = 0;
 
     //Initialization procedure, filter setup etc.
@@ -127,18 +151,37 @@ void adaptiveFilterTask(timeUs_t currentTimeUs) {
 
         combinedStd += std;
 
-        if (axis == 0) {
+        // if (axis == 0) {
-            DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 2, filteredStd * 1000.0f);
+        //     DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 2, filteredStd * 1000.0f);
-            DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 3, error * 1000.0f);
+        //     DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 3, error * 1000.0f);
-            DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 4, adjustedError * 1000.0f);
+        //     DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 4, adjustedError * 1000.0f);
-            DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 5, timeAdjustedError * 1000.0f);
+        //     DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 5, timeAdjustedError * 1000.0f);
-        }
+        // }
     }
 
+    //TODO filter gets updated only when ARMED
+
+    if (adaptiveFilterIntegrator > ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD) {
+        //In this case there is too much noise, we need to lower the LPF frequency
+        currentLpf = constrainf(currentLpf - 1.0f, minLpf, maxLpf);
+        gyroUpdateDynamicLpf(currentLpf);
+        adaptiveFilterResetIntegrator();
+    } else if (adaptiveFilterIntegrator < -ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD) {
+        //In this case there is too little noise, we can to increase the LPF frequency
+        currentLpf = constrainf(currentLpf + 1.0f, minLpf, maxLpf);
+        gyroUpdateDynamicLpf(currentLpf);
+        adaptiveFilterResetIntegrator();
+    }
+
+    // if (ARMING_FLAG(ARMED)) {
+    //     // 
+    // }
+
     combinedStd /= XYZ_AXIS_COUNT;
 
     DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 0, combinedStd * 1000.0f);
     DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 1, adaptiveFilterIntegrator);
+    DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 2, currentLpf);
 }
 
 

src/main/flight/adaptive_filter.h

@@ -29,7 +29,10 @@
 #define ADAPTIVE_FILTER_RATE_HZ 100
 #define ADAPTIVE_FILTER_LPF_HZ 1
 #define ADAPTIVE_FILTER_HPF_HZ 15
+#define ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD 20.0f
 
 void adaptiveFilterPush(const flight_dynamics_index_t index, const float value);
 void adaptiveFilterPushRate(const flight_dynamics_index_t index, const float rate, const uint8_t configRate);
+void adaptiveFilterResetIntegrator(void);
+void adaptiveFilterSetDefaultFrequency(int lpf, int min, int max);
 void adaptiveFilterTask(timeUs_t currentTimeUs);

src/main/sensors/gyro.c

@@ -264,6 +264,10 @@ static void gyroInitFilters(void)
     //Second gyro LPF runnig and PID frequency - this filter is dynamic when gyro_use_dyn_lpf = ON
     initGyroFilter(&gyroLpf2ApplyFn, gyroLpf2State, gyroConfig()->gyro_main_lpf_type, gyroConfig()->gyro_main_lpf_hz, getLooptime());
 
+#ifdef USE_ADAPTIVE_FILTER
+    adaptiveFilterSetDefaultFrequency(gyroConfig()->gyro_main_lpf_hz, 50, 150);
+#endif
+
 #ifdef USE_GYRO_KALMAN
     if (gyroConfig()->kalmanEnabled) {
         gyroKalmanInitialize(gyroConfig()->kalman_q);