rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #10673 from KarateBrot/sdft 

Remove Dynamic Notch from "Features"
This commit is contained in:
haslinghuis 2021-12-01 23:21:05 +01:00 committed by GitHub
parent ec96415e52 450cee3e08
commit 2a954c6203
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
12 changed files with 29 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/main/cli/cli.c
View File

@ -265,7 +265,7 @@ static const char * const featureNames[] = {
"RANGEFINDER", "TELEMETRY", "", "3D", "RX_PARALLEL_PWM",
"RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "OSD",
"", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE",
"", "", "RX_SPI", "", "ESC_SENSOR", "ANTI_GRAVITY", "DYNAMIC_FILTER", NULL
"", "", "RX_SPI", "", "ESC_SENSOR", "ANTI_GRAVITY", "", NULL
};
// sync this with rxFailsafeChannelMode_e

2
src/main/cli/settings.c
View File

@ -663,7 +663,7 @@ const clivalue_t valueTable[] = {
{ "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_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_count", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 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) },

6
src/main/cms/cms_menu_imu.c
View File

@ -828,10 +828,10 @@ static const OSD_Entry cmsx_menuDynFiltEntries[] =
{ "-- DYN FILT --", OME_Label, NULL, NULL, 0 },
#ifdef USE_DYN_NOTCH_FILTER
{ "NOTCH COUNT", OME_UINT8, NULL, &(OSD_UINT8_t) { &dynFiltNotchCount, 1, DYN_NOTCH_COUNT_MAX, 1 }, 0 },
{ "NOTCH COUNT", OME_UINT8, NULL, &(OSD_UINT8_t) { &dynFiltNotchCount, 0, 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 },
{ "NOTCH MIN HZ", OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltNotchMinHz, 60, 250, 1 }, 0 },
{ "NOTCH MAX HZ", OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltNotchMaxHz, 200, 1000, 1 }, 0 },
#endif
#ifdef USE_DYN_LPF

14
src/main/config/config.c
View File

@ -102,8 +102,6 @@ pidProfile_t *currentPidProfile;
#define RX_SPI_DEFAULT_PROTOCOL 0
#endif
#define DYNAMIC_FILTER_MAX_SUPPORTED_LOOP_TIME HZ_TO_INTERVAL_US(2000)
PG_REGISTER_WITH_RESET_TEMPLATE(pilotConfig_t, pilotConfig, PG_PILOT_CONFIG, 1);
PG_RESET_TEMPLATE(pilotConfig_t, pilotConfig,
@ -466,10 +464,6 @@ static void validateAndFixConfig(void)
featureDisableImmediate(FEATURE_ESC_SENSOR);
#endif
#ifndef USE_DYN_NOTCH_FILTER
featureDisableImmediate(FEATURE_DYNAMIC_FILTER);
#endif
#if !defined(USE_ADC)
featureDisableImmediate(FEATURE_RSSI_ADC);
#endif
@ -682,14 +676,6 @@ void validateAndFixGyroConfig(void)
}
}
#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) {
featureDisableImmediate(FEATURE_DYNAMIC_FILTER);
}
#endif
#ifdef USE_BLACKBOX
#ifndef USE_FLASHFS
if (blackboxConfig()->device == BLACKBOX_DEVICE_FLASH) {

4
src/main/config/feature.c
View File

@ -29,10 +29,10 @@
#include "pg/pg_ids.h"
PG_REGISTER_WITH_RESET_TEMPLATE(featureConfig_t, featureConfig, PG_FEATURE_CONFIG, 0);
PG_REGISTER_WITH_RESET_TEMPLATE(featureConfig_t, featureConfig, PG_FEATURE_CONFIG, 1);
PG_RESET_TEMPLATE(featureConfig_t, featureConfig,
.enabledFeatures = DEFAULT_FEATURES | DEFAULT_RX_FEATURE | FEATURE_DYNAMIC_FILTER | FEATURE_ANTI_GRAVITY | FEATURE_AIRMODE,
.enabledFeatures = DEFAULT_FEATURES | DEFAULT_RX_FEATURE | FEATURE_ANTI_GRAVITY | FEATURE_AIRMODE,
);
static uint32_t runtimeFeatureMask;

2
src/main/config/feature.h
View File

@ -53,7 +53,7 @@ typedef enum {
//FEATURE_SOFTSPI = 1 << 26, (removed)
FEATURE_ESC_SENSOR = 1 << 27,
FEATURE_ANTI_GRAVITY = 1 << 28,
FEATURE_DYNAMIC_FILTER = 1 << 29,
//FEATURE_DYNAMIC_FILTER = 1 << 29, (removed)
} features_e;
typedef struct featureConfig_s {

20
src/main/flight/dyn_notch_filter.c
View File

@ -29,6 +29,7 @@
*/
#include <math.h>
#include <stdint.h>
#include "platform.h"
@ -85,6 +86,7 @@
#define DYN_NOTCH_SMOOTH_HZ 4
#define DYN_NOTCH_CALC_TICKS (XYZ_AXIS_COUNT * STEP_COUNT) // 3 axes and 4 steps per axis
#define DYN_NOTCH_OSD_MIN_THROTTLE 20
#define DYN_NOTCH_UPDATE_MIN_HZ 2000
typedef enum {
@ -119,7 +121,7 @@ typedef struct dynNotch_s {
float maxHz;
int count;
uint16_t maxCenterFreq;
int maxCenterFreq;
float centerFreq[XYZ_AXIS_COUNT][DYN_NOTCH_COUNT_MAX];
timeUs_t looptimeUs;
@ -154,7 +156,7 @@ 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
// always initialise, since the dynamic notch could be activated at any time
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);
@ -164,6 +166,12 @@ void dynNotchInit(const dynNotchConfig_t *config, const timeUs_t targetLooptimeU
// dynNotchUpdate() is running at looprateHz (which is PID looprate aka. 1e6f / gyro.targetLooptime)
const float looprateHz = 1.0f / dynNotch.looptimeUs * 1e6f;
// Disable dynamic notch if dynNotchUpdate() would run at less than 2kHz
if (looprateHz < DYN_NOTCH_UPDATE_MIN_HZ) {
dynNotch.count = 0;
}
sampleCount = MAX(1, looprateHz / (2 * dynNotch.maxHz)); // 600hz, 8k looptime, 6.00
sampleCountRcp = 1.0f / sampleCount;
@ -380,19 +388,19 @@ static FAST_CODE_NOINLINE void dynNotchProcess(void)
FAST_CODE float dynNotchFilter(const int axis, float value)
{
for (uint8_t p = 0; p < dynNotch.count; p++) {
for (int p = 0; p < dynNotch.count; p++) {
value = biquadFilterApplyDF1(&dynNotch.notch[axis][p], value);
}
return value;
}
bool isDynamicFilterActive(void)
bool isDynNotchActive(void)
{
return featureIsEnabled(FEATURE_DYNAMIC_FILTER);
return dynNotch.count > 0;
}
uint16_t getMaxFFT(void)
int getMaxFFT(void)
{
return dynNotch.maxCenterFreq;
}

6
src/main/flight/dyn_notch_filter.h
View File

@ -20,7 +20,7 @@
#pragma once
#include <stdint.h>
#include <stdbool.h>
#include "common/time.h"
@ -32,6 +32,6 @@ void dynNotchInit(const dynNotchConfig_t *config, const timeUs_t targetLooptimeU
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);
bool isDynNotchActive(void);
int getMaxFFT(void);
void resetMaxFFT(void);

2
src/main/osd/osd.c
View File

@ -827,7 +827,7 @@ static bool osdDisplayStat(int statistic, uint8_t displayRow)
#if defined(USE_DYN_NOTCH_FILTER)
case OSD_STAT_MAX_FFT:
if (featureIsEnabled(FEATURE_DYNAMIC_FILTER)) {
if (isDynNotchActive()) {
int value = getMaxFFT();
if (value > 0) {
tfp_sprintf(buff, "%dHZ", value);

2
src/main/sensors/gyro.c
View File

@ -473,7 +473,7 @@ FAST_CODE void gyroFiltering(timeUs_t currentTimeUs)
}
#ifdef USE_DYN_NOTCH_FILTER
if (isDynamicFilterActive()) {
if (isDynNotchActive()) {
dynNotchUpdate();
}
#endif

2
src/main/sensors/gyro_filter_impl.c
View File

@ -65,7 +65,7 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(void)
GYRO_FILTER_AXIS_DEBUG_SET(axis, DEBUG_GYRO_SAMPLE, 3, lrintf(gyroADCf));
#ifdef USE_DYN_NOTCH_FILTER
if (isDynamicFilterActive()) {
if (isDynNotchActive()) {
if (axis == gyro.gyroDebugAxis) {
GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf));
GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf));

2
src/main/target/ALIENWHOOP/config.c
View File

@ -103,7 +103,7 @@ void targetConfiguration(void)
pidConfigMutable()->runaway_takeoff_prevention = false;
featureConfigSet((FEATURE_DYNAMIC_FILTER | FEATURE_AIRMODE | FEATURE_ANTI_GRAVITY) ^ FEATURE_RX_PARALLEL_PWM);
featureConfigSet((FEATURE_AIRMODE | FEATURE_ANTI_GRAVITY) ^ FEATURE_RX_PARALLEL_PWM);
/* AlienWhoop PIDs tested with 6mm and 7mm motors on most frames */
for (uint8_t pidProfileIndex = 0; pidProfileIndex < PID_PROFILE_COUNT; pidProfileIndex++) {