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

Drop the last static gyro notch

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2021-11-03 15:17:06 +01:00
parent f8c9c3ec0b
commit abeb205bb7
8 changed files with 7 additions and 45 deletions
Download patch file Download diff file Expand all files Collapse all files

4
src/main/blackbox/blackbox.c
View file

@ -1812,10 +1812,6 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchQ", "%d", gyroConfig()->dynamicGyroNotchQ);
BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchMinHz", "%d", gyroConfig()->dynamicGyroNotchMinHz);
#endif
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz", "%d,%d", gyroConfig()->gyro_notch_hz,
0);
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff", "%d,%d", gyroConfig()->gyro_notch_cutoff,
1);
BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz", "%d", accelerometerConfig()->acc_lpf_hz);
BLACKBOX_PRINT_HEADER_LINE("acc_hardware", "%d", accelerometerConfig()->acc_hardware);
#ifdef USE_BARO

3
src/main/fc/config.c
View file

@ -183,9 +183,6 @@ uint32_t getGyroLooptime(void) {
void validateAndFixConfig(void)
{
if (gyroConfig()->gyro_notch_cutoff >= gyroConfig()->gyro_notch_hz) {
gyroConfigMutable()->gyro_notch_hz = 0;
}
if (accelerometerConfig()->acc_notch_cutoff >= accelerometerConfig()->acc_notch_hz) {
accelerometerConfigMutable()->acc_notch_hz = 0;
}

8
src/main/fc/fc_msp.c
View file

@ -1238,8 +1238,8 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU8(dst, gyroConfig()->gyro_main_lpf_hz);
sbufWriteU16(dst, pidProfile()->dterm_lpf_hz);
sbufWriteU16(dst, pidProfile()->yaw_lpf_hz);
sbufWriteU16(dst, gyroConfig()->gyro_notch_hz);
sbufWriteU16(dst, gyroConfig()->gyro_notch_cutoff);
sbufWriteU16(dst, 0); //Was gyroConfig()->gyro_notch_hz
sbufWriteU16(dst, 1); //Was gyroConfig()->gyro_notch_cutoff
sbufWriteU16(dst, 0); //BF: pidProfile()->dterm_notch_hz
sbufWriteU16(dst, 1); //pidProfile()->dterm_notch_cutoff
@ -2176,8 +2176,8 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
pidProfileMutable()->dterm_lpf_hz = constrain(sbufReadU16(src), 0, 500);
pidProfileMutable()->yaw_lpf_hz = constrain(sbufReadU16(src), 0, 255);
if (dataSize >= 9) {
gyroConfigMutable()->gyro_notch_hz = constrain(sbufReadU16(src), 0, 500);
gyroConfigMutable()->gyro_notch_cutoff = constrain(sbufReadU16(src), 1, 500);
sbufReadU16(src); //Was gyroConfigMutable()->gyro_notch_hz
sbufReadU16(src); //Was gyroConfigMutable()->gyro_notch_cutoff
} else {
return MSP_RESULT_ERROR;
}

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

@ -232,15 +232,6 @@ groups:
default_value: 32
field: gyroMovementCalibrationThreshold
max: 128
- name: gyro_notch_hz
field: gyro_notch_hz
max: 500
default_value: 0
- name: gyro_notch_cutoff
field: gyro_notch_cutoff
min: 1
max: 500
default_value: 1
- name: gyro_main_lpf_hz
description: "Software based gyro main lowpass filter. Value is cutoff frequency (Hz)"
default_value: 60

4
src/main/io/osd_dji_hd.c
View file

@ -1428,8 +1428,8 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms
sbufWriteU8(dst, gyroConfig()->gyro_main_lpf_hz); // BF: gyroConfig()->gyro_lowpass_hz
sbufWriteU16(dst, pidProfile()->dterm_lpf_hz); // BF: currentPidProfile->dterm_lowpass_hz
sbufWriteU16(dst, pidProfile()->yaw_lpf_hz); // BF: currentPidProfile->yaw_lowpass_hz
sbufWriteU16(dst, gyroConfig()->gyro_notch_hz); // BF: gyroConfig()->gyro_soft_notch_hz_1
sbufWriteU16(dst, gyroConfig()->gyro_notch_cutoff); // BF: gyroConfig()->gyro_soft_notch_cutoff_1
sbufWriteU16(dst, 0); // BF: gyroConfig()->gyro_soft_notch_hz_1
sbufWriteU16(dst, 1); // BF: gyroConfig()->gyro_soft_notch_cutoff_1
sbufWriteU16(dst, 0); // BF: currentPidProfile->dterm_notch_hz
sbufWriteU16(dst, 1); // BF: currentPidProfile->dterm_notch_cutoff
sbufWriteU16(dst, 0); // BF: gyroConfig()->gyro_soft_notch_hz_2

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

@ -93,9 +93,6 @@ STATIC_FASTRAM filter_t gyroLpfState[XYZ_AXIS_COUNT];
STATIC_FASTRAM filterApplyFnPtr gyroLpf2ApplyFn;
STATIC_FASTRAM filter_t gyroLpf2State[XYZ_AXIS_COUNT];
STATIC_FASTRAM filterApplyFnPtr notchFilter1ApplyFn;
STATIC_FASTRAM void *notchFilter1[XYZ_AXIS_COUNT];
#ifdef USE_DYNAMIC_FILTERS
EXTENDED_FASTRAM gyroAnalyseState_t gyroAnalyseState;
@ -103,7 +100,7 @@ EXTENDED_FASTRAM dynamicGyroNotchState_t dynamicGyroNotchState;
#endif
PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 0);
PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 1);
PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyro_lpf = SETTING_GYRO_HARDWARE_LPF_DEFAULT,
@ -115,8 +112,6 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
#ifdef USE_DUAL_GYRO
.gyro_to_use = SETTING_GYRO_TO_USE_DEFAULT,
#endif
.gyro_notch_hz = SETTING_GYRO_NOTCH_HZ_DEFAULT,
.gyro_notch_cutoff = SETTING_GYRO_NOTCH_CUTOFF_DEFAULT,
.gyro_main_lpf_hz = SETTING_GYRO_MAIN_LPF_HZ_DEFAULT,
.gyro_main_lpf_type = SETTING_GYRO_MAIN_LPF_TYPE_DEFAULT,
.useDynamicLpf = SETTING_GYRO_USE_DYN_LPF_DEFAULT,
@ -282,24 +277,12 @@ static void initGyroFilter(filterApplyFnPtr *applyFn, filter_t state[], uint8_t
static void gyroInitFilters(void)
{
STATIC_FASTRAM biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT];
notchFilter1ApplyFn = nullFilterApply;
//First gyro LPF running at full gyro frequency 8kHz
initGyroFilter(&gyroLpfApplyFn, gyroLpfState, gyroConfig()->gyro_anti_aliasing_lpf_type, gyroConfig()->gyro_anti_aliasing_lpf_hz, getGyroLooptime());
//Second gyro LPF runnig and PID frequency - this filter is dynamic when gyro_use_dyn_lpf = ON
initGyroFilter(&gyroLpf2ApplyFn, gyroLpf2State, gyroConfig()->gyro_main_lpf_type, gyroConfig()->gyro_main_lpf_hz, getLooptime());
//Static Gyro notch running and PID frequency
if (gyroConfig()->gyro_notch_hz) {
notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply;
for (int axis = 0; axis < 3; axis++) {
notchFilter1[axis] = &gyroFilterNotch_1[axis];
biquadFilterInitNotch(notchFilter1[axis], getLooptime(), gyroConfig()->gyro_notch_hz, gyroConfig()->gyro_notch_cutoff);
}
}
#ifdef USE_GYRO_KALMAN
if (gyroConfig()->kalmanEnabled) {
gyroKalmanInitialize(gyroConfig()->kalman_q);
@ -470,7 +453,6 @@ void FAST_CODE NOINLINE gyroFilter()
#endif
gyroADCf = gyroLpf2ApplyFn((filter_t *) &gyroLpf2State[axis], gyroADCf);
gyroADCf = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf);
#ifdef USE_DYNAMIC_FILTERS
if (dynamicGyroNotchState.enabled) {

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

@ -59,8 +59,6 @@ typedef struct gyroConfig_s {
#ifdef USE_DUAL_GYRO
uint8_t gyro_to_use;
#endif
uint16_t gyro_notch_hz;
uint16_t gyro_notch_cutoff;
uint16_t gyro_main_lpf_hz;
uint8_t gyro_main_lpf_type;
uint8_t useDynamicLpf;

2
src/main/target/FALCORE/config.c
View file

@ -61,8 +61,6 @@ void targetConfiguration(void)
gyroConfigMutable()->looptime = 1000;
gyroConfigMutable()->gyro_lpf = 0; // 256 Hz
gyroConfigMutable()->gyro_main_lpf_hz = 90;
gyroConfigMutable()->gyro_notch_hz = 150;
gyroConfigMutable()->gyro_notch_cutoff = 80;
accelerometerConfigMutable()->acc_hardware = ACC_MPU6500;
accelerometerConfigMutable()->acc_lpf_hz = 15;