Dynamic notch refactoring
+ added new parameter group for Dyn Notch
This commit is contained in:
KarateBrot
Michael Keller
parent
4c9eb210a9
commit
8649f1003f
|
|
@ -91,7 +91,7 @@ COMMON_SRC = \
|
|||
flight/position.c \
|
||||
flight/failsafe.c \
|
||||
flight/gps_rescue.c \
|
||||
flight/gyroanalyse.c \
|
||||
flight/dyn_notch_filter.c \
|
||||
flight/imu.c \
|
||||
flight/feedforward.c \
|
||||
flight/mixer.c \
|
||||
|
|
@ -257,7 +257,7 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \
|
|||
fc/rc.c \
|
||||
fc/rc_controls.c \
|
||||
fc/runtime_config.c \
|
||||
flight/gyroanalyse.c \
|
||||
flight/dyn_notch_filter.c \
|
||||
flight/imu.c \
|
||||
flight/mixer.c \
|
||||
flight/pid.c \
|
||||
|
|
|
|||
|
|
@ -1419,11 +1419,11 @@ static bool blackboxWriteSysinfo(void)
|
|||
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff", "%d,%d", gyroConfig()->gyro_soft_notch_cutoff_1,
|
||||
gyroConfig()->gyro_soft_notch_cutoff_2);
|
||||
BLACKBOX_PRINT_HEADER_LINE("gyro_to_use", "%d", gyroConfig()->gyro_to_use);
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
BLACKBOX_PRINT_HEADER_LINE("dyn_notch_max_hz", "%d", gyroConfig()->dyn_notch_max_hz);
|
||||
BLACKBOX_PRINT_HEADER_LINE("dyn_notch_count", "%d", gyroConfig()->dyn_notch_count);
|
||||
BLACKBOX_PRINT_HEADER_LINE("dyn_notch_q", "%d", gyroConfig()->dyn_notch_q);
|
||||
BLACKBOX_PRINT_HEADER_LINE("dyn_notch_min_hz", "%d", gyroConfig()->dyn_notch_min_hz);
|
||||
#ifdef USE_DYN_NOTCH_FILTER
|
||||
BLACKBOX_PRINT_HEADER_LINE("dyn_notch_max_hz", "%d", dynNotchConfig()->dyn_notch_max_hz);
|
||||
BLACKBOX_PRINT_HEADER_LINE("dyn_notch_count", "%d", dynNotchConfig()->dyn_notch_count);
|
||||
BLACKBOX_PRINT_HEADER_LINE("dyn_notch_q", "%d", dynNotchConfig()->dyn_notch_q);
|
||||
BLACKBOX_PRINT_HEADER_LINE("dyn_notch_min_hz", "%d", dynNotchConfig()->dyn_notch_min_hz);
|
||||
#endif
|
||||
#ifdef USE_DSHOT_TELEMETRY
|
||||
BLACKBOX_PRINT_HEADER_LINE("dshot_bidir", "%d", motorConfig()->dev.useDshotTelemetry);
|
||||
|
|
|
|||
|
|
@ -81,6 +81,7 @@
|
|||
#include "pg/bus_i2c.h"
|
||||
#include "pg/dashboard.h"
|
||||
#include "pg/displayport_profiles.h"
|
||||
#include "pg/dyn_notch.h"
|
||||
#include "pg/flash.h"
|
||||
#include "pg/gyrodev.h"
|
||||
#include "pg/max7456.h"
|
||||
|
|
@ -660,11 +661,11 @@ const clivalue_t valueTable[] = {
|
|||
#ifdef USE_MULTI_GYRO
|
||||
{ "gyro_to_use", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_to_use) },
|
||||
#endif
|
||||
#if defined(USE_GYRO_DATA_ANALYSE)
|
||||
{ "dyn_notch_count", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 1, DYN_NOTCH_COUNT_MAX }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_count) },
|
||||
{ "dyn_notch_q", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_q) },
|
||||
{ "dyn_notch_min_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 60, 250 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_min_hz) },
|
||||
{ "dyn_notch_max_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 200, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_max_hz) },
|
||||
#if defined(USE_DYN_NOTCH_FILTER)
|
||||
{ "dyn_notch_count", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 1, DYN_NOTCH_COUNT_MAX }, PG_DYN_NOTCH_CONFIG, offsetof(dynNotchConfig_t, dyn_notch_count) },
|
||||
{ "dyn_notch_q", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1, 1000 }, PG_DYN_NOTCH_CONFIG, offsetof(dynNotchConfig_t, dyn_notch_q) },
|
||||
{ "dyn_notch_min_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 60, 250 }, PG_DYN_NOTCH_CONFIG, offsetof(dynNotchConfig_t, dyn_notch_min_hz) },
|
||||
{ "dyn_notch_max_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 200, 1000 }, PG_DYN_NOTCH_CONFIG, offsetof(dynNotchConfig_t, dyn_notch_max_hz) },
|
||||
#endif
|
||||
#ifdef USE_DYN_LPF
|
||||
{ "dyn_lpf_gyro_min_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, DYN_LPF_FILTER_FREQUENCY_MAX }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_lpf_gyro_min_hz) },
|
||||
|
|
@ -1434,7 +1435,7 @@ const clivalue_t valueTable[] = {
|
|||
{ "osd_stat_max_esc_rpm", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_STAT_MAX_ESC_RPM, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats)},
|
||||
{ "osd_stat_min_link_quality", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_STAT_MIN_LINK_QUALITY,PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats)},
|
||||
{ "osd_stat_flight_dist", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_STAT_FLIGHT_DISTANCE, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats)},
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
#ifdef USE_DYN_NOTCH_FILTER
|
||||
{ "osd_stat_max_fft", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_STAT_MAX_FFT, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats)},
|
||||
#endif
|
||||
#ifdef USE_PERSISTENT_STATS
|
||||
|
|
|
|||
|
|
@ -752,11 +752,11 @@ static CMS_Menu cmsx_menuFilterGlobal = {
|
|||
.entries = cmsx_menuFilterGlobalEntries,
|
||||
};
|
||||
|
||||
#if (defined(USE_GYRO_DATA_ANALYSE) || defined(USE_DYN_LPF)) && defined(USE_EXTENDED_CMS_MENUS)
|
||||
#if (defined(USE_DYN_NOTCH_FILTER) || defined(USE_DYN_LPF)) && defined(USE_EXTENDED_CMS_MENUS)
|
||||
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
#ifdef USE_DYN_NOTCH_FILTER
|
||||
static uint16_t dynFiltNotchMaxHz;
|
||||
static uint8_t dynFiltCount;
|
||||
static uint8_t dynFiltNotchCount;
|
||||
static uint16_t dynFiltNotchQ;
|
||||
static uint16_t dynFiltNotchMinHz;
|
||||
#endif
|
||||
|
|
@ -773,11 +773,11 @@ static const void *cmsx_menuDynFilt_onEnter(displayPort_t *pDisp)
|
|||
{
|
||||
UNUSED(pDisp);
|
||||
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
dynFiltNotchMaxHz = gyroConfig()->dyn_notch_max_hz;
|
||||
dynFiltCount = gyroConfig()->dyn_notch_count;
|
||||
dynFiltNotchQ = gyroConfig()->dyn_notch_q;
|
||||
dynFiltNotchMinHz = gyroConfig()->dyn_notch_min_hz;
|
||||
#ifdef USE_DYN_NOTCH_FILTER
|
||||
dynFiltNotchMaxHz = dynNotchConfig()->dyn_notch_max_hz;
|
||||
dynFiltNotchCount = dynNotchConfig()->dyn_notch_count;
|
||||
dynFiltNotchQ = dynNotchConfig()->dyn_notch_q;
|
||||
dynFiltNotchMinHz = dynNotchConfig()->dyn_notch_min_hz;
|
||||
#endif
|
||||
#ifdef USE_DYN_LPF
|
||||
const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);
|
||||
|
|
@ -797,11 +797,11 @@ static const void *cmsx_menuDynFilt_onExit(displayPort_t *pDisp, const OSD_Entry
|
|||
UNUSED(pDisp);
|
||||
UNUSED(self);
|
||||
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
gyroConfigMutable()->dyn_notch_max_hz = dynFiltNotchMaxHz;
|
||||
gyroConfigMutable()->dyn_notch_count = dynFiltCount;
|
||||
gyroConfigMutable()->dyn_notch_q = dynFiltNotchQ;
|
||||
gyroConfigMutable()->dyn_notch_min_hz = dynFiltNotchMinHz;
|
||||
#ifdef USE_DYN_NOTCH_FILTER
|
||||
dynNotchConfigMutable()->dyn_notch_max_hz = dynFiltNotchMaxHz;
|
||||
dynNotchConfigMutable()->dyn_notch_count = dynFiltNotchCount;
|
||||
dynNotchConfigMutable()->dyn_notch_q = dynFiltNotchQ;
|
||||
dynNotchConfigMutable()->dyn_notch_min_hz = dynFiltNotchMinHz;
|
||||
#endif
|
||||
#ifdef USE_DYN_LPF
|
||||
pidProfile_t *pidProfile = currentPidProfile;
|
||||
|
|
@ -820,8 +820,8 @@ static const OSD_Entry cmsx_menuDynFiltEntries[] =
|
|||
{
|
||||
{ "-- DYN FILT --", OME_Label, NULL, NULL, 0 },
|
||||
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
{ "NOTCH COUNT", OME_UINT8, NULL, &(OSD_UINT8_t) { &dynFiltCount, 1, DYN_NOTCH_COUNT_MAX, 1 }, 0 },
|
||||
#ifdef USE_DYN_NOTCH_FILTER
|
||||
{ "NOTCH COUNT", OME_UINT8, NULL, &(OSD_UINT8_t) { &dynFiltNotchCount, 1, DYN_NOTCH_COUNT_MAX, 1 }, 0 },
|
||||
{ "NOTCH Q", OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltNotchQ, 1, 1000, 1 }, 0 },
|
||||
{ "NOTCH MIN HZ", OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltNotchMinHz, 0, 1000, 1 }, 0 },
|
||||
{ "NOTCH MAX HZ", OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltNotchMaxHz, 0, 1000, 1 }, 0 },
|
||||
|
|
@ -1006,7 +1006,7 @@ static const OSD_Entry cmsx_menuImuEntries[] =
|
|||
{"RATE", OME_Submenu, cmsMenuChange, &cmsx_menuRateProfile, 0},
|
||||
|
||||
{"FILT GLB", OME_Submenu, cmsMenuChange, &cmsx_menuFilterGlobal, 0},
|
||||
#if (defined(USE_GYRO_DATA_ANALYSE) || defined(USE_DYN_LPF)) && defined(USE_EXTENDED_CMS_MENUS)
|
||||
#if (defined(USE_DYN_NOTCH_FILTER) || defined(USE_DYN_LPF)) && defined(USE_EXTENDED_CMS_MENUS)
|
||||
{"DYN FILT", OME_Submenu, cmsMenuChange, &cmsx_menuDynFilt, 0},
|
||||
#endif
|
||||
|
||||
|
|
|
|||
|
|
@ -466,7 +466,7 @@ static void validateAndFixConfig(void)
|
|||
featureDisableImmediate(FEATURE_ESC_SENSOR);
|
||||
#endif
|
||||
|
||||
#ifndef USE_GYRO_DATA_ANALYSE
|
||||
#ifndef USE_DYN_NOTCH_FILTER
|
||||
featureDisableImmediate(FEATURE_DYNAMIC_FILTER);
|
||||
#endif
|
||||
|
||||
|
|
@ -682,7 +682,7 @@ void validateAndFixGyroConfig(void)
|
|||
}
|
||||
}
|
||||
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
#ifdef USE_DYN_NOTCH_FILTER
|
||||
// Disable dynamic filter if gyro loop is less than 2KHz
|
||||
const uint32_t configuredLooptime = (gyro.sampleRateHz > 0) ? (pidConfig()->pid_process_denom * 1e6 / gyro.sampleRateHz) : 0;
|
||||
if (configuredLooptime > DYNAMIC_FILTER_MAX_SUPPORTED_LOOP_TIME) {
|
||||
|
|
|
|||
|
|
@ -61,8 +61,8 @@
|
|||
#include "flight/failsafe.h"
|
||||
#include "flight/gps_rescue.h"
|
||||
|
||||
#if defined(USE_GYRO_DATA_ANALYSE)
|
||||
#include "flight/gyroanalyse.h"
|
||||
#if defined(USE_DYN_NOTCH_FILTER)
|
||||
#include "flight/dyn_notch_filter.h"
|
||||
#endif
|
||||
|
||||
#include "flight/imu.h"
|
||||
|
|
@ -550,7 +550,7 @@ void tryArm(void)
|
|||
}
|
||||
imuQuaternionHeadfreeOffsetSet();
|
||||
|
||||
#if defined(USE_GYRO_DATA_ANALYSE)
|
||||
#if defined(USE_DYN_NOTCH_FILTER)
|
||||
resetMaxFFT();
|
||||
#endif
|
||||
|
||||
|
|
|
|||
|
|
@ -32,23 +32,24 @@
|
|||
|
||||
#include "platform.h"
|
||||
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
#ifdef USE_DYN_NOTCH_FILTER
|
||||
#include "build/debug.h"
|
||||
|
||||
#include "common/axis.h"
|
||||
#include "common/filter.h"
|
||||
#include "common/maths.h"
|
||||
#include "common/sdft.h"
|
||||
#include "common/time.h"
|
||||
#include "common/utils.h"
|
||||
|
||||
#include "drivers/accgyro/accgyro.h"
|
||||
#include "drivers/time.h"
|
||||
#include "config/feature.h"
|
||||
|
||||
#include "sensors/gyro.h"
|
||||
#include "drivers/time.h"
|
||||
|
||||
#include "fc/core.h"
|
||||
|
||||
#include "gyroanalyse.h"
|
||||
#include "sensors/gyro.h"
|
||||
|
||||
#include "dyn_notch_filter.h"
|
||||
|
||||
// SDFT_SAMPLE_SIZE defaults to 72 (common/sdft.h).
|
||||
// We get 36 frequency bins from 72 consecutive data values, called SDFT_BIN_COUNT (common/sdft.h)
|
||||
|
|
@ -56,7 +57,7 @@
|
|||
// Only bins 1 to 35 are usable.
|
||||
|
||||
// A gyro sample is collected every PID loop.
|
||||
// maxSampleCount recent gyro values are accumulated and averaged
|
||||
// sampleCount recent gyro values are accumulated and averaged
|
||||
// to ensure that 72 samples are collected at the right rate for the required SDFT bandwidth.
|
||||
|
||||
// For an 8k PID loop, at default 600hz max, 6 sequential gyro data points are averaged, SDFT runs 1333Hz.
|
||||
|
|
@ -66,8 +67,8 @@
|
|||
// For Bosch at 3200Hz gyro, max of 600, int(3200/1200) = 2, sdftSampleRateHz = 1600, range to 800hz.
|
||||
// For Bosch on XClass, better to set a max of 300, int(3200/600) = 5, sdftSampleRateHz = 640, range to 320Hz.
|
||||
|
||||
// When sampleCount reaches maxSampleCount, the averaged gyro value is put into the corresponding SDFT.
|
||||
// At 8k, with 600Hz max, maxSampleCount = 6, this happens every 6 * 0.125us, or every 0.75ms.
|
||||
// When sampleIndex reaches sampleCount, the averaged gyro value is put into the corresponding SDFT.
|
||||
// At 8k, with 600Hz max, sampleCount = 6, this happens every 6 * 0.125us, or every 0.75ms.
|
||||
// Hence to completely replace all 72 samples of the SDFT input buffer with clean new data takes 54ms.
|
||||
|
||||
// The SDFT code is split into steps. It takes 4 PID loops to calculate the SDFT, track peaks and update the filters for one axis.
|
||||
|
|
@ -102,33 +103,71 @@ typedef struct peak_s {
|
|||
|
||||
} peak_t;
|
||||
|
||||
static sdft_t FAST_DATA_ZERO_INIT sdft[XYZ_AXIS_COUNT];
|
||||
static peak_t FAST_DATA_ZERO_INIT peaks[DYN_NOTCH_COUNT_MAX];
|
||||
static float FAST_DATA_ZERO_INIT sdftData[SDFT_BIN_COUNT];
|
||||
static float FAST_DATA_ZERO_INIT sdftSampleRateHz;
|
||||
static float FAST_DATA_ZERO_INIT sdftResolutionHz;
|
||||
static int FAST_DATA_ZERO_INIT sdftStartBin;
|
||||
static int FAST_DATA_ZERO_INIT sdftEndBin;
|
||||
static float FAST_DATA_ZERO_INIT sdftMeanSq;
|
||||
static float FAST_DATA_ZERO_INIT dynNotchQ;
|
||||
static float FAST_DATA_ZERO_INIT dynNotchMinHz;
|
||||
static float FAST_DATA_ZERO_INIT dynNotchMaxHz;
|
||||
static uint16_t FAST_DATA_ZERO_INIT dynNotchMaxFFT;
|
||||
static float FAST_DATA_ZERO_INIT gain;
|
||||
static int FAST_DATA_ZERO_INIT numSamples;
|
||||
typedef struct state_s {
|
||||
|
||||
void gyroDataAnalyseInit(gyroAnalyseState_t *state, const uint32_t targetLooptimeUs)
|
||||
// state machine step information
|
||||
int tick;
|
||||
int step;
|
||||
int axis;
|
||||
|
||||
} state_t;
|
||||
|
||||
typedef struct dynNotch_s {
|
||||
|
||||
float q;
|
||||
float minHz;
|
||||
float maxHz;
|
||||
int count;
|
||||
|
||||
uint16_t maxCenterFreq;
|
||||
float centerFreq[XYZ_AXIS_COUNT][DYN_NOTCH_COUNT_MAX];
|
||||
|
||||
timeUs_t looptimeUs;
|
||||
biquadFilter_t notch[XYZ_AXIS_COUNT][DYN_NOTCH_COUNT_MAX];
|
||||
|
||||
} dynNotch_t;
|
||||
|
||||
// dynamic notch instance (singleton)
|
||||
static FAST_DATA_ZERO_INIT dynNotch_t dynNotch;
|
||||
|
||||
// accumulator for oversampled data => no aliasing and less noise
|
||||
static FAST_DATA_ZERO_INIT int sampleIndex;
|
||||
static FAST_DATA_ZERO_INIT int sampleCount;
|
||||
static FAST_DATA_ZERO_INIT float sampleCountRcp;
|
||||
static FAST_DATA_ZERO_INIT float sampleAccumulator[XYZ_AXIS_COUNT];
|
||||
|
||||
// downsampled data for frequency analysis
|
||||
static FAST_DATA_ZERO_INIT float sampleAvg[XYZ_AXIS_COUNT];
|
||||
|
||||
// parameters for peak detection and frequency analysis
|
||||
static FAST_DATA_ZERO_INIT state_t state;
|
||||
static FAST_DATA_ZERO_INIT sdft_t sdft[XYZ_AXIS_COUNT];
|
||||
static FAST_DATA_ZERO_INIT peak_t peaks[DYN_NOTCH_COUNT_MAX];
|
||||
static FAST_DATA_ZERO_INIT float sdftData[SDFT_BIN_COUNT];
|
||||
static FAST_DATA_ZERO_INIT float sdftSampleRateHz;
|
||||
static FAST_DATA_ZERO_INIT float sdftResolutionHz;
|
||||
static FAST_DATA_ZERO_INIT int sdftStartBin;
|
||||
static FAST_DATA_ZERO_INIT int sdftEndBin;
|
||||
static FAST_DATA_ZERO_INIT float sdftMeanSq;
|
||||
static FAST_DATA_ZERO_INIT float gain;
|
||||
|
||||
|
||||
void dynNotchInit(const dynNotchConfig_t *config, const timeUs_t targetLooptimeUs)
|
||||
{
|
||||
// initialise even if FEATURE_DYNAMIC_FILTER not set, since it may be set later
|
||||
dynNotchQ = gyroConfig()->dyn_notch_q / 100.0f;
|
||||
dynNotchMinHz = gyroConfig()->dyn_notch_min_hz;
|
||||
dynNotchMaxHz = MAX(2 * dynNotchMinHz, gyroConfig()->dyn_notch_max_hz);
|
||||
dynNotch.q = config->dyn_notch_q / 100.0f;
|
||||
dynNotch.minHz = config->dyn_notch_min_hz;
|
||||
dynNotch.maxHz = MAX(2 * dynNotch.minHz, config->dyn_notch_max_hz);
|
||||
dynNotch.count = config->dyn_notch_count;
|
||||
dynNotch.looptimeUs = targetLooptimeUs;
|
||||
dynNotch.maxCenterFreq = 0;
|
||||
|
||||
// gyroDataAnalyse() is running at targetLoopRateHz (which is PID loop rate aka. 1e6f/gyro.targetLooptimeUs)
|
||||
const float targetLoopRateHz = 1.0f / targetLooptimeUs * 1e6f;
|
||||
numSamples = MAX(1, targetLoopRateHz / (2 * dynNotchMaxHz)); // 600hz, 8k looptime, 6.00
|
||||
// dynNotchUpdate() is running at looprateHz (which is PID looprate aka. 1e6f / gyro.targetLooptime)
|
||||
const float looprateHz = 1.0f / dynNotch.looptimeUs * 1e6f;
|
||||
sampleCount = MAX(1, looprateHz / (2 * dynNotch.maxHz)); // 600hz, 8k looptime, 6.00
|
||||
sampleCountRcp = 1.0f / sampleCount;
|
||||
|
||||
sdftSampleRateHz = targetLoopRateHz / numSamples;
|
||||
sdftSampleRateHz = looprateHz / sampleCount;
|
||||
// eg 8k, user max 600hz, int(8000/1200) = 6 (6.666), sdftSampleRateHz = 1333hz, range 666Hz
|
||||
// eg 4k, user max 600hz, int(4000/1200) = 3 (3.333), sdftSampleRateHz = 1333hz, range 666Hz
|
||||
// eg 2k, user max 600hz, int(2000/1200) = 1 (1.666) sdftSampleRateHz = 2000hz, range 1000Hz
|
||||
|
|
@ -136,93 +175,90 @@ void gyroDataAnalyseInit(gyroAnalyseState_t *state, const uint32_t targetLooptim
|
|||
// eg 1k, user max 600hz, int(1000/1200) = 1 (max(1,0.8333)) sdftSampleRateHz = 1000hz, range 500Hz
|
||||
// the upper limit of DN is always going to be the Nyquist frequency (= sampleRate / 2)
|
||||
|
||||
sdftResolutionHz = sdftSampleRateHz / SDFT_SAMPLE_SIZE; // 13.3hz per bin at 8k
|
||||
sdftStartBin = MAX(2, dynNotchMinHz / sdftResolutionHz + 0.5f); // can't use bin 0 because it is DC.
|
||||
sdftEndBin = MIN(SDFT_BIN_COUNT - 1, dynNotchMaxHz / sdftResolutionHz + 0.5f); // can't use more than SDFT_BIN_COUNT bins.
|
||||
gain = pt1FilterGain(DYN_NOTCH_SMOOTH_HZ, DYN_NOTCH_CALC_TICKS / targetLoopRateHz); // minimum PT1 k value
|
||||
sdftResolutionHz = sdftSampleRateHz / SDFT_SAMPLE_SIZE; // 18.5hz per bin at 8k and 600Hz maxHz
|
||||
sdftStartBin = MAX(2, dynNotch.minHz / sdftResolutionHz + 0.5f); // can't use bin 0 because it is DC.
|
||||
sdftEndBin = MIN(SDFT_BIN_COUNT - 1, dynNotch.maxHz / sdftResolutionHz + 0.5f); // can't use more than SDFT_BIN_COUNT bins.
|
||||
gain = pt1FilterGain(DYN_NOTCH_SMOOTH_HZ, DYN_NOTCH_CALC_TICKS / looprateHz); // minimum PT1 k value
|
||||
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
sdftInit(&sdft[axis], sdftStartBin, sdftEndBin, numSamples);
|
||||
sdftInit(&sdft[axis], sdftStartBin, sdftEndBin, sampleCount);
|
||||
}
|
||||
|
||||
state->maxSampleCount = numSamples;
|
||||
state->maxSampleCountRcp = 1.0f / state->maxSampleCount;
|
||||
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
for (int p = 0; p < gyro.notchFilterDynCount; p++) {
|
||||
for (int p = 0; p < dynNotch.count; p++) {
|
||||
// any init value is fine, but evenly spreading centerFreqs across frequency range makes notch filters stick to peaks quicker
|
||||
state->centerFreq[axis][p] = (p + 0.5f) * (dynNotchMaxHz - dynNotchMinHz) / (float)gyro.notchFilterDynCount + dynNotchMinHz;
|
||||
dynNotch.centerFreq[axis][p] = (p + 0.5f) * (dynNotch.maxHz - dynNotch.minHz) / (float)dynNotch.count + dynNotch.minHz;
|
||||
biquadFilterInit(&dynNotch.notch[axis][p], dynNotch.centerFreq[axis][p], dynNotch.looptimeUs, dynNotch.q, FILTER_NOTCH, 1.0f);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Collect gyro data, to be downsampled and analysed in gyroDataAnalyse() function
|
||||
void gyroDataAnalysePush(gyroAnalyseState_t *state, const int axis, const float sample)
|
||||
// Collect gyro data, to be downsampled and analysed in dynNotchUpdate() function
|
||||
FAST_CODE void dynNotchPush(const int axis, const float sample)
|
||||
{
|
||||
state->oversampledGyroAccumulator[axis] += sample;
|
||||
sampleAccumulator[axis] += sample;
|
||||
}
|
||||
|
||||
static void gyroDataAnalyseUpdate(gyroAnalyseState_t *state);
|
||||
static void dynNotchProcess(void);
|
||||
|
||||
// Downsample and analyse gyro data
|
||||
FAST_CODE void gyroDataAnalyse(gyroAnalyseState_t *state)
|
||||
FAST_CODE void dynNotchUpdate(void)
|
||||
{
|
||||
// samples should have been pushed by `gyroDataAnalysePush`
|
||||
// samples should have been pushed by `dynNotchPush`
|
||||
// if gyro sampling is > 1kHz, accumulate and average multiple gyro samples
|
||||
if (state->sampleCount == state->maxSampleCount) {
|
||||
state->sampleCount = 0;
|
||||
if (sampleIndex == sampleCount) {
|
||||
sampleIndex = 0;
|
||||
|
||||
// calculate mean value of accumulated samples
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
const float sample = state->oversampledGyroAccumulator[axis] * state->maxSampleCountRcp;
|
||||
state->downsampledGyroData[axis] = sample;
|
||||
if (axis == 0) {
|
||||
DEBUG_SET(DEBUG_FFT, 2, lrintf(sample));
|
||||
sampleAvg[axis] = sampleAccumulator[axis] * sampleCountRcp;
|
||||
sampleAccumulator[axis] = 0;
|
||||
if (axis == gyro.gyroDebugAxis) {
|
||||
DEBUG_SET(DEBUG_FFT, 2, lrintf(sampleAvg[axis]));
|
||||
}
|
||||
state->oversampledGyroAccumulator[axis] = 0;
|
||||
}
|
||||
|
||||
// We need DYN_NOTCH_CALC_TICKS ticks to update all axes with newly sampled value
|
||||
// recalculation of filters takes 4 calls per axis => each filter gets updated every DYN_NOTCH_CALC_TICKS calls
|
||||
// at 8kHz PID loop rate this means 8kHz / 4 / 3 = 666Hz => update every 1.5ms
|
||||
// at 4kHz PID loop rate this means 4kHz / 4 / 3 = 333Hz => update every 3ms
|
||||
state->updateTicks = DYN_NOTCH_CALC_TICKS;
|
||||
state.tick = DYN_NOTCH_CALC_TICKS;
|
||||
}
|
||||
|
||||
// 2us @ F722
|
||||
// SDFT processing in batches to synchronize with incoming downsampled data
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
sdftPushBatch(&sdft[axis], state->downsampledGyroData[axis], state->sampleCount);
|
||||
sdftPushBatch(&sdft[axis], sampleAvg[axis], sampleIndex);
|
||||
}
|
||||
state->sampleCount++;
|
||||
sampleIndex++;
|
||||
|
||||
// Find frequency peaks and update filters
|
||||
if (state->updateTicks > 0) {
|
||||
gyroDataAnalyseUpdate(state);
|
||||
--state->updateTicks;
|
||||
if (state.tick > 0) {
|
||||
dynNotchProcess();
|
||||
--state.tick;
|
||||
}
|
||||
}
|
||||
|
||||
// Find frequency peaks and update filters
|
||||
static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state)
|
||||
static FAST_CODE_NOINLINE void dynNotchProcess(void)
|
||||
{
|
||||
uint32_t startTime = 0;
|
||||
if (debugMode == (DEBUG_FFT_TIME)) {
|
||||
if (debugMode == DEBUG_FFT_TIME) {
|
||||
startTime = micros();
|
||||
}
|
||||
|
||||
DEBUG_SET(DEBUG_FFT_TIME, 0, state->updateStep);
|
||||
DEBUG_SET(DEBUG_FFT_TIME, 0, state.step);
|
||||
|
||||
switch (state->updateStep) {
|
||||
switch (state.step) {
|
||||
|
||||
case STEP_WINDOW: // 6us @ F722
|
||||
{
|
||||
sdftWinSq(&sdft[state->updateAxis], sdftData);
|
||||
sdftWinSq(&sdft[state.axis], sdftData);
|
||||
|
||||
// Calculate mean square over frequency range (= average power of vibrations)
|
||||
sdftMeanSq = 0.0f;
|
||||
for (int bin = (sdftStartBin + 1); bin < sdftEndBin; bin++) { // don't use startBin or endBin because they are not windowed properly
|
||||
sdftMeanSq += sdftData[bin]; // sdftData is already squared (see sdftWinSq)
|
||||
sdftMeanSq += sdftData[bin]; // sdftData is already squared (see sdftWinSq)
|
||||
}
|
||||
sdftMeanSq /= sdftEndBin - sdftStartBin - 1;
|
||||
|
||||
|
|
@ -233,7 +269,7 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state)
|
|||
case STEP_DETECT_PEAKS: // 6us @ F722
|
||||
{
|
||||
// Get memory ready for new peak data on current axis
|
||||
for (int p = 0; p < gyro.notchFilterDynCount; p++) {
|
||||
for (int p = 0; p < dynNotch.count; p++) {
|
||||
peaks[p].bin = 0;
|
||||
peaks[p].value = 0.0f;
|
||||
}
|
||||
|
|
@ -244,9 +280,9 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state)
|
|||
if ((sdftData[bin] > sdftData[bin - 1]) && (sdftData[bin] > sdftData[bin + 1])) {
|
||||
// Check if peak is big enough to be one of N biggest peaks.
|
||||
// If so, insert peak and sort peaks in descending height order
|
||||
for (int p = 0; p < gyro.notchFilterDynCount; p++) {
|
||||
for (int p = 0; p < dynNotch.count; p++) {
|
||||
if (sdftData[bin] > peaks[p].value) {
|
||||
for (int k = gyro.notchFilterDynCount - 1; k > p; k--) {
|
||||
for (int k = dynNotch.count - 1; k > p; k--) {
|
||||
peaks[k] = peaks[k - 1];
|
||||
}
|
||||
peaks[p].bin = bin;
|
||||
|
|
@ -259,7 +295,7 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state)
|
|||
}
|
||||
|
||||
// Sort N biggest peaks in ascending bin order (example: 3, 8, 25, 0, 0, ..., 0)
|
||||
for (int p = gyro.notchFilterDynCount - 1; p > 0; p--) {
|
||||
for (int p = dynNotch.count - 1; p > 0; p--) {
|
||||
for (int k = 0; k < p; k++) {
|
||||
// Swap peaks but ignore swapping void peaks (bin = 0). This leaves
|
||||
// void peaks at the end of peaks array without moving them
|
||||
|
|
@ -277,9 +313,9 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state)
|
|||
}
|
||||
case STEP_CALC_FREQUENCIES: // 4us @ F722
|
||||
{
|
||||
for (int p = 0; p < gyro.notchFilterDynCount; p++) {
|
||||
for (int p = 0; p < dynNotch.count; p++) {
|
||||
|
||||
// Only update state->centerFreq if there is a peak (ignore void peaks) and if peak is above noise floor
|
||||
// Only update dynNotch.centerFreq if there is a peak (ignore void peaks) and if peak is above noise floor
|
||||
if (peaks[p].bin != 0 && peaks[p].value > sdftMeanSq) {
|
||||
|
||||
float meanBin = peaks[p].bin;
|
||||
|
|
@ -296,28 +332,28 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state)
|
|||
}
|
||||
|
||||
// Convert bin to frequency: freq = bin * binResoultion (bin 0 is 0Hz)
|
||||
const float centerFreq = constrainf(meanBin * sdftResolutionHz, dynNotchMinHz, dynNotchMaxHz);
|
||||
const float centerFreq = constrainf(meanBin * sdftResolutionHz, dynNotch.minHz, dynNotch.maxHz);
|
||||
|
||||
// PT1 style smoothing moves notch center freqs rapidly towards big peaks and slowly away, up to 8x faster
|
||||
// DYN_NOTCH_SMOOTH_HZ = 4 & gainMultiplier = 1 .. 8 => PT1 -3dB cutoff frequency = 4Hz .. 41Hz
|
||||
const float gainMultiplier = constrainf(peaks[p].value / sdftMeanSq, 1.0f, 8.0f);
|
||||
|
||||
// Finally update notch center frequency p on current axis
|
||||
state->centerFreq[state->updateAxis][p] += gain * gainMultiplier * (centerFreq - state->centerFreq[state->updateAxis][p]);
|
||||
dynNotch.centerFreq[state.axis][p] += gain * gainMultiplier * (centerFreq - dynNotch.centerFreq[state.axis][p]);
|
||||
}
|
||||
}
|
||||
|
||||
if(calculateThrottlePercentAbs() > DYN_NOTCH_OSD_MIN_THROTTLE) {
|
||||
for (int p = 0; p < gyro.notchFilterDynCount; p++) {
|
||||
dynNotchMaxFFT = MAX(dynNotchMaxFFT, state->centerFreq[state->updateAxis][p]);
|
||||
for (int p = 0; p < dynNotch.count; p++) {
|
||||
dynNotch.maxCenterFreq = MAX(dynNotch.maxCenterFreq, dynNotch.centerFreq[state.axis][p]);
|
||||
}
|
||||
}
|
||||
|
||||
if (state->updateAxis == gyro.gyroDebugAxis) {
|
||||
for (int p = 0; p < gyro.notchFilterDynCount && p < 3; p++) {
|
||||
DEBUG_SET(DEBUG_FFT_FREQ, p, lrintf(state->centerFreq[state->updateAxis][p]));
|
||||
if (state.axis == gyro.gyroDebugAxis) {
|
||||
for (int p = 0; p < dynNotch.count && p < 3; p++) {
|
||||
DEBUG_SET(DEBUG_FFT_FREQ, p, lrintf(dynNotch.centerFreq[state.axis][p]));
|
||||
}
|
||||
DEBUG_SET(DEBUG_DYN_LPF, 1, lrintf(state->centerFreq[state->updateAxis][0]));
|
||||
DEBUG_SET(DEBUG_DYN_LPF, 1, lrintf(dynNotch.centerFreq[state.axis][0]));
|
||||
}
|
||||
|
||||
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
|
||||
|
|
@ -326,29 +362,44 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state)
|
|||
}
|
||||
case STEP_UPDATE_FILTERS: // 7us @ F722
|
||||
{
|
||||
for (int p = 0; p < gyro.notchFilterDynCount; p++) {
|
||||
for (int p = 0; p < dynNotch.count; p++) {
|
||||
// Only update notch filter coefficients if the corresponding peak got its center frequency updated in the previous step
|
||||
if (peaks[p].bin != 0 && peaks[p].value > sdftMeanSq) {
|
||||
biquadFilterUpdate(&gyro.notchFilterDyn[state->updateAxis][p], state->centerFreq[state->updateAxis][p], gyro.targetLooptime, dynNotchQ, FILTER_NOTCH, 1.0f);
|
||||
biquadFilterUpdate(&dynNotch.notch[state.axis][p], dynNotch.centerFreq[state.axis][p], dynNotch.looptimeUs, dynNotch.q, FILTER_NOTCH, 1.0f);
|
||||
}
|
||||
}
|
||||
|
||||
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
|
||||
|
||||
state->updateAxis = (state->updateAxis + 1) % XYZ_AXIS_COUNT;
|
||||
state.axis = (state.axis + 1) % XYZ_AXIS_COUNT;
|
||||
}
|
||||
}
|
||||
|
||||
state->updateStep = (state->updateStep + 1) % STEP_COUNT;
|
||||
state.step = (state.step + 1) % STEP_COUNT;
|
||||
}
|
||||
|
||||
FAST_CODE float dynNotchFilter(const int axis, float value)
|
||||
{
|
||||
for (uint8_t p = 0; p < dynNotch.count; p++) {
|
||||
value = biquadFilterApplyDF1(&dynNotch.notch[axis][p], value);
|
||||
}
|
||||
|
||||
uint16_t getMaxFFT(void) {
|
||||
return dynNotchMaxFFT;
|
||||
return value;
|
||||
}
|
||||
|
||||
void resetMaxFFT(void) {
|
||||
dynNotchMaxFFT = 0;
|
||||
bool isDynamicFilterActive(void)
|
||||
{
|
||||
return featureIsEnabled(FEATURE_DYNAMIC_FILTER);
|
||||
}
|
||||
|
||||
#endif // USE_GYRO_DATA_ANALYSE
|
||||
uint16_t getMaxFFT(void)
|
||||
{
|
||||
return dynNotch.maxCenterFreq;
|
||||
}
|
||||
|
||||
void resetMaxFFT(void)
|
||||
{
|
||||
dynNotch.maxCenterFreq = 0;
|
||||
}
|
||||
|
||||
#endif // USE_DYN_NOTCH_FILTER
|
||||
|
|
@ -22,32 +22,16 @@
|
|||
|
||||
#include <stdint.h>
|
||||
|
||||
#include "common/axis.h"
|
||||
#include "common/time.h"
|
||||
|
||||
#include "pg/dyn_notch.h"
|
||||
|
||||
#define DYN_NOTCH_COUNT_MAX 5
|
||||
|
||||
typedef struct gyroAnalyseState_s {
|
||||
|
||||
// accumulator for oversampled data => no aliasing and less noise
|
||||
uint8_t sampleCount;
|
||||
uint8_t maxSampleCount;
|
||||
float maxSampleCountRcp;
|
||||
float oversampledGyroAccumulator[XYZ_AXIS_COUNT];
|
||||
|
||||
// downsampled gyro data for frequency analysis
|
||||
float downsampledGyroData[XYZ_AXIS_COUNT];
|
||||
|
||||
// update state machine step information
|
||||
uint8_t updateTicks;
|
||||
uint8_t updateStep;
|
||||
uint8_t updateAxis;
|
||||
|
||||
float centerFreq[XYZ_AXIS_COUNT][DYN_NOTCH_COUNT_MAX];
|
||||
|
||||
} gyroAnalyseState_t;
|
||||
|
||||
void gyroDataAnalyseInit(gyroAnalyseState_t *state, const uint32_t targetLooptimeUs);
|
||||
void gyroDataAnalysePush(gyroAnalyseState_t *state, const int axis, const float sample);
|
||||
void gyroDataAnalyse(gyroAnalyseState_t *state);
|
||||
void dynNotchInit(const dynNotchConfig_t *config, const timeUs_t targetLooptimeUs);
|
||||
void dynNotchPush(const int axis, const float sample);
|
||||
void dynNotchUpdate(void);
|
||||
float dynNotchFilter(const int axis, float value);
|
||||
bool isDynamicFilterActive(void);
|
||||
uint16_t getMaxFFT(void);
|
||||
void resetMaxFFT(void);
|
||||
|
|
@ -117,6 +117,7 @@
|
|||
|
||||
#include "pg/beeper.h"
|
||||
#include "pg/board.h"
|
||||
#include "pg/dyn_notch.h"
|
||||
#include "pg/gyrodev.h"
|
||||
#include "pg/motor.h"
|
||||
#include "pg/rx.h"
|
||||
|
|
@ -1779,11 +1780,11 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
|
|||
sbufWriteU16(dst, 0);
|
||||
#endif
|
||||
// Added in MSP API 1.42
|
||||
#if defined(USE_GYRO_DATA_ANALYSE)
|
||||
#if defined(USE_DYN_NOTCH_FILTER)
|
||||
sbufWriteU8(dst, 0); // DEPRECATED 1.43: dyn_notch_range
|
||||
sbufWriteU8(dst, 0); // DEPRECATED 1.44: dyn_notch_width_percent
|
||||
sbufWriteU16(dst, gyroConfig()->dyn_notch_q);
|
||||
sbufWriteU16(dst, gyroConfig()->dyn_notch_min_hz);
|
||||
sbufWriteU16(dst, dynNotchConfig()->dyn_notch_q);
|
||||
sbufWriteU16(dst, dynNotchConfig()->dyn_notch_min_hz);
|
||||
#else
|
||||
sbufWriteU8(dst, 0);
|
||||
sbufWriteU8(dst, 0);
|
||||
|
|
@ -1797,9 +1798,9 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
|
|||
sbufWriteU8(dst, 0);
|
||||
sbufWriteU8(dst, 0);
|
||||
#endif
|
||||
#if defined(USE_GYRO_DATA_ANALYSE)
|
||||
#if defined(USE_DYN_NOTCH_FILTER)
|
||||
// Added in MSP API 1.43
|
||||
sbufWriteU16(dst, gyroConfig()->dyn_notch_max_hz);
|
||||
sbufWriteU16(dst, dynNotchConfig()->dyn_notch_max_hz);
|
||||
#else
|
||||
sbufWriteU16(dst, 0);
|
||||
#endif
|
||||
|
|
@ -1809,8 +1810,8 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
|
|||
#else
|
||||
sbufWriteU8(dst, 0);
|
||||
#endif
|
||||
#if defined(USE_GYRO_DATA_ANALYSE)
|
||||
sbufWriteU8(dst, gyroConfig()->dyn_notch_count);
|
||||
#if defined(USE_DYN_NOTCH_FILTER)
|
||||
sbufWriteU8(dst, dynNotchConfig()->dyn_notch_count);
|
||||
#else
|
||||
sbufWriteU8(dst, 0);
|
||||
#endif
|
||||
|
|
@ -2653,11 +2654,11 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
|
|||
}
|
||||
if (sbufBytesRemaining(src) >= 8) {
|
||||
// Added in MSP API 1.42
|
||||
#if defined(USE_GYRO_DATA_ANALYSE)
|
||||
#if defined(USE_DYN_NOTCH_FILTER)
|
||||
sbufReadU8(src); // DEPRECATED 1.43: dyn_notch_range
|
||||
sbufReadU8(src); // DEPRECATED 1.44: dyn_notch_width_percent
|
||||
gyroConfigMutable()->dyn_notch_q = sbufReadU16(src);
|
||||
gyroConfigMutable()->dyn_notch_min_hz = sbufReadU16(src);
|
||||
dynNotchConfigMutable()->dyn_notch_q = sbufReadU16(src);
|
||||
dynNotchConfigMutable()->dyn_notch_min_hz = sbufReadU16(src);
|
||||
#else
|
||||
sbufReadU8(src);
|
||||
sbufReadU8(src);
|
||||
|
|
@ -2673,9 +2674,9 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
|
|||
#endif
|
||||
}
|
||||
if (sbufBytesRemaining(src) >= 2) {
|
||||
#if defined(USE_GYRO_DATA_ANALYSE)
|
||||
#if defined(USE_DYN_NOTCH_FILTER)
|
||||
// Added in MSP API 1.43
|
||||
gyroConfigMutable()->dyn_notch_max_hz = sbufReadU16(src);
|
||||
dynNotchConfigMutable()->dyn_notch_max_hz = sbufReadU16(src);
|
||||
#else
|
||||
sbufReadU16(src);
|
||||
#endif
|
||||
|
|
@ -2687,8 +2688,8 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
|
|||
#else
|
||||
sbufReadU8(src);
|
||||
#endif
|
||||
#if defined(USE_GYRO_DATA_ANALYSE)
|
||||
gyroConfigMutable()->dyn_notch_count = sbufReadU8(src);
|
||||
#if defined(USE_DYN_NOTCH_FILTER)
|
||||
dynNotchConfigMutable()->dyn_notch_count = sbufReadU8(src);
|
||||
#else
|
||||
sbufReadU8(src);
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -64,8 +64,8 @@
|
|||
#include "fc/rc_modes.h"
|
||||
#include "fc/runtime_config.h"
|
||||
|
||||
#if defined(USE_GYRO_DATA_ANALYSE)
|
||||
#include "flight/gyroanalyse.h"
|
||||
#if defined(USE_DYN_NOTCH_FILTER)
|
||||
#include "flight/dyn_notch_filter.h"
|
||||
#endif
|
||||
#include "flight/imu.h"
|
||||
#include "flight/mixer.h"
|
||||
|
|
@ -823,7 +823,7 @@ static bool osdDisplayStat(int statistic, uint8_t displayRow)
|
|||
return true;
|
||||
#endif
|
||||
|
||||
#if defined(USE_GYRO_DATA_ANALYSE)
|
||||
#if defined(USE_DYN_NOTCH_FILTER)
|
||||
case OSD_STAT_MAX_FFT:
|
||||
if (featureIsEnabled(FEATURE_DYNAMIC_FILTER)) {
|
||||
int value = getMaxFFT();
|
||||
|
|
|
|||
|
|
@ -0,0 +1,39 @@
|
|||
/*
|
||||
* This file is part of Cleanflight and Betaflight.
|
||||
*
|
||||
* Cleanflight and Betaflight are free software. You can redistribute
|
||||
* this software and/or modify this software 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 and Betaflight are distributed in the hope that they
|
||||
* 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 software.
|
||||
*
|
||||
* If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#ifdef USE_DYN_NOTCH_FILTER
|
||||
|
||||
#include "pg/pg.h"
|
||||
#include "pg/pg_ids.h"
|
||||
|
||||
#include "dyn_notch.h"
|
||||
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(dynNotchConfig_t, dynNotchConfig, PG_DYN_NOTCH_CONFIG, 0);
|
||||
|
||||
PG_RESET_TEMPLATE(dynNotchConfig_t, dynNotchConfig,
|
||||
.dyn_notch_min_hz = 150,
|
||||
.dyn_notch_max_hz = 600,
|
||||
.dyn_notch_q = 300,
|
||||
.dyn_notch_count = 3
|
||||
);
|
||||
|
||||
#endif // USE_DYN_NOTCH_FILTER
|
||||
|
|
@ -0,0 +1,36 @@
|
|||
/*
|
||||
* This file is part of Cleanflight and Betaflight.
|
||||
*
|
||||
* Cleanflight and Betaflight are free software. You can redistribute
|
||||
* this software and/or modify this software 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 and Betaflight are distributed in the hope that they
|
||||
* 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 software.
|
||||
*
|
||||
* If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include "pg/pg.h"
|
||||
|
||||
typedef struct dynNotchConfig_s
|
||||
{
|
||||
uint16_t dyn_notch_min_hz;
|
||||
uint16_t dyn_notch_max_hz;
|
||||
uint16_t dyn_notch_q;
|
||||
uint8_t dyn_notch_count;
|
||||
|
||||
} dynNotchConfig_t;
|
||||
|
||||
PG_DECLARE(dynNotchConfig_t, dynNotchConfig);
|
||||
|
|
@ -150,7 +150,8 @@
|
|||
#define PG_PULLUP_CONFIG 551
|
||||
#define PG_PULLDOWN_CONFIG 552
|
||||
#define PG_MODE_ACTIVATION_CONFIG 553
|
||||
#define PG_BETAFLIGHT_END 553
|
||||
#define PG_DYN_NOTCH_CONFIG 554
|
||||
#define PG_BETAFLIGHT_END 554
|
||||
|
||||
|
||||
// OSD configuration (subject to change)
|
||||
|
|
|
|||
|
|
@ -45,8 +45,8 @@
|
|||
#include "config/config.h"
|
||||
#include "fc/runtime_config.h"
|
||||
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
#include "flight/gyroanalyse.h"
|
||||
#ifdef USE_DYN_NOTCH_FILTER
|
||||
#include "flight/dyn_notch_filter.h"
|
||||
#endif
|
||||
#include "flight/rpm_filter.h"
|
||||
|
||||
|
|
@ -128,23 +128,12 @@ void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig)
|
|||
gyroConfig->yaw_spin_threshold = 1950;
|
||||
gyroConfig->dyn_lpf_gyro_min_hz = DYN_LPF_GYRO_MIN_HZ_DEFAULT;
|
||||
gyroConfig->dyn_lpf_gyro_max_hz = DYN_LPF_GYRO_MAX_HZ_DEFAULT;
|
||||
gyroConfig->dyn_notch_max_hz = 600;
|
||||
gyroConfig->dyn_notch_count = 3;
|
||||
gyroConfig->dyn_notch_q = 300;
|
||||
gyroConfig->dyn_notch_min_hz = 150;
|
||||
gyroConfig->gyro_filter_debug_axis = FD_ROLL;
|
||||
gyroConfig->dyn_lpf_curve_expo = 5;
|
||||
gyroConfig->simplified_gyro_filter = false;
|
||||
gyroConfig->simplified_gyro_filter_multiplier = SIMPLIFIED_TUNING_DEFAULT;
|
||||
}
|
||||
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
bool isDynamicFilterActive(void)
|
||||
{
|
||||
return featureIsEnabled(FEATURE_DYNAMIC_FILTER);
|
||||
}
|
||||
#endif
|
||||
|
||||
FAST_CODE bool isGyroSensorCalibrationComplete(const gyroSensor_t *gyroSensor)
|
||||
{
|
||||
return gyroSensor->calibration.cyclesRemaining == 0;
|
||||
|
|
@ -482,9 +471,9 @@ FAST_CODE void gyroFiltering(timeUs_t currentTimeUs)
|
|||
filterGyroDebug();
|
||||
}
|
||||
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
#ifdef USE_DYN_NOTCH_FILTER
|
||||
if (isDynamicFilterActive()) {
|
||||
gyroDataAnalyse(&gyro.gyroAnalyseState);
|
||||
dynNotchUpdate();
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
|
|||
|
|
@ -28,8 +28,8 @@
|
|||
#include "drivers/bus.h"
|
||||
#include "drivers/sensor.h"
|
||||
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
#include "flight/gyroanalyse.h"
|
||||
#ifdef USE_DYN_NOTCH_FILTER
|
||||
#include "flight/dyn_notch_filter.h"
|
||||
#endif
|
||||
|
||||
#include "flight/pid.h"
|
||||
|
|
@ -109,14 +109,6 @@ typedef struct gyro_s {
|
|||
filterApplyFnPtr notchFilter2ApplyFn;
|
||||
biquadFilter_t notchFilter2[XYZ_AXIS_COUNT];
|
||||
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
filterApplyFnPtr notchFilterDynApplyFn;
|
||||
biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT][DYN_NOTCH_COUNT_MAX];
|
||||
uint8_t notchFilterDynCount;
|
||||
|
||||
gyroAnalyseState_t gyroAnalyseState;
|
||||
#endif
|
||||
|
||||
uint16_t accSampleRateHz;
|
||||
uint8_t gyroToUse;
|
||||
uint8_t gyroDebugMode;
|
||||
|
|
@ -198,11 +190,6 @@ typedef struct gyroConfig_s {
|
|||
uint16_t dyn_lpf_gyro_min_hz;
|
||||
uint16_t dyn_lpf_gyro_max_hz;
|
||||
|
||||
uint16_t dyn_notch_max_hz;
|
||||
uint8_t dyn_notch_count;
|
||||
uint16_t dyn_notch_q;
|
||||
uint16_t dyn_notch_min_hz;
|
||||
|
||||
uint8_t gyro_filter_debug_axis;
|
||||
|
||||
uint8_t gyrosDetected; // What gyros should detection be attempted for on startup. Automatically set on first startup.
|
||||
|
|
@ -231,6 +218,3 @@ void dynLpfGyroUpdate(float throttle);
|
|||
#ifdef USE_YAW_SPIN_RECOVERY
|
||||
void initYawSpinRecovery(int maxYawRate);
|
||||
#endif
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
bool isDynamicFilterActive(void);
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -64,7 +64,7 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(void)
|
|||
// DEBUG_GYRO_SAMPLE(3) Record the post-static notch and lowpass filter value for the selected debug axis
|
||||
GYRO_FILTER_AXIS_DEBUG_SET(axis, DEBUG_GYRO_SAMPLE, 3, lrintf(gyroADCf));
|
||||
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
#ifdef USE_DYN_NOTCH_FILTER
|
||||
if (isDynamicFilterActive()) {
|
||||
if (axis == gyro.gyroDebugAxis) {
|
||||
GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf));
|
||||
|
|
@ -72,10 +72,8 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(void)
|
|||
GYRO_FILTER_DEBUG_SET(DEBUG_DYN_LPF, 0, lrintf(gyroADCf));
|
||||
}
|
||||
|
||||
gyroDataAnalysePush(&gyro.gyroAnalyseState, axis, gyroADCf);
|
||||
for (uint8_t p = 0; p < gyro.notchFilterDynCount; p++) {
|
||||
gyroADCf = gyro.notchFilterDynApplyFn((filter_t*)&gyro.notchFilterDyn[axis][p], gyroADCf);
|
||||
}
|
||||
dynNotchPush(axis, gyroADCf);
|
||||
gyroADCf = dynNotchFilter(axis, gyroADCf);
|
||||
|
||||
if (axis == gyro.gyroDebugAxis) {
|
||||
GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf));
|
||||
|
|
|
|||
|
|
@ -63,8 +63,8 @@
|
|||
|
||||
#include "fc/runtime_config.h"
|
||||
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
#include "flight/gyroanalyse.h"
|
||||
#ifdef USE_DYN_NOTCH_FILTER
|
||||
#include "flight/dyn_notch_filter.h"
|
||||
#endif
|
||||
|
||||
#include "pg/gyrodev.h"
|
||||
|
|
@ -72,11 +72,6 @@
|
|||
#include "sensors/gyro.h"
|
||||
#include "sensors/sensors.h"
|
||||
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
#define DYNAMIC_NOTCH_DEFAULT_CENTER_HZ 350
|
||||
#define DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ 300
|
||||
#endif
|
||||
|
||||
#ifdef USE_MULTI_GYRO
|
||||
#define ACTIVE_GYRO ((gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_2) ? &gyro.gyroSensor2 : &gyro.gyroSensor1)
|
||||
#else
|
||||
|
|
@ -129,25 +124,6 @@ static void gyroInitFilterNotch2(uint16_t notchHz, uint16_t notchCutoffHz)
|
|||
}
|
||||
}
|
||||
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
static void gyroInitFilterDynamicNotch()
|
||||
{
|
||||
gyro.notchFilterDynApplyFn = nullFilterApply;
|
||||
|
||||
if (isDynamicFilterActive()) {
|
||||
gyro.notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2
|
||||
gyro.notchFilterDynCount = gyroConfig()->dyn_notch_count;
|
||||
|
||||
const float notchQ = filterGetNotchQ(DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ); // any defaults OK here
|
||||
for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
for (uint8_t p = 0; p < gyro.notchFilterDynCount; p++) {
|
||||
biquadFilterInit(&gyro.notchFilterDyn[axis][p], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, gyro.targetLooptime, notchQ, FILTER_NOTCH, 1.0f);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
static bool gyroInitLowpassFilterLpf(int slot, int type, uint16_t lpfHz, uint32_t looptime)
|
||||
{
|
||||
filterApplyFnPtr *lowpassFilterApplyFn;
|
||||
|
|
@ -279,14 +255,11 @@ void gyroInitFilters(void)
|
|||
|
||||
gyroInitFilterNotch1(gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1);
|
||||
gyroInitFilterNotch2(gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2);
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
gyroInitFilterDynamicNotch();
|
||||
#endif
|
||||
#ifdef USE_DYN_LPF
|
||||
dynLpfFilterInit();
|
||||
#endif
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
gyroDataAnalyseInit(&gyro.gyroAnalyseState, gyro.targetLooptime);
|
||||
#ifdef USE_DYN_NOTCH_FILTER
|
||||
dynNotchInit(dynNotchConfig(), gyro.targetLooptime);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -302,7 +302,7 @@
|
|||
|
||||
#if defined(SIMULATOR_BUILD) || defined(UNIT_TEST)
|
||||
// This feature uses 'arm_math.h', which does not exist for x86.
|
||||
#undef USE_GYRO_DATA_ANALYSE
|
||||
#undef USE_DYN_NOTCH_FILTER
|
||||
#endif
|
||||
|
||||
#ifndef USE_CMS
|
||||
|
|
|
|||
|
|
@ -40,7 +40,7 @@
|
|||
#ifdef STM32F3
|
||||
#define MINIMAL_CLI
|
||||
#define USE_DSHOT
|
||||
#define USE_GYRO_DATA_ANALYSE
|
||||
#define USE_DYN_NOTCH_FILTER
|
||||
#define USE_CCM_CODE
|
||||
#endif
|
||||
|
||||
|
|
@ -54,7 +54,7 @@
|
|||
#define USE_DSHOT_TELEMETRY_STATS
|
||||
#define USE_RPM_FILTER
|
||||
#define USE_DYN_IDLE
|
||||
#define USE_GYRO_DATA_ANALYSE
|
||||
#define USE_DYN_NOTCH_FILTER
|
||||
#define USE_ADC
|
||||
#define USE_ADC_INTERNAL
|
||||
#define USE_USB_CDC_HID
|
||||
|
|
@ -81,7 +81,7 @@
|
|||
#define USE_DSHOT_TELEMETRY_STATS
|
||||
#define USE_RPM_FILTER
|
||||
#define USE_DYN_IDLE
|
||||
#define USE_GYRO_DATA_ANALYSE
|
||||
#define USE_DYN_NOTCH_FILTER
|
||||
#define USE_OVERCLOCK
|
||||
#define USE_ADC_INTERNAL
|
||||
#define USE_USB_CDC_HID
|
||||
|
|
@ -103,7 +103,7 @@
|
|||
#define USE_DSHOT_TELEMETRY_STATS
|
||||
#define USE_RPM_FILTER
|
||||
#define USE_DYN_IDLE
|
||||
#define USE_GYRO_DATA_ANALYSE
|
||||
#define USE_DYN_NOTCH_FILTER
|
||||
#define USE_ADC_INTERNAL
|
||||
#define USE_USB_CDC_HID
|
||||
#define USE_DMA_SPEC
|
||||
|
|
@ -126,7 +126,7 @@
|
|||
#define USE_RPM_FILTER
|
||||
#define USE_DYN_IDLE
|
||||
#define USE_OVERCLOCK
|
||||
#define USE_GYRO_DATA_ANALYSE
|
||||
#define USE_DYN_NOTCH_FILTER
|
||||
#define USE_ADC_INTERNAL
|
||||
#define USE_USB_MSC
|
||||
#define USE_USB_CDC_HID
|
||||
|
|
|
|||
Loading…
Reference in New Issue