commit
30eb699d10
14
Makefile
14
Makefile
|
@ -276,14 +276,14 @@ STARTUP_SRC = startup_stm32f30x_md_gcc.S
|
|||
STDPERIPH_SRC := $(filter-out ${EXCLUDES}, $(STDPERIPH_SRC))
|
||||
DEVICE_STDPERIPH_SRC = $(STDPERIPH_SRC)
|
||||
|
||||
VPATH := $(VPATH):$(CMSIS_DIR)/CM1/CoreSupport:$(CMSIS_DIR)/CM1/DeviceSupport/ST/STM32F30x
|
||||
CMSIS_SRC = $(notdir $(wildcard $(CMSIS_DIR)/CM1/CoreSupport/*.c \
|
||||
$(CMSIS_DIR)/CM1/DeviceSupport/ST/STM32F30x/*.c))
|
||||
VPATH := $(VPATH):$(CMSIS_DIR)/CM4/CoreSupport:$(CMSIS_DIR)/CM4/DeviceSupport/ST/STM32F30x
|
||||
CMSIS_SRC = $(notdir $(wildcard $(CMSIS_DIR)/CM4/CoreSupport/*.c \
|
||||
$(CMSIS_DIR)/CM4/DeviceSupport/ST/STM32F30x/*.c))
|
||||
|
||||
INCLUDE_DIRS := $(INCLUDE_DIRS) \
|
||||
$(STDPERIPH_DIR)/inc \
|
||||
$(CMSIS_DIR)/CM1/CoreSupport \
|
||||
$(CMSIS_DIR)/CM1/DeviceSupport/ST/STM32F30x
|
||||
$(CMSIS_DIR)/CM4/CoreSupport \
|
||||
$(CMSIS_DIR)/CM4/DeviceSupport/ST/STM32F30x
|
||||
|
||||
ifneq ($(filter VCP, $(FEATURES)),)
|
||||
INCLUDE_DIRS := $(INCLUDE_DIRS) \
|
||||
|
@ -1023,12 +1023,13 @@ else ifeq ($(TARGET),$(filter $(TARGET),$(SITL_TARGETS)))
|
|||
SRC := $(TARGET_SRC) $(SITL_SRC) $(VARIANT_SRC)
|
||||
endif
|
||||
|
||||
ifneq ($(filter $(TARGET),$(F4_TARGETS) $(F7_TARGETS)),)
|
||||
ifneq ($(filter $(TARGET),$(F3_TARGETS) $(F4_TARGETS) $(F7_TARGETS)),)
|
||||
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
|
||||
|
||||
SRC += $(DSPLIB)/Source/BasicMathFunctions/arm_mult_f32.c
|
||||
SRC += $(DSPLIB)/Source/TransformFunctions/arm_rfft_fast_f32.c
|
||||
SRC += $(DSPLIB)/Source/TransformFunctions/arm_cfft_f32.c
|
||||
SRC += $(DSPLIB)/Source/TransformFunctions/arm_rfft_fast_init_f32.c
|
||||
|
@ -1039,6 +1040,7 @@ SRC += $(DSPLIB)/Source/ComplexMathFunctions/arm_cmplx_mag_f32.c
|
|||
SRC += $(DSPLIB)/Source/StatisticsFunctions/arm_max_f32.c
|
||||
|
||||
SRC += $(wildcard $(DSPLIB)/Source/*/*.S)
|
||||
|
||||
endif
|
||||
|
||||
|
||||
|
|
|
@ -31,7 +31,7 @@ __attribute__( ( always_inline ) ) static inline void __set_BASEPRI_MAX_nb(uint3
|
|||
__ASM volatile ("\tMSR basepri_max, %0\n" : : "r" (basePri) );
|
||||
}
|
||||
|
||||
#if !defined(STM32F4) && !defined(STM32F7) /* already defined in /lib/main/CMSIS/CM4/CoreSupport/core_cmFunc.h for F4 targets */
|
||||
#if !defined(STM32F3) && !defined(STM32F4) && !defined(STM32F7) /* already defined in /lib/main/CMSIS/CM4/CoreSupport/core_cmFunc.h for F4 targets */
|
||||
__attribute__( ( always_inline ) ) static inline void __set_BASEPRI_MAX(uint32_t basePri)
|
||||
{
|
||||
__ASM volatile ("\tMSR basepri_max, %0\n" : : "r" (basePri) : "memory" );
|
||||
|
|
|
@ -65,5 +65,8 @@ typedef enum {
|
|||
DEBUG_ESC_SENSOR_RPM,
|
||||
DEBUG_ESC_SENSOR_TMP,
|
||||
DEBUG_ALTITUDE,
|
||||
DEBUG_FFT,
|
||||
DEBUG_FFT_TIME,
|
||||
DEBUG_FFT_FREQ,
|
||||
DEBUG_COUNT
|
||||
} debugType_e;
|
||||
|
|
|
@ -26,11 +26,9 @@
|
|||
|
||||
#define M_LN2_FLOAT 0.69314718055994530942f
|
||||
#define M_PI_FLOAT 3.14159265358979323846f
|
||||
|
||||
#define BIQUAD_BANDWIDTH 1.9f /* bandwidth in octaves */
|
||||
#define BIQUAD_Q 1.0f / sqrtf(2.0f) /* quality factor - butterworth*/
|
||||
|
||||
|
||||
// NULL filter
|
||||
|
||||
float nullFilterApply(void *filter, float input)
|
||||
|
@ -79,22 +77,22 @@ void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refr
|
|||
{
|
||||
biquadFilterInit(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF);
|
||||
}
|
||||
|
||||
void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType)
|
||||
{
|
||||
// setup variables
|
||||
const float sampleRate = 1 / ((float)refreshRate * 0.000001f);
|
||||
const float omega = 2 * M_PI_FLOAT * filterFreq / sampleRate;
|
||||
const float omega = 2.0f * M_PI_FLOAT * filterFreq * refreshRate * 0.000001f;
|
||||
const float sn = sinf(omega);
|
||||
const float cs = cosf(omega);
|
||||
const float alpha = sn / (2 * Q);
|
||||
const float alpha = sn / (2.0f * Q);
|
||||
|
||||
float b0 = 0, b1 = 0, b2 = 0, a0 = 0, a1 = 0, a2 = 0;
|
||||
|
||||
switch (filterType) {
|
||||
case FILTER_LPF:
|
||||
b0 = (1 - cs) / 2;
|
||||
b0 = (1 - cs) * 0.5f;
|
||||
b1 = 1 - cs;
|
||||
b2 = (1 - cs) / 2;
|
||||
b2 = (1 - cs) * 0.5f;
|
||||
a0 = 1 + alpha;
|
||||
a1 = -2 * cs;
|
||||
a2 = 1 - alpha;
|
||||
|
@ -107,6 +105,14 @@ void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refresh
|
|||
a1 = -2 * cs;
|
||||
a2 = 1 - alpha;
|
||||
break;
|
||||
case FILTER_BPF:
|
||||
b0 = alpha;
|
||||
b1 = 0;
|
||||
b2 = -alpha;
|
||||
a0 = 1 + alpha;
|
||||
a1 = -2 * cs;
|
||||
a2 = 1 - alpha;
|
||||
break;
|
||||
}
|
||||
|
||||
// precompute the coefficients
|
||||
|
@ -117,10 +123,50 @@ void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refresh
|
|||
filter->a2 = a2 / a0;
|
||||
|
||||
// zero initial samples
|
||||
filter->x1 = filter->x2 = 0;
|
||||
filter->y1 = filter->y2 = 0;
|
||||
filter->d1 = filter->d2 = 0;
|
||||
}
|
||||
|
||||
/* Computes a biquadFilter_t filter on a sample */
|
||||
void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType)
|
||||
{
|
||||
// backup state
|
||||
float x1 = filter->x1;
|
||||
float x2 = filter->x2;
|
||||
float y1 = filter->y1;
|
||||
float y2 = filter->y2;
|
||||
float d1 = filter->d1;
|
||||
float d2 = filter->d2;
|
||||
|
||||
biquadFilterInit(filter, filterFreq, refreshRate, Q, filterType);
|
||||
|
||||
// restore state
|
||||
filter->x1 = x1;
|
||||
filter->x2 = x2;
|
||||
filter->y1 = y1;
|
||||
filter->y2 = y2;
|
||||
filter->d1 = d1;
|
||||
filter->d2 = d2;
|
||||
}
|
||||
|
||||
/* Computes a biquadFilter_t filter on a sample (slightly less precise than df2 but works in dynamic mode) */
|
||||
float biquadFilterApplyDF1(biquadFilter_t *filter, float input)
|
||||
{
|
||||
/* compute result */
|
||||
const float result = filter->b0 * input + filter->b1 * filter->x1 + filter->b2 * filter->x2 - filter->a1 * filter->y1 - filter->a2 * filter->y2;
|
||||
|
||||
/* shift x1 to x2, input to x1 */
|
||||
filter->x2 = filter->x1;
|
||||
filter->x1 = input;
|
||||
|
||||
/* shift y1 to y2, result to y1 */
|
||||
filter->y2 = filter->y1;
|
||||
filter->y1 = result;
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/* Computes a biquadFilter_t filter in direct form 2 on a sample (higher precision but can't handle changes in coefficients */
|
||||
float biquadFilterApply(biquadFilter_t *filter, float input)
|
||||
{
|
||||
const float result = filter->b0 * input + filter->d1;
|
||||
|
|
|
@ -33,6 +33,7 @@ typedef struct pt1Filter_s {
|
|||
/* this holds the data required to update samples thru a filter */
|
||||
typedef struct biquadFilter_s {
|
||||
float b0, b1, b2, a1, a2;
|
||||
float x1, x2, y1, y2;
|
||||
float d1, d2;
|
||||
} biquadFilter_t;
|
||||
|
||||
|
@ -52,7 +53,8 @@ typedef enum {
|
|||
|
||||
typedef enum {
|
||||
FILTER_LPF,
|
||||
FILTER_NOTCH
|
||||
FILTER_NOTCH,
|
||||
FILTER_BPF,
|
||||
} biquadFilterType_e;
|
||||
|
||||
typedef struct firFilter_s {
|
||||
|
@ -71,9 +73,14 @@ float nullFilterApply(void *filter, float input);
|
|||
|
||||
void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate);
|
||||
void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
|
||||
void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
|
||||
float biquadFilterApplyDF1(biquadFilter_t *filter, float input);
|
||||
float biquadFilterApply(biquadFilter_t *filter, float input);
|
||||
float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff);
|
||||
|
||||
// not exactly correct, but very very close and much much faster
|
||||
#define filterGetNotchQApprox(centerFreq, cutoff) ((float)(cutoff * centerFreq) / ((float)(centerFreq - cutoff) * (float)(centerFreq + cutoff)))
|
||||
|
||||
void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT);
|
||||
float pt1FilterApply(pt1Filter_t *filter, float input);
|
||||
float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT);
|
||||
|
|
|
@ -150,7 +150,7 @@ static const char * const featureNames[] = {
|
|||
"SONAR", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM",
|
||||
"RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "OSD",
|
||||
"UNUSED", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE",
|
||||
"SDCARD", "VTX", "RX_SPI", "SOFTSPI", "ESC_SENSOR", "ANTI_GRAVITY", NULL
|
||||
"SDCARD", "VTX", "RX_SPI", "SOFTSPI", "ESC_SENSOR", "ANTI_GRAVITY", "DYNAMIC_FILTER", NULL
|
||||
};
|
||||
|
||||
// sync this with rxFailsafeChannelMode_e
|
||||
|
|
|
@ -60,6 +60,7 @@ typedef enum {
|
|||
FEATURE_SOFTSPI = 1 << 26,
|
||||
FEATURE_ESC_SENSOR = 1 << 27,
|
||||
FEATURE_ANTI_GRAVITY = 1 << 28,
|
||||
FEATURE_DYNAMIC_FILTER = 1 << 29,
|
||||
} features_e;
|
||||
|
||||
#define MAX_NAME_LENGTH 16
|
||||
|
|
|
@ -75,7 +75,6 @@
|
|||
#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"
|
||||
|
||||
|
@ -352,9 +351,6 @@ void fcTasksInit(void)
|
|||
setTaskEnabled(TASK_VTXCTRL, true);
|
||||
#endif
|
||||
#endif
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
setTaskEnabled(TASK_GYRO_DATA_ANALYSE, true);
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
||||
|
@ -597,14 +593,5 @@ cfTask_t cfTasks[TASK_COUNT] = {
|
|||
.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
|
||||
#endif
|
||||
};
|
||||
|
|
|
@ -204,7 +204,10 @@ static const char * const lookupTableDebug[DEBUG_COUNT] = {
|
|||
"STACK",
|
||||
"ESC_SENSOR_RPM",
|
||||
"ESC_SENSOR_TMP",
|
||||
"ALTITUDE"
|
||||
"ALTITUDE",
|
||||
"FFT",
|
||||
"FFT_TIME",
|
||||
"FFT_FREQ"
|
||||
};
|
||||
|
||||
#ifdef OSD
|
||||
|
|
|
@ -110,9 +110,6 @@ typedef enum {
|
|||
#ifdef VTX_CONTROL
|
||||
TASK_VTXCTRL,
|
||||
#endif
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
TASK_GYRO_DATA_ANALYSE,
|
||||
#endif
|
||||
|
||||
/* Count of real tasks */
|
||||
TASK_COUNT,
|
||||
|
|
|
@ -96,6 +96,8 @@ typedef struct gyroSensor_s {
|
|||
biquadFilter_t notchFilter1[XYZ_AXIS_COUNT];
|
||||
filterApplyFnPtr notchFilter2ApplyFn;
|
||||
biquadFilter_t notchFilter2[XYZ_AXIS_COUNT];
|
||||
filterApplyFnPtr notchFilterDynApplyFn;
|
||||
biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT];
|
||||
} gyroSensor_t;
|
||||
|
||||
static gyroSensor_t gyroSensor0;
|
||||
|
@ -407,7 +409,6 @@ void gyroInitFilterNotch1(gyroSensor_t *gyroSensor, uint16_t notchHz, uint16_t n
|
|||
|
||||
void gyroInitFilterNotch2(gyroSensor_t *gyroSensor, uint16_t notchHz, uint16_t notchCutoffHz)
|
||||
{
|
||||
|
||||
gyroSensor->notchFilter2ApplyFn = nullFilterApply;
|
||||
const uint32_t gyroFrequencyNyquist = (1.0f / (gyro.targetLooptime * 0.000001f)) / 2; // No rounding needed
|
||||
if (notchHz && notchHz <= gyroFrequencyNyquist) {
|
||||
|
@ -419,11 +420,21 @@ void gyroInitFilterNotch2(gyroSensor_t *gyroSensor, uint16_t notchHz, uint16_t n
|
|||
}
|
||||
}
|
||||
|
||||
void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor)
|
||||
{
|
||||
gyroSensor->notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2
|
||||
const float notchQ = filterGetNotchQ(400, 390); //just any init value
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
biquadFilterInit(&gyroSensor->notchFilterDyn[axis], 400, gyro.targetLooptime, notchQ, FILTER_NOTCH);
|
||||
}
|
||||
}
|
||||
|
||||
static void gyroInitSensorFilters(gyroSensor_t *gyroSensor)
|
||||
{
|
||||
gyroInitFilterLpf(gyroSensor, gyroConfig()->gyro_soft_lpf_hz);
|
||||
gyroInitFilterNotch1(gyroSensor, gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1);
|
||||
gyroInitFilterNotch2(gyroSensor, gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2);
|
||||
gyroInitFilterDynamicNotch(gyroSensor);
|
||||
}
|
||||
|
||||
void gyroInitFilters(void)
|
||||
|
@ -506,6 +517,7 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t
|
|||
void gyroUpdateSensor(gyroSensor_t *gyroSensor)
|
||||
{
|
||||
if (!gyroSensor->gyroDev.readFn(&gyroSensor->gyroDev)) {
|
||||
|
||||
return;
|
||||
}
|
||||
gyroSensor->gyroDev.dataReady = false;
|
||||
|
@ -527,24 +539,37 @@ void gyroUpdateSensor(gyroSensor_t *gyroSensor)
|
|||
return;
|
||||
}
|
||||
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
gyroDataAnalyse(&gyroSensor->gyroDev, gyroSensor->notchFilterDyn);
|
||||
#endif
|
||||
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
// scale gyro output to degrees per second
|
||||
float gyroADCf = (float)gyroSensor->gyroDev.gyroADC[axis] * gyroSensor->gyroDev.scale;
|
||||
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
// Apply Dynamic Notch filtering
|
||||
if (axis == 0)
|
||||
DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf)); // store raw data
|
||||
|
||||
if (isDynamicFilterActive())
|
||||
gyroADCf = gyroSensor->notchFilterDynApplyFn(&gyroSensor->notchFilterDyn[axis], gyroADCf);
|
||||
|
||||
if (axis == 0)
|
||||
DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf)); // store data after dynamic notch
|
||||
#endif
|
||||
|
||||
// Apply Static Notch filtering
|
||||
DEBUG_SET(DEBUG_NOTCH, axis, lrintf(gyroADCf));
|
||||
gyroADCf = gyroSensor->notchFilter1ApplyFn(&gyroSensor->notchFilter1[axis], gyroADCf);
|
||||
gyroADCf = gyroSensor->notchFilter2ApplyFn(&gyroSensor->notchFilter2[axis], gyroADCf);
|
||||
|
||||
// Apply LPF
|
||||
DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf));
|
||||
gyroADCf = gyroSensor->softLpfFilterApplyFn(gyroSensor->softLpfFilterPtr[axis], gyroADCf);
|
||||
|
||||
// Apply Notch filtering
|
||||
DEBUG_SET(DEBUG_NOTCH, axis, lrintf(gyroADCf));
|
||||
gyroADCf = gyroSensor->notchFilter1ApplyFn(&gyroSensor->notchFilter1[axis], gyroADCf);
|
||||
gyroADCf = gyroSensor->notchFilter2ApplyFn(&gyroSensor->notchFilter2[axis], gyroADCf);
|
||||
gyro.gyroADCf[axis] = gyroADCf;
|
||||
}
|
||||
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
gyroDataAnalyse(&gyroSensor->gyroDev, &gyro);
|
||||
#endif
|
||||
}
|
||||
|
||||
void gyroUpdate(void)
|
||||
|
|
|
@ -66,6 +66,7 @@ typedef struct gyroConfig_s {
|
|||
PG_DECLARE(gyroConfig_t, gyroConfig);
|
||||
|
||||
bool gyroInit(void);
|
||||
|
||||
void gyroInitFilters(void);
|
||||
void gyroUpdate(void);
|
||||
const busDevice_t *gyroSensorBus(void);
|
||||
|
|
|
@ -1,35 +1,323 @@
|
|||
/*
|
||||
* 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/>.
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
|
||||
#include "arm_math.h"
|
||||
|
||||
#include "build/debug.h"
|
||||
|
||||
#include "common/filter.h"
|
||||
#include "common/maths.h"
|
||||
#include "common/time.h"
|
||||
#include "common/utils.h"
|
||||
|
||||
#include "config/feature.h"
|
||||
#include "config/parameter_group.h"
|
||||
#include "config/parameter_group_ids.h"
|
||||
|
||||
#include "drivers/accgyro/accgyro.h"
|
||||
#include "drivers/system.h"
|
||||
|
||||
#include "fc/config.h"
|
||||
#include "fc/rc_controls.h"
|
||||
|
||||
#include "sensors/gyro.h"
|
||||
#include "sensors/gyroanalyse.h"
|
||||
|
||||
#include "common/filter.h"
|
||||
|
||||
void gyroDataAnalyseInit(void)
|
||||
// The FFT splits the frequency domain into an number of bins
|
||||
// A sampling frequency of 1000 and max frequency of 500 at a window size of 32 gives 16 frequency bins each with a width 31.25Hz
|
||||
// Eg [0,31), [31,62), [62, 93) etc
|
||||
|
||||
#define FFT_WINDOW_SIZE 32 // max for f3 targets
|
||||
#define FFT_MIN_FREQ 100 // not interested in filtering frequencies below 100Hz
|
||||
#define FFT_SAMPLING_RATE 1000 // allows analysis up to 500Hz which is more than motors create
|
||||
#define FFT_BPF_HZ 200 // use a bandpass on gyro data to ignore extreme low and extreme high frequencies
|
||||
#define DYN_NOTCH_WIDTH 100 // just an orientation and start value
|
||||
#define DYN_NOTCH_CHANGERATE 60 // lower cut does not improve the performance much, higher cut makes it worse...
|
||||
#define DYN_NOTCH_MIN_CUTOFF 120 // don't cut too deep into low frequencies
|
||||
#define DYN_NOTCH_MAX_CUTOFF 200 // don't go above this cutoff (better filtering with "constant" delay at higher center frequencies)
|
||||
|
||||
#define BIQUAD_Q 1.0f / sqrtf(2.0f) // quality factor - butterworth
|
||||
|
||||
static uint16_t samplingFrequency; // gyro rate
|
||||
static uint8_t fftBinCount;
|
||||
static float fftResolution; // hz per bin
|
||||
static float gyroData[3][FFT_WINDOW_SIZE]; // gyro data used for frequency analysis
|
||||
|
||||
static arm_rfft_fast_instance_f32 fftInstance;
|
||||
static float fftData[FFT_WINDOW_SIZE];
|
||||
static float rfftData[FFT_WINDOW_SIZE];
|
||||
static gyroFftData_t fftResult[3];
|
||||
static uint16_t fftMaxFreq = 0; // nyquist rate
|
||||
static uint16_t fftIdx = 0; // use a circular buffer for the last FFT_WINDOW_SIZE samples
|
||||
|
||||
|
||||
// accumulator for oversampled data => no aliasing and less noise
|
||||
static float fftAcc[3] = {0, 0, 0};
|
||||
static int fftAccCount = 0;
|
||||
static int fftSamplingScale;
|
||||
|
||||
// bandpass filter gyro data
|
||||
static biquadFilter_t fftGyroFilter[3];
|
||||
|
||||
// filter for smoothing frequency estimation
|
||||
static biquadFilter_t fftFreqFilter[3];
|
||||
|
||||
// Hanning window, see https://en.wikipedia.org/wiki/Window_function#Hann_.28Hanning.29_window
|
||||
static float hanningWindow[FFT_WINDOW_SIZE];
|
||||
|
||||
void initHanning()
|
||||
{
|
||||
for (int i = 0; i < FFT_WINDOW_SIZE; i++) {
|
||||
hanningWindow[i] = (0.5 - 0.5 * cosf(2 * M_PIf * i / (FFT_WINDOW_SIZE - 1)));
|
||||
}
|
||||
}
|
||||
|
||||
void gyroDataAnalyse(const gyroDev_t *gyroDev, const gyro_t *gyro)
|
||||
void initGyroData()
|
||||
{
|
||||
UNUSED(gyroDev);
|
||||
UNUSED(gyro);
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
for (int i = 0; i < FFT_WINDOW_SIZE; i++) {
|
||||
gyroData[axis][i] = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void gyroDataAnalyseUpdate(timeUs_t currentTimeUs)
|
||||
static inline int fftFreqToBin(int freq)
|
||||
{
|
||||
UNUSED(currentTimeUs);
|
||||
return ((FFT_WINDOW_SIZE / 2 - 1) * freq) / (fftMaxFreq);
|
||||
}
|
||||
|
||||
void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
|
||||
{
|
||||
// initialise even if FEATURE_DYNAMIC_FILTER not set, since it may be set later
|
||||
samplingFrequency = 1000000 / targetLooptimeUs;
|
||||
fftSamplingScale = samplingFrequency / FFT_SAMPLING_RATE;
|
||||
fftMaxFreq = FFT_SAMPLING_RATE / 2;
|
||||
fftBinCount = fftFreqToBin(fftMaxFreq) + 1;
|
||||
fftResolution = FFT_SAMPLING_RATE / FFT_WINDOW_SIZE;
|
||||
arm_rfft_fast_init_f32(&fftInstance, FFT_WINDOW_SIZE);
|
||||
|
||||
initGyroData();
|
||||
initHanning();
|
||||
|
||||
// recalculation of filters takes 4 calls per axis => each filter gets updated every 3 * 4 = 12 calls
|
||||
// at 4khz gyro loop rate this means 4khz / 4 / 3 = 333Hz => update every 3ms
|
||||
float looptime = targetLooptimeUs * 4 * 3;
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
fftResult[axis].centerFreq = 200; // any init value
|
||||
biquadFilterInitLPF(&fftFreqFilter[axis], DYN_NOTCH_CHANGERATE, looptime);
|
||||
biquadFilterInit(&fftGyroFilter[axis], FFT_BPF_HZ, 1000000 / FFT_SAMPLING_RATE, BIQUAD_Q, FILTER_BPF);
|
||||
}
|
||||
}
|
||||
|
||||
// used in OSD
|
||||
const gyroFftData_t *gyroFftData(int axis)
|
||||
{
|
||||
return &fftResult[axis];
|
||||
}
|
||||
|
||||
bool isDynamicFilterActive(void)
|
||||
{
|
||||
return feature(FEATURE_DYNAMIC_FILTER);
|
||||
}
|
||||
|
||||
/*
|
||||
* Collect gyro data, to be analysed in gyroDataAnalyseUpdate function
|
||||
*/
|
||||
void gyroDataAnalyse(const gyroDev_t *gyroDev, biquadFilter_t *notchFilterDyn)
|
||||
{
|
||||
if (!isDynamicFilterActive()) {
|
||||
return;
|
||||
}
|
||||
|
||||
// if gyro sampling is > 1kHz, accumulate multiple samples
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
fftAcc[axis] += gyroDev->gyroADC[axis];
|
||||
}
|
||||
fftAccCount++;
|
||||
|
||||
// this runs at 1kHz
|
||||
if (fftAccCount == fftSamplingScale) {
|
||||
fftAccCount = 0;
|
||||
|
||||
//calculate mean value of accumulated samples
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
float sample = fftAcc[axis] / fftSamplingScale;
|
||||
sample = biquadFilterApply(&fftGyroFilter[axis], sample);
|
||||
gyroData[axis][fftIdx] = sample;
|
||||
if (axis == 0)
|
||||
DEBUG_SET(DEBUG_FFT, 2, lrintf(sample * gyroDev->scale));
|
||||
fftAcc[axis] = 0;
|
||||
}
|
||||
|
||||
fftIdx = (fftIdx + 1) % FFT_WINDOW_SIZE;
|
||||
}
|
||||
|
||||
// calculate FFT and update filters
|
||||
gyroDataAnalyseUpdate(notchFilterDyn);
|
||||
}
|
||||
|
||||
void stage_rfft_f32(arm_rfft_fast_instance_f32 * S, float32_t * p, float32_t * pOut);
|
||||
void arm_cfft_radix8by2_f32( arm_cfft_instance_f32 * S, float32_t * p1);
|
||||
void arm_cfft_radix8by4_f32( arm_cfft_instance_f32 * S, float32_t * p1);
|
||||
void arm_radix8_butterfly_f32(float32_t * pSrc, uint16_t fftLen, const float32_t * pCoef, uint16_t twidCoefModifier);
|
||||
void arm_bitreversal_32(uint32_t * pSrc, const uint16_t bitRevLen, const uint16_t * pBitRevTable);
|
||||
|
||||
typedef enum {
|
||||
STEP_ARM_CFFT_F32,
|
||||
STEP_BITREVERSAL,
|
||||
STEP_STAGE_RFFT_F32,
|
||||
STEP_ARM_CMPLX_MAG_F32,
|
||||
STEP_CALC_FREQUENCIES,
|
||||
STEP_UPDATE_FILTERS,
|
||||
STEP_HANNING,
|
||||
STEP_COUNT
|
||||
} UpdateStep_e;
|
||||
|
||||
/*
|
||||
* Analyse last gyro data from the last FFT_WINDOW_SIZE milliseconds
|
||||
*/
|
||||
void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn)
|
||||
{
|
||||
static int axis = 0;
|
||||
static int step = 0;
|
||||
arm_cfft_instance_f32 * Sint = &(fftInstance.Sint);
|
||||
|
||||
uint32_t startTime = 0;
|
||||
if (debugMode == (DEBUG_FFT_TIME))
|
||||
startTime = micros();
|
||||
|
||||
DEBUG_SET(DEBUG_FFT_TIME, 0, step);
|
||||
switch (step) {
|
||||
case STEP_ARM_CFFT_F32:
|
||||
{
|
||||
switch (FFT_WINDOW_SIZE / 2) {
|
||||
case 16:
|
||||
// 16us
|
||||
arm_cfft_radix8by2_f32(Sint, fftData);
|
||||
break;
|
||||
case 32:
|
||||
// 35us
|
||||
arm_cfft_radix8by4_f32(Sint, fftData);
|
||||
break;
|
||||
case 64:
|
||||
// 70us
|
||||
arm_radix8_butterfly_f32(fftData, FFT_WINDOW_SIZE / 2, Sint->pTwiddle, 1);
|
||||
break;
|
||||
}
|
||||
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
|
||||
break;
|
||||
}
|
||||
case STEP_BITREVERSAL:
|
||||
{
|
||||
// 6us
|
||||
arm_bitreversal_32((uint32_t*) fftData, Sint->bitRevLength, Sint->pBitRevTable);
|
||||
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
|
||||
step++;
|
||||
// fall through
|
||||
}
|
||||
case STEP_STAGE_RFFT_F32:
|
||||
{
|
||||
// 14us
|
||||
// this does not work in place => fftData AND rfftData needed
|
||||
stage_rfft_f32(&fftInstance, fftData, rfftData);
|
||||
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
|
||||
break;
|
||||
}
|
||||
case STEP_ARM_CMPLX_MAG_F32:
|
||||
{
|
||||
// 8us
|
||||
arm_cmplx_mag_f32(rfftData, fftData, fftBinCount);
|
||||
DEBUG_SET(DEBUG_FFT_TIME, 2, micros() - startTime);
|
||||
step++;
|
||||
// fall through
|
||||
}
|
||||
case STEP_CALC_FREQUENCIES:
|
||||
{
|
||||
// 13us
|
||||
float fftSum = 0;
|
||||
float fftWeightedSum = 0;
|
||||
|
||||
fftResult[axis].maxVal = 0;
|
||||
// iterate over fft data and calculate weighted indexes
|
||||
float squaredData;
|
||||
for (int i = 0; i < fftBinCount; i++) {
|
||||
squaredData = fftData[i] * fftData[i]; //more weight on higher peaks
|
||||
fftResult[axis].maxVal = MAX(fftResult[axis].maxVal, squaredData);
|
||||
fftSum += squaredData;
|
||||
fftWeightedSum += squaredData * (i + 1); // calculate weighted index starting at 1, not 0
|
||||
}
|
||||
|
||||
// get weighted center of relevant frequency range (this way we have a better resolution than 31.25Hz)
|
||||
if (fftSum > 0) {
|
||||
// idx was shifted by 1 to start at 1, not 0
|
||||
float fftMeanIndex = (fftWeightedSum / fftSum) - 1;
|
||||
// the index points at the center frequency of each bin so index 0 is actually 16.125Hz
|
||||
// fftMeanIndex += 0.5;
|
||||
|
||||
// don't go below the minimal cutoff frequency + 10 and don't jump around too much
|
||||
float centerFreq;
|
||||
centerFreq = constrain(fftMeanIndex * fftResolution, DYN_NOTCH_MIN_CUTOFF + 10, fftMaxFreq);
|
||||
centerFreq = biquadFilterApply(&fftFreqFilter[axis], centerFreq);
|
||||
centerFreq = constrain(centerFreq, DYN_NOTCH_MIN_CUTOFF + 10, fftMaxFreq);
|
||||
fftResult[axis].centerFreq = centerFreq;
|
||||
if (axis == 0) {
|
||||
DEBUG_SET(DEBUG_FFT, 3, lrintf(fftMeanIndex * 100));
|
||||
}
|
||||
}
|
||||
|
||||
DEBUG_SET(DEBUG_FFT_FREQ, axis, fftResult[axis].centerFreq);
|
||||
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
|
||||
break;
|
||||
}
|
||||
case STEP_UPDATE_FILTERS:
|
||||
{
|
||||
// 7us
|
||||
// calculate new filter coefficients
|
||||
float cutoffFreq = constrain(fftResult[axis].centerFreq - DYN_NOTCH_WIDTH, DYN_NOTCH_MIN_CUTOFF, DYN_NOTCH_MAX_CUTOFF);
|
||||
float notchQ = filterGetNotchQApprox(fftResult[axis].centerFreq, cutoffFreq);
|
||||
biquadFilterUpdate(¬chFilterDyn[axis], fftResult[axis].centerFreq, gyro.targetLooptime, notchQ, FILTER_NOTCH);
|
||||
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
|
||||
|
||||
axis = (axis + 1) % 3;
|
||||
step++;
|
||||
// fall through
|
||||
}
|
||||
case STEP_HANNING:
|
||||
{
|
||||
// 5us
|
||||
// apply hanning window to gyro samples and store result in fftData
|
||||
// hanning starts and ends with 0, could be skipped for minor speed improvement
|
||||
uint8_t ringBufIdx = FFT_WINDOW_SIZE - fftIdx;
|
||||
arm_mult_f32(&gyroData[axis][fftIdx], &hanningWindow[0], &fftData[0], ringBufIdx);
|
||||
if (fftIdx > 0)
|
||||
arm_mult_f32(&gyroData[axis][0], &hanningWindow[ringBufIdx], &fftData[ringBufIdx], fftIdx);
|
||||
|
||||
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
|
||||
}
|
||||
}
|
||||
|
||||
step = (step + 1) % STEP_COUNT;
|
||||
}
|
||||
|
||||
#endif // USE_GYRO_DATA_ANALYSE
|
||||
|
|
|
@ -18,9 +18,17 @@
|
|||
#pragma once
|
||||
|
||||
#include "common/time.h"
|
||||
#include "common/filter.h"
|
||||
|
||||
void gyroDataAnalyseInit(void);
|
||||
#define GYRO_FFT_BIN_COUNT 16 // FFT_WINDOW_SIZE / 2
|
||||
typedef struct gyroFftData_s {
|
||||
float maxVal;
|
||||
uint16_t centerFreq;
|
||||
} gyroFftData_t;
|
||||
|
||||
void gyroDataAnalyseInit(uint32_t targetLooptime);
|
||||
const gyroFftData_t *gyroFftData(int axis);
|
||||
struct gyroDev_s;
|
||||
struct gyro_s;
|
||||
void gyroDataAnalyse(const struct gyroDev_s *gyroDev, const struct gyro_s *gyro);
|
||||
void gyroDataAnalyseUpdate(timeUs_t currentTimeUs);
|
||||
void gyroDataAnalyse(const struct gyroDev_s *gyroDev, biquadFilter_t *notchFilterDyn);
|
||||
void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn);
|
||||
bool isDynamicFilterActive();
|
||||
|
|
|
@ -20,6 +20,7 @@
|
|||
#undef TELEMETRY_IBUS //no space left
|
||||
#undef TELEMETRY_HOTT //no space left
|
||||
#undef TELEMETRY_JETIEXBUS
|
||||
#undef USE_GYRO_DATA_ANALYSE
|
||||
|
||||
#define TARGET_BOARD_IDENTIFIER "OMNI" // https://en.wikipedia.org/wiki/Omnibus
|
||||
|
||||
|
|
|
@ -41,6 +41,7 @@
|
|||
#ifdef STM32F3
|
||||
#define MINIMAL_CLI
|
||||
#define USE_DSHOT
|
||||
#define USE_GYRO_DATA_ANALYSE
|
||||
#endif
|
||||
|
||||
#ifdef STM32F4
|
||||
|
@ -48,6 +49,7 @@
|
|||
#define USE_ESC_SENSOR
|
||||
#define I2C3_OVERCLOCK true
|
||||
#define TELEMETRY_IBUS
|
||||
#define USE_GYRO_DATA_ANALYSE
|
||||
#endif
|
||||
|
||||
#ifdef STM32F7
|
||||
|
@ -56,6 +58,7 @@
|
|||
#define I2C3_OVERCLOCK true
|
||||
#define I2C4_OVERCLOCK true
|
||||
#define TELEMETRY_IBUS
|
||||
#define USE_GYRO_DATA_ANALYSE
|
||||
#endif
|
||||
|
||||
#if defined(STM32F4) || defined(STM32F7)
|
||||
|
|
Loading…
Reference in New Issue