From ea2ec8b0698986c64804ce848957d4aca351613b Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 23 Feb 2017 09:23:54 +0000 Subject: [PATCH] Added hooks for gyro data analysis --- Makefile | 21 +++++++ lib/main/CMSIS/CM4/CoreSupport/arm_math.h | 3 + .../Source/CommonTables/arm_common_tables.c | 1 + src/main/drivers/accgyro.h | 4 +- src/main/drivers/exti.h | 2 + src/main/drivers/sensor.h | 3 + src/main/fc/fc_tasks.c | 19 ++++++- src/main/scheduler/scheduler.h | 3 + src/main/sensors/gyro.c | 56 ++++++++++--------- src/main/sensors/gyroanalyse.c | 35 ++++++++++++ src/main/sensors/gyroanalyse.h | 26 +++++++++ 11 files changed, 143 insertions(+), 30 deletions(-) create mode 100644 src/main/sensors/gyroanalyse.c create mode 100644 src/main/sensors/gyroanalyse.h diff --git a/Makefile b/Makefile index c6e19237eb..261f1de226 100644 --- a/Makefile +++ b/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 \ diff --git a/lib/main/CMSIS/CM4/CoreSupport/arm_math.h b/lib/main/CMSIS/CM4/CoreSupport/arm_math.h index e4b2f62e80..47a0124a92 100644 --- a/lib/main/CMSIS/CM4/CoreSupport/arm_math.h +++ b/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]; diff --git a/lib/main/DSP_Lib/Source/CommonTables/arm_common_tables.c b/lib/main/DSP_Lib/Source/CommonTables/arm_common_tables.c index 79fe976e17..0d5c6b2767 100644 --- a/lib/main/DSP_Lib/Source/CommonTables/arm_common_tables.c +++ b/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 diff --git a/src/main/drivers/accgyro.h b/src/main/drivers/accgyro.h index 995a2b224e..c92b0d1957 100644 --- a/src/main/drivers/accgyro.h +++ b/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; diff --git a/src/main/drivers/exti.h b/src/main/drivers/exti.h index 205dc6d5f8..fbdcd3c09e 100644 --- a/src/main/drivers/exti.h +++ b/src/main/drivers/exti.h @@ -18,6 +18,8 @@ #pragma once +#include + #include "io_types.h" // old EXTI interface, to be replaced diff --git a/src/main/drivers/sensor.h b/src/main/drivers/sensor.h index 1f50a9a1df..e39e401b21 100644 --- a/src/main/drivers/sensor.h +++ b/src/main/drivers/sensor.h @@ -17,6 +17,9 @@ #pragma once +#include +#include + typedef enum { ALIGN_DEFAULT = 0, // driver-provided alignment CW0_DEG = 1, diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index ecbb07659d..3a76687c4a 100644 --- a/src/main/fc/fc_tasks.c +++ b/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 }; diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 7231767513..ea12cd6fca 100644 --- a/src/main/scheduler/scheduler.h +++ b/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, diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index c20f0fd670..767de5c933 100644 --- a/src/main/sensors/gyro.c +++ b/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]; - devPush(&var[axis], gyroADC[axis]); + g[axis] += gyroDev->gyroADC[axis]; + devPush(&var[axis], gyroDev->gyroADC[axis]); // Reset global variables to prevent other code from using un-calibrated data - gyroADC[axis] = 0; - gyroZero[axis] = 0; + gyroDev->gyroADC[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]; - gyroADC[Y] = gyroDev->gyroADCRaw[Y]; - gyroADC[Z] = gyroDev->gyroADCRaw[Z]; + gyroDev->gyroADC[X] = gyroDev->gyroADCRaw[X]; + gyroDev->gyroADC[Y] = gyroDev->gyroADCRaw[Y]; + 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]; - gyroADC[Y] = gyroDev0.gyroADCRaw[Y]; - gyroADC[Z] = gyroDev0.gyroADCRaw[Z]; + gyroDev0.gyroADC[X] = gyroDev0.gyroADCRaw[X]; + gyroDev0.gyroADC[Y] = gyroDev0.gyroADCRaw[Y]; + 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); - gyroADC[Y] = lrintf(gyro.gyroADCf[Y] / gyroDev0.scale); - gyroADC[Z] = lrintf(gyro.gyroADCf[Z] / gyroDev0.scale); + gyroDev0.gyroADC[X] = lrintf(gyro.gyroADCf[X] / gyroDev0.scale); + gyroDev0.gyroADC[Y] = lrintf(gyro.gyroADCf[Y] / gyroDev0.scale); + gyroDev0.gyroADC[Z] = lrintf(gyro.gyroADCf[Z] / gyroDev0.scale); } +#ifdef USE_GYRO_DATA_ANALYSE + gyroDataAnalyse(&gyroDev0, &gyro); +#endif } void gyroReadTemperature(void) diff --git a/src/main/sensors/gyroanalyse.c b/src/main/sensors/gyroanalyse.c new file mode 100644 index 0000000000..13258cf296 --- /dev/null +++ b/src/main/sensors/gyroanalyse.c @@ -0,0 +1,35 @@ +#include + +#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 diff --git a/src/main/sensors/gyroanalyse.h b/src/main/sensors/gyroanalyse.h new file mode 100644 index 0000000000..d63b5023e7 --- /dev/null +++ b/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 . + */ + +#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);