Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-26 01:35:41 +03:00
Code Issues Wiki Activity

Merge pull request #7427 from fujin/dyn-lpf-debug-pitch

Browse source 
Extend gyro filter implementation debugging for pitch and yaw axis
This commit is contained in:
Michael Keller 2019-01-22 18:11:53 +13:00 • committed by GitHub
commit c39ea81b6a
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
5 changed files with 14 additions and 2 deletions
Download patch file Download diff file Expand all files Collapse all files

7
src/main/interface/settings.c
View file

@ -453,6 +453,10 @@ const char * const lookupTableLEDRaceColors[COLOR_COUNT] = {
"DEEP_PINK" "DEEP_PINK"
}; };
static const char * const lookupTableGyroFilterDebug[] = {
"ROLL", "PITCH", "YAW"
};
#define LOOKUP_TABLE_ENTRY(name) { name, ARRAYLEN(name) } #define LOOKUP_TABLE_ENTRY(name) { name, ARRAYLEN(name) }
const lookupTableEntry_t lookupTables[] = { const lookupTableEntry_t lookupTables[] = {
@ -565,6 +569,8 @@ const lookupTableEntry_t lookupTables[] = {
LOOKUP_TABLE_ENTRY(lookupTableLEDProfile), LOOKUP_TABLE_ENTRY(lookupTableLEDProfile),
LOOKUP_TABLE_ENTRY(lookupTableLEDRaceColors), LOOKUP_TABLE_ENTRY(lookupTableLEDRaceColors),
#endif #endif
LOOKUP_TABLE_ENTRY(lookupTableGyroFilterDebug),
}; };
#undef LOOKUP_TABLE_ENTRY #undef LOOKUP_TABLE_ENTRY
@ -621,6 +627,7 @@ const clivalue_t valueTable[] = {
{ "dyn_lpf_dterm_min_hz", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dyn_lpf_dterm_min_hz) }, { "dyn_lpf_dterm_min_hz", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dyn_lpf_dterm_min_hz) },
{ "dyn_lpf_dterm_max_hz", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dyn_lpf_dterm_max_hz) }, { "dyn_lpf_dterm_max_hz", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dyn_lpf_dterm_max_hz) },
#endif #endif
{ "gyro_filter_debug_axis", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO_FILTER_DEBUG }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_filter_debug_axis) },
// PG_ACCELEROMETER_CONFIG // PG_ACCELEROMETER_CONFIG
{ "align_acc", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_align) }, { "align_acc", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_align) },

1
src/main/interface/settings.h
View file

@ -135,6 +135,7 @@ typedef enum {
TABLE_LED_PROFILE, TABLE_LED_PROFILE,
TABLE_LED_RACE_COLOR, TABLE_LED_RACE_COLOR,
#endif #endif
TABLE_GYRO_FILTER_DEBUG,
LOOKUP_TABLE_COUNT LOOKUP_TABLE_COUNT
} lookupTableIndex_e; } lookupTableIndex_e;

3
src/main/sensors/gyro.c
View file

@ -159,6 +159,7 @@ typedef struct gyroSensor_s {
gyroAnalyseState_t gyroAnalyseState; gyroAnalyseState_t gyroAnalyseState;
#endif #endif
flight_dynamics_index_t gyroDebugAxis;
} gyroSensor_t; } gyroSensor_t;
STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT gyroSensor_t gyroSensor1; STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT gyroSensor_t gyroSensor1;
@ -228,6 +229,7 @@ void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig)
gyroConfig->gyro_lowpass_type = FILTER_BIQUAD; gyroConfig->gyro_lowpass_type = FILTER_BIQUAD;
gyroConfig->gyro_lowpass2_hz = 0; gyroConfig->gyro_lowpass2_hz = 0;
#endif #endif
gyroConfig->gyro_filter_debug_axis = FD_ROLL;
} }
#ifdef USE_MULTI_GYRO #ifdef USE_MULTI_GYRO
@ -414,6 +416,7 @@ static void gyroPreInitSensor(const gyroDeviceConfig_t *config)
static bool gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *config) static bool gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *config)
{ {
gyroSensor->gyroDebugAxis = gyroConfig()->gyro_filter_debug_axis;
gyroSensor->gyroDev.gyro_high_fsr = gyroConfig()->gyro_high_fsr; gyroSensor->gyroDev.gyro_high_fsr = gyroConfig()->gyro_high_fsr;
gyroSensor->gyroDev.gyroAlign = config->align; gyroSensor->gyroDev.gyroAlign = config->align;
gyroSensor->gyroDev.mpuIntExtiTag = config->extiTag; gyroSensor->gyroDev.mpuIntExtiTag = config->extiTag;

1
src/main/sensors/gyro.h
View file

@ -104,6 +104,7 @@ typedef struct gyroConfig_s {
uint8_t dyn_notch_width_percent; uint8_t dyn_notch_width_percent;
uint16_t dyn_notch_q; uint16_t dyn_notch_q;
uint16_t dyn_notch_min_hz; uint16_t dyn_notch_min_hz;
uint8_t gyro_filter_debug_axis;
} gyroConfig_t; } gyroConfig_t;
PG_DECLARE(gyroConfig_t, gyroConfig); PG_DECLARE(gyroConfig_t, gyroConfig);

4
src/main/sensors/gyro_filter_impl.h
View file

@ -9,7 +9,7 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor)
#ifdef USE_GYRO_DATA_ANALYSE #ifdef USE_GYRO_DATA_ANALYSE
if (isDynamicFilterActive()) { if (isDynamicFilterActive()) {
if (axis == X) { if (axis == gyroSensor->gyroDebugAxis) {
GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf)); GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf));
GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf)); GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf));
GYRO_FILTER_DEBUG_SET(DEBUG_DYN_LPF, 0, lrintf(gyroADCf)); GYRO_FILTER_DEBUG_SET(DEBUG_DYN_LPF, 0, lrintf(gyroADCf));
@ -30,7 +30,7 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor)
#ifdef USE_GYRO_DATA_ANALYSE #ifdef USE_GYRO_DATA_ANALYSE
if (isDynamicFilterActive()) { if (isDynamicFilterActive()) {
if (axis == X) { if (axis == gyroSensor->gyroDebugAxis) {
GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf)); GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf));
GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 2, lrintf(gyroADCf)); GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 2, lrintf(gyroADCf));
GYRO_FILTER_DEBUG_SET(DEBUG_DYN_LPF, 3, lrintf(gyroADCf)); GYRO_FILTER_DEBUG_SET(DEBUG_DYN_LPF, 3, lrintf(gyroADCf));