diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 67e60746a..58e1fbd45 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -453,6 +453,10 @@ const char * const lookupTableLEDRaceColors[COLOR_COUNT] = { "DEEP_PINK" }; +static const char * const lookupTableGyroFilterDebug[] = { + "ROLL", "PITCH", "YAW" +}; + #define LOOKUP_TABLE_ENTRY(name) { name, ARRAYLEN(name) } const lookupTableEntry_t lookupTables[] = { @@ -565,6 +569,8 @@ const lookupTableEntry_t lookupTables[] = { LOOKUP_TABLE_ENTRY(lookupTableLEDProfile), LOOKUP_TABLE_ENTRY(lookupTableLEDRaceColors), #endif + + LOOKUP_TABLE_ENTRY(lookupTableGyroFilterDebug), }; #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_max_hz", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dyn_lpf_dterm_max_hz) }, #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 { "align_acc", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_align) }, diff --git a/src/main/interface/settings.h b/src/main/interface/settings.h index 753055474..cec73d175 100644 --- a/src/main/interface/settings.h +++ b/src/main/interface/settings.h @@ -135,6 +135,7 @@ typedef enum { TABLE_LED_PROFILE, TABLE_LED_RACE_COLOR, #endif + TABLE_GYRO_FILTER_DEBUG, LOOKUP_TABLE_COUNT } lookupTableIndex_e; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index fd76fa976..7a9e1798d 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -159,6 +159,7 @@ typedef struct gyroSensor_s { gyroAnalyseState_t gyroAnalyseState; #endif + flight_dynamics_index_t gyroDebugAxis; } gyroSensor_t; 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_lowpass2_hz = 0; #endif + gyroConfig->gyro_filter_debug_axis = FD_ROLL; } #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) { + gyroSensor->gyroDebugAxis = gyroConfig()->gyro_filter_debug_axis; gyroSensor->gyroDev.gyro_high_fsr = gyroConfig()->gyro_high_fsr; gyroSensor->gyroDev.gyroAlign = config->align; gyroSensor->gyroDev.mpuIntExtiTag = config->extiTag; diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 71833aaca..d9e5577ac 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -104,6 +104,7 @@ typedef struct gyroConfig_s { uint8_t dyn_notch_width_percent; uint16_t dyn_notch_q; uint16_t dyn_notch_min_hz; + uint8_t gyro_filter_debug_axis; } gyroConfig_t; PG_DECLARE(gyroConfig_t, gyroConfig); diff --git a/src/main/sensors/gyro_filter_impl.h b/src/main/sensors/gyro_filter_impl.h index 977eb5f30..119ecbf32 100644 --- a/src/main/sensors/gyro_filter_impl.h +++ b/src/main/sensors/gyro_filter_impl.h @@ -9,7 +9,7 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor) #ifdef USE_GYRO_DATA_ANALYSE if (isDynamicFilterActive()) { - if (axis == X) { + if (axis == gyroSensor->gyroDebugAxis) { GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf)); GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 3, 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 if (isDynamicFilterActive()) { - if (axis == X) { + if (axis == gyroSensor->gyroDebugAxis) { GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf)); GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 2, lrintf(gyroADCf)); GYRO_FILTER_DEBUG_SET(DEBUG_DYN_LPF, 3, lrintf(gyroADCf));