Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 01:05:21 +03:00
Code Issues Wiki Activity

Add a debug for new dynamic filters

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2020-02-04 19:38:06 +01:00
parent 51eb825bab
commit 1e06e7fd1c
4 changed files with 8 additions and 1 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/build/debug.h
View file

@ -67,5 +67,7 @@ typedef enum {
DEBUG_ERPM, DEBUG_ERPM,
DEBUG_RPM_FILTER, DEBUG_RPM_FILTER,
DEBUG_RPM_FREQ, DEBUG_RPM_FREQ,
DEBUG_DYNAMIC_FILTER,
DEBUG_DYNAMIC_FILTER_FREQUENCY,
DEBUG_COUNT DEBUG_COUNT
} debugType_e; } debugType_e;

2
src/main/fc/settings.yaml
View file

@ -81,7 +81,7 @@ tables:
values: ["NONE", "GYRO", "AGL", "FLOW_RAW", values: ["NONE", "GYRO", "AGL", "FLOW_RAW",
"FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE", "FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE",
"VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "ITERM_RELAX", "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "ITERM_RELAX",
"ERPM", "RPM_FILTER", "RPM_FREQ"] "ERPM", "RPM_FILTER", "RPM_FREQ", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY"]
- name: async_mode - name: async_mode
values: ["NONE", "GYRO", "ALL"] values: ["NONE", "GYRO", "ALL"]
- name: aux_operator - name: aux_operator

3
src/main/flight/dynamic_gyro_notch.c
View file

@ -29,6 +29,7 @@
#include <stdint.h> #include <stdint.h>
#include "dynamic_gyro_notch.h" #include "dynamic_gyro_notch.h"
#include "fc/config.h" #include "fc/config.h"
#include "build/debug.h"
#include "sensors/gyro.h" #include "sensors/gyro.h"
void dynamicGyroNotchFiltersInit(dynamicGyroNotchState_t *state) { void dynamicGyroNotchFiltersInit(dynamicGyroNotchState_t *state) {
@ -75,6 +76,8 @@ void dynamicGyroNotchFiltersUpdate(dynamicGyroNotchState_t *state, int axis, uin
state->frequency[axis] = frequency; state->frequency[axis] = frequency;
DEBUG_SET(DEBUG_DYNAMIC_FILTER_FREQUENCY, axis, frequency);
if (state->filterType == DYNAMIC_GYRO_NOTCH_SINGLE) { if (state->filterType == DYNAMIC_GYRO_NOTCH_SINGLE) {
biquadFilterUpdate(&state->filters[axis][0], frequency, state->looptime, state->dynNotchQ, FILTER_NOTCH); biquadFilterUpdate(&state->filters[axis][0], frequency, state->looptime, state->dynNotchQ, FILTER_NOTCH);
} else if (state->filterType == DYNAMIC_GYRO_NOTCH_DUAL) { } else if (state->filterType == DYNAMIC_GYRO_NOTCH_DUAL) {

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

@ -449,7 +449,9 @@ void FAST_CODE NOINLINE gyroUpdate()
#ifdef USE_DYNAMIC_FILTERS #ifdef USE_DYNAMIC_FILTERS
if (dynamicGyroNotchState.filterType) { if (dynamicGyroNotchState.filterType) {
gyroDataAnalysePush(&gyroAnalyseState, axis, gyroADCf); gyroDataAnalysePush(&gyroAnalyseState, axis, gyroADCf);
DEBUG_SET(DEBUG_DYNAMIC_FILTER, axis, gyroADCf);
gyroADCf = dynamicGyroNotchFiltersApply(&dynamicGyroNotchState, axis, gyroADCf); gyroADCf = dynamicGyroNotchFiltersApply(&dynamicGyroNotchState, axis, gyroADCf);
DEBUG_SET(DEBUG_DYNAMIC_FILTER, axis + 3, gyroADCf);
} }
#endif #endif
gyro.gyroADCf[axis] = gyroADCf; gyro.gyroADCf[axis] = gyroADCf;