Merge pull request #2474 from martinbudden/bf_gyro_analysis_hooks

Added hooks for gyro data analysis

Makefile

@@ -643,6 +643,7 @@ COMMON_SRC = \
             sensors/boardalignment.c \
             sensors/compass.c \
             sensors/gyro.c \
+            sensors/gyroanalyse.c \
             sensors/initialisation.c \
             blackbox/blackbox.c \
             blackbox/blackbox_io.c \
@@ -748,6 +749,7 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \
             sensors/acceleration.c \
             sensors/boardalignment.c \
             sensors/gyro.c \
+            sensors/gyroanalyse.c \
             $(CMSIS_SRC) \
             $(DEVICE_STDPERIPH_SRC) \
             drivers/display_ug2864hsweg01.c \
@@ -873,6 +875,25 @@ else ifeq ($(TARGET),$(filter $(TARGET),$(F1_TARGETS)))
 TARGET_SRC := $(STARTUP_SRC) $(STM32F10x_COMMON_SRC) $(TARGET_SRC)
 endif
 
+ifneq ($(filter GYROFFT,$(FEATURES)),)
+DSPLIB := $(ROOT)/lib/main/DSP_Lib
+DEVICE_FLAGS += -DARM_MATH_CM4 -DARM_MATH_MATRIX_CHECK -DARM_MATH_ROUNDING -D__FPU_PRESENT=1 -DUNALIGNED_SUPPORT_DISABLE
+
+INCLUDE_DIRS += $(DSPLIB)/Include
+
+TARGET_SRC += $(DSPLIB)/Source/TransformFunctions/arm_rfft_fast_f32.c
+TARGET_SRC += $(DSPLIB)/Source/TransformFunctions/arm_cfft_f32.c
+TARGET_SRC += $(DSPLIB)/Source/TransformFunctions/arm_rfft_fast_init_f32.c
+TARGET_SRC += $(DSPLIB)/Source/TransformFunctions/arm_cfft_radix8_f32.c
+TARGET_SRC += $(DSPLIB)/Source/CommonTables/arm_common_tables.c
+
+TARGET_SRC += $(DSPLIB)/Source/ComplexMathFunctions/arm_cmplx_mag_f32.c
+TARGET_SRC += $(DSPLIB)/Source/StatisticsFunctions/arm_max_f32.c
+
+TARGET_SRC += $(wildcard $(DSPLIB)/Source/*/*.S)
+endif
+
+
 ifneq ($(filter ONBOARDFLASH,$(FEATURES)),)
 TARGET_SRC += \
             drivers/flash_m25p16.c \

lib/main/CMSIS/CM4/CoreSupport/arm_math.h

@@ -5211,8 +5211,11 @@ void arm_rfft_fast_f32(
     /* acc += y[n-1] */
     acc += (q31_t) S->state[2] << 15;
 
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wpedantic"
     /* saturate the output */
     out = (q15_t) (__SSAT((acc >> 15), 16));
+#pragma GCC diagnostic push
 
     /* Update state */
     S->state[1] = S->state[0];

lib/main/DSP_Lib/Source/CommonTables/arm_common_tables.c

@@ -41,6 +41,7 @@
 
 #include "arm_math.h"
 #include "arm_common_tables.h"
+#pragma GCC diagnostic ignored "-Woverflow"
 
 /**    
  * @ingroup groupTransforms    

src/main/drivers/accgyro.h

@@ -50,7 +50,9 @@ typedef struct gyroDev_s {
     sensorGyroUpdateFuncPtr update;
     extiCallbackRec_t exti;
     float scale;                                            // scalefactor
-    volatile int16_t gyroADCRaw[XYZ_AXIS_COUNT];
+    int16_t gyroADCRaw[XYZ_AXIS_COUNT];
+    int32_t gyroZero[XYZ_AXIS_COUNT];
+    int32_t gyroADC[XYZ_AXIS_COUNT];                        // gyro data after calibration and alignment
     uint8_t lpf;
     gyroRateKHz_e gyroRateKHz;
     uint8_t mpuDividerDrops;

src/main/drivers/exti.h

@@ -18,6 +18,8 @@
 
 #pragma once
 
+#include <stdbool.h>
+
 #include "io_types.h"
 
 // old EXTI interface, to be replaced

src/main/drivers/sensor.h

@@ -17,6 +17,9 @@
 
 #pragma once
 
+#include <stdbool.h>
+#include <stdint.h>
+
 typedef enum {
     ALIGN_DEFAULT = 0,                                      // driver-provided alignment
     CW0_DEG = 1,

src/main/fc/fc_tasks.c

@@ -72,6 +72,7 @@
 #include "sensors/battery.h"
 #include "sensors/compass.h"
 #include "sensors/gyro.h"
+#include "sensors/gyroanalyse.h"
 #include "sensors/sonar.h"
 #include "sensors/esc_sensor.h"
 
@@ -308,6 +309,9 @@ void fcTasksInit(void)
     setTaskEnabled(TASK_VTXCTRL, true);
 #endif
 #endif
+#ifdef USE_GYRO_DATA_ANALYSE
+    setTaskEnabled(TASK_GYRO_DATA_ANALYSE, true);
+#endif
 }
 
 cfTask_t cfTasks[TASK_COUNT] = {
@@ -409,7 +413,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
     [TASK_SONAR] = {
         .taskName = "SONAR",
         .taskFunc = sonarUpdate,
-        .desiredPeriod = TASK_PERIOD_MS(70),        // 70ms required so that SONAR pulses do not interfer with each other
+        .desiredPeriod = TASK_PERIOD_MS(70),        // 70ms required so that SONAR pulses do not interfere with each other
         .staticPriority = TASK_PRIORITY_LOW,
     },
 #endif
@@ -479,7 +483,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
     [TASK_ESC_SENSOR] = {
         .taskName = "ESC_SENSOR",
         .taskFunc = escSensorProcess,
-        .desiredPeriod = TASK_PERIOD_HZ(100),       // 100 Hz every 10ms
+        .desiredPeriod = TASK_PERIOD_HZ(100),       // 100 Hz, 10ms
         .staticPriority = TASK_PRIORITY_LOW,
     },
 #endif
@@ -506,8 +510,17 @@ cfTask_t cfTasks[TASK_COUNT] = {
     [TASK_VTXCTRL] = {
         .taskName = "VTXCTRL",
         .taskFunc = taskVtxControl,
-        .desiredPeriod = TASK_PERIOD_HZ(5),          // 5Hz @200msec
+        .desiredPeriod = TASK_PERIOD_HZ(5),          // 5 Hz, 200ms
         .staticPriority = TASK_PRIORITY_IDLE,
     },
 #endif
+
+#ifdef USE_GYRO_DATA_ANALYSE
+    [TASK_GYRO_DATA_ANALYSE] = {
+        .taskName = "GYROFFT",
+        .taskFunc = gyroDataAnalyseUpdate,
+        .desiredPeriod = TASK_PERIOD_HZ(100),        // 100 Hz, 10ms
+        .staticPriority = TASK_PRIORITY_MEDIUM,
+    },
+#endif
 };

src/main/scheduler/scheduler.h

@@ -105,6 +105,9 @@ typedef enum {
 #ifdef VTX_CONTROL
     TASK_VTXCTRL,
 #endif
+#ifdef USE_GYRO_DATA_ANALYSE
+    TASK_GYRO_DATA_ANALYSE,
+#endif
 
     /* Count of real tasks */
     TASK_COUNT,

src/main/sensors/gyro.c

@@ -62,6 +62,7 @@
 
 #include "sensors/boardalignment.h"
 #include "sensors/gyro.h"
+#include "sensors/gyroanalyse.h"
 #include "sensors/sensors.h"
 
 #ifdef USE_HARDWARE_REVISION_DETECTION
@@ -73,9 +74,6 @@ gyro_t gyro;
 STATIC_UNIT_TESTED gyroDev_t gyroDev0;
 static int16_t gyroTemperature0;
 
-static int32_t gyroADC[XYZ_AXIS_COUNT];
-
-STATIC_UNIT_TESTED int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 };
 static uint16_t calibratingG = 0;
 
 static filterApplyFnPtr softLpfFilterApplyFn;
@@ -371,6 +369,9 @@ void gyroInitFilters(void)
             biquadFilterInit(notchFilter2[axis], gyroConfig()->gyro_soft_notch_hz_2, gyro.targetLooptime, gyroSoftNotchQ2, FILTER_NOTCH);
         }
     }
+#ifdef USE_GYRO_DATA_ANALYSE
+    gyroDataAnalyseInit();
+#endif
 }
 
 bool isGyroCalibrationComplete(void)
@@ -398,7 +399,7 @@ void gyroSetCalibrationCycles(void)
     calibratingG = gyroCalculateCalibratingCycles();
 }
 
-STATIC_UNIT_TESTED void performGyroCalibration(uint8_t gyroMovementCalibrationThreshold)
+STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *gyroDev, uint8_t gyroMovementCalibrationThreshold)
 {
     static int32_t g[3];
     static stdev_t var[3];
@@ -412,15 +413,15 @@ STATIC_UNIT_TESTED void performGyroCalibration(uint8_t gyroMovementCalibrationTh
         }
 
         // Sum up CALIBRATING_GYRO_CYCLES readings
-        g[axis] += gyroADC[axis];
+        g[axis] += gyroDev->gyroADC[axis];
-        devPush(&var[axis], gyroADC[axis]);
+        devPush(&var[axis], gyroDev->gyroADC[axis]);
 
         // Reset global variables to prevent other code from using un-calibrated data
-        gyroADC[axis] = 0;
+        gyroDev->gyroADC[axis] = 0;
-        gyroZero[axis] = 0;
+        gyroDev->gyroZero[axis] = 0;
 
         if (isOnFinalGyroCalibrationCycle()) {
-            float dev = devStandardDeviation(&var[axis]);
+            const float dev = devStandardDeviation(&var[axis]);
 
             DEBUG_SET(DEBUG_GYRO, DEBUG_GYRO_CALIBRATION, lrintf(dev));
 
@@ -429,7 +430,7 @@ STATIC_UNIT_TESTED void performGyroCalibration(uint8_t gyroMovementCalibrationTh
                 gyroSetCalibrationCycles();
                 return;
             }
-            gyroZero[axis] = (g[axis] + (gyroCalculateCalibratingCycles() / 2)) / gyroCalculateCalibratingCycles();
+            gyroDev->gyroZero[axis] = (g[axis] + (gyroCalculateCalibratingCycles() / 2)) / gyroCalculateCalibratingCycles();
         }
     }
 
@@ -452,16 +453,16 @@ static bool gyroUpdateISR(gyroDev_t* gyroDev)
 #endif
     gyroDev->dataReady = false;
     // move gyro data into 32-bit variables to avoid overflows in calculations
-    gyroADC[X] = gyroDev->gyroADCRaw[X];
+    gyroDev->gyroADC[X] = gyroDev->gyroADCRaw[X];
-    gyroADC[Y] = gyroDev->gyroADCRaw[Y];
+    gyroDev->gyroADC[Y] = gyroDev->gyroADCRaw[Y];
-    gyroADC[Z] = gyroDev->gyroADCRaw[Z];
+    gyroDev->gyroADC[Z] = gyroDev->gyroADCRaw[Z];
 
-    alignSensors(gyroADC, gyroDev->gyroAlign);
+    alignSensors(gyroDev->gyroADC, gyroDev->gyroAlign);
 
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-        gyroADC[axis] -= gyroZero[axis];
+        gyroDev->gyroADC[axis] -= gyroDev->gyroZero[axis];
         // scale gyro output to degrees per second
-        float gyroADCf = (float)gyroADC[axis] * gyroDev->scale;
+        float gyroADCf = (float)gyroDev->gyroADC[axis] * gyroDev->scale;
         gyroADCf = softLpfFilterApplyFn(softLpfFilter[axis], gyroADCf);
         gyroADCf = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf);
         gyroADCf = notchFilter2ApplyFn(notchFilter2[axis], gyroADCf);
@@ -483,11 +484,11 @@ void gyroUpdate(void)
     }
     gyroDev0.dataReady = false;
     // move gyro data into 32-bit variables to avoid overflows in calculations
-    gyroADC[X] = gyroDev0.gyroADCRaw[X];
+    gyroDev0.gyroADC[X] = gyroDev0.gyroADCRaw[X];
-    gyroADC[Y] = gyroDev0.gyroADCRaw[Y];
+    gyroDev0.gyroADC[Y] = gyroDev0.gyroADCRaw[Y];
-    gyroADC[Z] = gyroDev0.gyroADCRaw[Z];
+    gyroDev0.gyroADC[Z] = gyroDev0.gyroADCRaw[Z];
 
-    alignSensors(gyroADC, gyroDev0.gyroAlign);
+    alignSensors(gyroDev0.gyroADC, gyroDev0.gyroAlign);
 
     const bool calibrationComplete = isGyroCalibrationComplete();
     if (calibrationComplete) {
@@ -502,13 +503,13 @@ void gyroUpdate(void)
         debug[3] = (uint16_t)(micros() & 0xffff);
 #endif
     } else {
-        performGyroCalibration(gyroConfig()->gyroMovementCalibrationThreshold);
+        performGyroCalibration(&gyroDev0, gyroConfig()->gyroMovementCalibrationThreshold);
     }
 
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-        gyroADC[axis] -= gyroZero[axis];
+        gyroDev0.gyroADC[axis] -= gyroDev0.gyroZero[axis];
         // scale gyro output to degrees per second
-        float gyroADCf = (float)gyroADC[axis] * gyroDev0.scale;
+        float gyroADCf = (float)gyroDev0.gyroADC[axis] * gyroDev0.scale;
 
         // Apply LPF
         DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf));
@@ -522,10 +523,13 @@ void gyroUpdate(void)
     }
 
     if (!calibrationComplete) {
-        gyroADC[X] = lrintf(gyro.gyroADCf[X] / gyroDev0.scale);
+        gyroDev0.gyroADC[X] = lrintf(gyro.gyroADCf[X] / gyroDev0.scale);
-        gyroADC[Y] = lrintf(gyro.gyroADCf[Y] / gyroDev0.scale);
+        gyroDev0.gyroADC[Y] = lrintf(gyro.gyroADCf[Y] / gyroDev0.scale);
-        gyroADC[Z] = lrintf(gyro.gyroADCf[Z] / gyroDev0.scale);
+        gyroDev0.gyroADC[Z] = lrintf(gyro.gyroADCf[Z] / gyroDev0.scale);
     }
+#ifdef USE_GYRO_DATA_ANALYSE
+    gyroDataAnalyse(&gyroDev0, &gyro);
+#endif
 }
 
 void gyroReadTemperature(void)

src/main/sensors/gyroanalyse.c

@@ -0,0 +1,35 @@
+#include <stdint.h>
+
+#include "platform.h"
+
+#ifdef USE_GYRO_DATA_ANALYSE
+
+#include "arm_math.h"
+
+#include "build/debug.h"
+
+#include "common/maths.h"
+#include "common/time.h"
+#include "common/utils.h"
+
+#include "drivers/accgyro.h"
+
+#include "sensors/gyro.h"
+#include "sensors/gyroanalyse.h"
+
+
+void gyroDataAnalyseInit(void)
+{
+}
+
+void gyroDataAnalyse(const gyroDev_t *gyroDev, const gyro_t *gyro)
+{
+    UNUSED(gyroDev);
+    UNUSED(gyro);
+}
+
+void gyroDataAnalyseUpdate(timeUs_t currentTimeUs)
+{
+    UNUSED(currentTimeUs);
+}
+#endif // USE_GYRO_DATA_ANALYSE

src/main/sensors/gyroanalyse.h

@@ -0,0 +1,26 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it 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.
+ *
+ * Cleanflight 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "common/time.h"
+
+void gyroDataAnalyseInit(void);
+struct gyroDev_s;
+struct gyro_s;
+void gyroDataAnalyse(const struct gyroDev_s *gyroDev, const struct gyro_s *gyro);
+void gyroDataAnalyseUpdate(timeUs_t currentTimeUs);