Compute RMS of filtered gyro data

.vscode/c_cpp_properties.json

@@ -57,7 +57,8 @@
                 "USE_SDCARD_SDIO",
                 "USE_SDCARD",
                 "USE_Q_TUNE",
-                "USE_GYRO_FFT_FILTER"
+                "USE_GYRO_FFT_FILTER",
+                "USE_ADAPTIVE_FILTER",
             ],
             "configurationProvider": "ms-vscode.cmake-tools"
         }

cmake/at32.cmake

@@ -18,6 +18,7 @@ set(CMSIS_DSP_SRC
     BasicMathFunctions/arm_scale_f32.c
     BasicMathFunctions/arm_sub_f32.c
     BasicMathFunctions/arm_mult_f32.c
+    BasicMathFunctions/arm_offset_f32.c
     TransformFunctions/arm_rfft_fast_f32.c
     TransformFunctions/arm_cfft_f32.c
     TransformFunctions/arm_rfft_fast_init_f32.c
@@ -26,6 +27,8 @@ set(CMSIS_DSP_SRC
     CommonTables/arm_common_tables.c
     ComplexMathFunctions/arm_cmplx_mag_f32.c
     StatisticsFunctions/arm_max_f32.c
+    StatisticsFunctions/arm_rms_f32.c
+    StatisticsFunctions/arm_mean_f32.c
 )
 list(TRANSFORM CMSIS_DSP_SRC PREPEND "${CMSIS_DSP_DIR}/Source/")
 

cmake/stm32.cmake

@@ -19,6 +19,7 @@ set(CMSIS_DSP_SRC
     BasicMathFunctions/arm_scale_f32.c
     BasicMathFunctions/arm_sub_f32.c
     BasicMathFunctions/arm_mult_f32.c
+    BasicMathFunctions/arm_offset_f32.c
     TransformFunctions/arm_rfft_fast_f32.c
     TransformFunctions/arm_cfft_f32.c
     TransformFunctions/arm_rfft_fast_init_f32.c
@@ -27,6 +28,8 @@ set(CMSIS_DSP_SRC
     CommonTables/arm_common_tables.c
     ComplexMathFunctions/arm_cmplx_mag_f32.c
     StatisticsFunctions/arm_max_f32.c
+    StatisticsFunctions/arm_rms_f32.c
+    StatisticsFunctions/arm_mean_f32.c
 )
 list(TRANSFORM CMSIS_DSP_SRC PREPEND "${CMSIS_DSP_DIR}/Source/")
 

src/main/CMakeLists.txt

@@ -343,6 +343,8 @@ main_sources(COMMON_SRC
     flight/dynamic_lpf.h
     flight/ez_tune.c
     flight/ez_tune.h
+    flight/adaptive_filter.c
+    flight/adaptive_filter.h
 
     io/adsb.c
     io/beeper.c

src/main/build/debug.h

@@ -72,5 +72,6 @@ typedef enum {
     DEBUG_RATE_DYNAMICS,
     DEBUG_LANDING,
     DEBUG_POS_EST,
+    DEBUG_ADAPTIVE_FILTER,
     DEBUG_COUNT
 } debugType_e;

src/main/fc/fc_tasks.c

@@ -51,6 +51,7 @@
 #include "flight/rpm_filter.h"
 #include "flight/servos.h"
 #include "flight/wind_estimator.h"
+#include "flight/adaptive_filter.h"
 
 #include "navigation/navigation.h"
 
@@ -421,6 +422,10 @@ void fcTasksInit(void)
 #if defined(USE_SMARTPORT_MASTER)
     setTaskEnabled(TASK_SMARTPORT_MASTER, true);
 #endif
+
+#ifdef USE_ADAPTIVE_FILTER
+    setTaskEnabled(TASK_ADAPTIVE_FILTER, true);
+#endif
 }
 
 cfTask_t cfTasks[TASK_COUNT] = {
@@ -672,4 +677,12 @@ cfTask_t cfTasks[TASK_COUNT] = {
         .desiredPeriod = TASK_PERIOD_HZ(TASK_AUX_RATE_HZ),          // 100Hz @10ms
         .staticPriority = TASK_PRIORITY_HIGH,
     },
+#ifdef USE_ADAPTIVE_FILTER
+    [TASK_ADAPTIVE_FILTER] = {
+        .taskName = "ADAPTIVE_FILTER",
+        .taskFunc = adaptiveFilterTask,
+        .desiredPeriod = TASK_PERIOD_HZ(ADAPTIVE_FILTER_RATE_HZ),          // 100Hz @10ms
+        .staticPriority = TASK_PRIORITY_LOW,
+    },
+#endif
 };

src/main/fc/settings.yaml

@@ -88,7 +88,7 @@ tables:
     values: ["NONE", "AGL", "FLOW_RAW", "FLOW", "ALWAYS", "SAG_COMP_VOLTAGE",
       "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC",
       "NAV_YAW", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL", "ALTITUDE",
-      "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST"]
+      "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST", "ADAPTIVE_FILTER"]
   - name: aux_operator
     values: ["OR", "AND"]
     enum: modeActivationOperator_e

src/main/flight/adaptive_filter.c

@@ -0,0 +1,77 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * 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 "platform.h"
+
+#ifdef USE_ADAPTIVE_FILTER
+
+#include <stdlib.h>
+#include "flight/adaptive_filter.h"
+#include "arm_math.h"
+#include <math.h>
+#include "common/maths.h"
+#include "common/axis.h"
+#include "build/debug.h"
+
+static float32_t adaptiveFilterSamples[XYZ_AXIS_COUNT][ADAPTIVE_FILTER_BUFFER_SIZE];
+static uint8_t adaptiveFilterSampleIndex = 0;
+
+void adaptiveFilterPush(const flight_dynamics_index_t index, const float value) {
+    //Push new sample to the buffer so later we can compute RMS and other measures
+    adaptiveFilterSamples[index][adaptiveFilterSampleIndex] = value;
+    adaptiveFilterSampleIndex = (adaptiveFilterSampleIndex + 1) % ADAPTIVE_FILTER_BUFFER_SIZE;
+}
+
+void adaptiveFilterTask(timeUs_t currentTimeUs) {
+    UNUSED(currentTimeUs);
+
+    //Compute RMS for each axis
+    for (flight_dynamics_index_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+
+        //Copy axis samples to a temporary buffer
+        float32_t tempBuffer[ADAPTIVE_FILTER_BUFFER_SIZE];
+        //Use memcpy to copy the samples to the temporary buffer
+        memcpy(tempBuffer, adaptiveFilterSamples[axis], sizeof(adaptiveFilterSamples[axis]));
+
+        //Use arm_mean_f32 to compute the mean of the samples
+        float32_t mean;
+        arm_mean_f32(tempBuffer, ADAPTIVE_FILTER_BUFFER_SIZE, &mean);
+
+        float32_t normalizedBuffer[ADAPTIVE_FILTER_BUFFER_SIZE];
+
+        //Use arm_offset_f32 to subtract the mean from the samples
+        arm_offset_f32(tempBuffer, -mean, normalizedBuffer, ADAPTIVE_FILTER_BUFFER_SIZE);
+
+        //Compute RMS from normalizedBuffer using arm_rms_f32
+        float32_t rms;
+        arm_rms_f32(normalizedBuffer, ADAPTIVE_FILTER_BUFFER_SIZE, &rms);
+
+        DEBUG_SET(DEBUG_ADAPTIVE_FILTER, axis, rms * 1000.0f);
+    }
+
+
+}
+
+
+#endif /* USE_ADAPTIVE_FILTER */

src/main/flight/adaptive_filter.h

@@ -0,0 +1,32 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * 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 "common/axis.h"
+#include "common/time.h"
+
+#define ADAPTIVE_FILTER_BUFFER_SIZE 32
+#define ADAPTIVE_FILTER_RATE_HZ 100
+
+void adaptiveFilterPush(const flight_dynamics_index_t index, const float value);
+void adaptiveFilterTask(timeUs_t currentTimeUs);

src/main/scheduler/scheduler.h

@@ -121,6 +121,9 @@ typedef enum {
 #endif
 #ifdef USE_IRLOCK
     TASK_IRLOCK,
+#endif
+#ifdef USE_ADAPTIVE_FILTER
+    TASK_ADAPTIVE_FILTER,
 #endif
     /* Count of real tasks */
     TASK_COUNT,

src/main/sensors/gyro.c

@@ -68,6 +68,7 @@
 #include "flight/gyroanalyse.h"
 #include "flight/rpm_filter.h"
 #include "flight/kalman.h"
+#include "flight/adaptive_filter.h"
 
 #ifdef USE_HARDWARE_REVISION_DETECTION
 #include "hardware_revision.h"
@@ -453,6 +454,8 @@ void FAST_CODE NOINLINE gyroFilter(void)
 
         gyroADCf = gyroLpf2ApplyFn((filter_t *) &gyroLpf2State[axis], gyroADCf);
 
+        adaptiveFilterPush(axis, gyroADCf);
+
 #ifdef USE_DYNAMIC_FILTERS
         if (dynamicGyroNotchState.enabled) {
             gyroDataAnalysePush(&gyroAnalyseState, axis, gyroADCf);

src/main/target/common.h

@@ -203,3 +203,9 @@
 #define MAX_MIXER_PROFILE_COUNT 1
 #endif
 #define USE_EZ_TUNE
+
+#ifdef STM32H7
+
+#define USE_ADAPTIVE_FILTER
+
+#endif