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

Rename gyro filters

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2021-05-03 18:54:16 +02:00
parent 07075a4457
commit 570ba6d11d
8 changed files with 39 additions and 42 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1723,9 +1723,8 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE("deadband", "%d", rcControlsConfig()->deadband);
BLACKBOX_PRINT_HEADER_LINE("yaw_deadband", "%d", rcControlsConfig()->yaw_deadband);
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf", "%d", gyroConfig()->gyro_lpf);
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf_hz", "%d", gyroConfig()->gyro_soft_lpf_hz);
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf_type", "%d", gyroConfig()->gyro_soft_lpf_type);
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf2_hz", "%d", gyroConfig()->gyro_stage2_lowpass_hz);
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf_hz", "%d", gyroConfig()->gyro_main_lpf_hz);
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf_type", "%d", gyroConfig()->gyro_main_lpf_type);
#ifdef USE_DYNAMIC_FILTERS
BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchRange", "%d", gyroConfig()->dynamicGyroNotchRange);
BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchQ", "%d", gyroConfig()->dynamicGyroNotchQ);
@ -1755,7 +1754,6 @@ static bool blackboxWriteSysinfo(void)
#endif
BLACKBOX_PRINT_HEADER_LINE("acc_notch_hz", "%d", accelerometerConfig()->acc_notch_hz);
BLACKBOX_PRINT_HEADER_LINE("acc_notch_cutoff", "%d", accelerometerConfig()->acc_notch_cutoff);
BLACKBOX_PRINT_HEADER_LINE("gyro_stage2_lowpass_hz", "%d", gyroConfig()->gyro_stage2_lowpass_hz);
BLACKBOX_PRINT_HEADER_LINE("pidSumLimit", "%d", pidProfile()->pidSumLimit);
BLACKBOX_PRINT_HEADER_LINE("pidSumLimitYaw", "%d", pidProfile()->pidSumLimitYaw);
BLACKBOX_PRINT_HEADER_LINE("axisAccelerationLimitYaw", "%d", pidProfile()->axisAccelerationLimitYaw);

3
src/main/cms/cms_menu_imu.c
View file

@ -402,8 +402,7 @@ static const OSD_Entry cmsx_menuFilterPerProfileEntries[] =
{
OSD_LABEL_DATA_ENTRY("-- FILTERING --", profileIndexString),
OSD_SETTING_ENTRY("HARDWARE LPF", SETTING_GYRO_HARDWARE_LPF),
OSD_SETTING_ENTRY("GYRO LPF", SETTING_GYRO_LPF_HZ),
OSD_SETTING_ENTRY("GYRO LPF2", SETTING_GYRO_STAGE2_LOWPASS_HZ),
OSD_SETTING_ENTRY("GYRO MAIN", SETTING_GYRO_MAIN_LPF_HZ),
OSD_SETTING_ENTRY("DTERM LPF", SETTING_DTERM_LPF_HZ),
OSD_SETTING_ENTRY("DTERM LPF2", SETTING_DTERM_LPF2_HZ),
#ifdef USE_DYNAMIC_FILTERS

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

@ -1233,7 +1233,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
break;
case MSP_FILTER_CONFIG :
sbufWriteU8(dst, gyroConfig()->gyro_soft_lpf_hz);
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);
@ -1247,7 +1247,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU16(dst, accelerometerConfig()->acc_notch_hz);
sbufWriteU16(dst, accelerometerConfig()->acc_notch_cutoff);
sbufWriteU16(dst, gyroConfig()->gyro_stage2_lowpass_hz);
sbufWriteU16(dst, 0); //Was gyroConfig()->gyro_stage2_lowpass_hz
break;
case MSP_PID_ADVANCED:
@ -2171,7 +2171,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
case MSP_SET_FILTER_CONFIG :
if (dataSize >= 5) {
gyroConfigMutable()->gyro_soft_lpf_hz = sbufReadU8(src);
gyroConfigMutable()->gyro_main_lpf_hz = sbufReadU8(src);
pidProfileMutable()->dterm_lpf_hz = constrain(sbufReadU16(src), 0, 500);
pidProfileMutable()->yaw_lpf_hz = constrain(sbufReadU16(src), 0, 255);
if (dataSize >= 9) {
@ -2202,7 +2202,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
}
if (dataSize >= 22) {
gyroConfigMutable()->gyro_stage2_lowpass_hz = constrain(sbufReadU16(src), 0, 500);
sbufReadU16(src); //Was gyro_stage2_lowpass_hz
} else {
return MSP_RESULT_ERROR;
}

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

@ -207,15 +207,15 @@ groups:
default_value: "256HZ"
field: gyro_lpf
table: gyro_lpf
- name: gyro_lpf_hz
description: "Software-based filter to remove mechanical vibrations from the gyro signal. Value is cutoff frequency (Hz). For larger frames with bigger props set to lower value."
default_value: 60
field: gyro_soft_lpf_hz
max: 200
- name: gyro_lpf_type
description: "Specifies the type of the software LPF of the gyro signals. BIQUAD gives better filtering and more delay, PT1 less filtering and less delay, so use only on clean builds."
default_value: "BIQUAD"
field: gyro_soft_lpf_type
- name: gyro_anti_aliasing_lpf_hz
description: "Gyro processing anti-aliasing filter cutoff frequency. In normal operation this filter setting should never be changed. In Hz"
default_value: 250
field: gyro_anti_aliasing_lpf_hz
max: 255
- name: gyro_anti_aliasing_lpf_type
description: "Specifies the type of the software LPF of the gyro signals."
default_value: "PT1"
field: gyro_anti_aliasing_lpf_type
table: filter_type
- name: moron_threshold
description: "When powering up, gyro bias is calculated. If the model is shaking/moving during this initial calibration, offsets are calculated incorrectly, and could lead to poor flying performance. This threshold means how much average gyro reading could differ before re-calibration is triggered."
@ -231,16 +231,16 @@ groups:
min: 1
max: 500
default_value: 1
- name: gyro_stage2_lowpass_hz
description: "Software based second stage lowpass filter for gyro. Value is cutoff frequency (Hz)"
default_value: 0
field: gyro_stage2_lowpass_hz
- name: gyro_main_lpf_hz
description: "Software based gyro main lowpass filter. Value is cutoff frequency (Hz)"
default_value: 60
field: gyro_main_lpf_hz
min: 0
max: 500
- name: gyro_stage2_lowpass_type
description: "Defines the type of stage 2 gyro LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation."
- name: gyro_main_lpf_type
description: "Defines the type of the main gyro LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation."
default_value: BIQUAD
field: gyro_stage2_lowpass_type
field: gyro_main_lpf_type
table: filter_type
- name: gyro_use_dyn_lpf
description: "Use Dynamic LPF instead of static gyro stage1 LPF. Dynamic Gyro LPF updates gyro LPF based on the throttle position."

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

@ -1188,7 +1188,7 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms
break;
case DJI_MSP_FILTER_CONFIG:
sbufWriteU8(dst, gyroConfig()->gyro_soft_lpf_hz); // BF: gyroConfig()->gyro_lowpass_hz
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
@ -1200,8 +1200,8 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms
sbufWriteU8(dst, 0); // BF: currentPidProfile->dterm_filter_type
sbufWriteU8(dst, gyroConfig()->gyro_lpf); // BF: gyroConfig()->gyro_hardware_lpf);
sbufWriteU8(dst, 0); // BF: DEPRECATED: gyro_32khz_hardware_lpf
sbufWriteU16(dst, gyroConfig()->gyro_soft_lpf_hz); // BF: gyroConfig()->gyro_lowpass_hz);
sbufWriteU16(dst, gyroConfig()->gyro_stage2_lowpass_hz); // BF: gyroConfig()->gyro_lowpass2_hz);
sbufWriteU16(dst, gyroConfig()->gyro_main_lpf_hz); // BF: gyroConfig()->gyro_lowpass_hz);
sbufWriteU16(dst, 0); // BF: gyroConfig()->gyro_lowpass2_hz);
sbufWriteU8(dst, 0); // BF: gyroConfig()->gyro_lowpass_type);
sbufWriteU8(dst, 0); // BF: gyroConfig()->gyro_lowpass2_type);
sbufWriteU16(dst, 0); // BF: currentPidProfile->dterm_lowpass2_hz);

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

@ -115,8 +115,8 @@ PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 13);
PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyro_lpf = SETTING_GYRO_HARDWARE_LPF_DEFAULT,
.gyro_soft_lpf_hz = SETTING_GYRO_LPF_HZ_DEFAULT,
.gyro_soft_lpf_type = SETTING_GYRO_LPF_TYPE_DEFAULT,
.gyro_anti_aliasing_lpf_hz = SETTING_GYRO_ANTI_ALIASING_LPF_HZ_DEFAULT,
.gyro_anti_aliasing_lpf_type = SETTING_GYRO_ANTI_ALIASING_LPF_TYPE_DEFAULT,
.gyro_align = SETTING_ALIGN_GYRO_DEFAULT,
.gyroMovementCalibrationThreshold = SETTING_MORON_THRESHOLD_DEFAULT,
.looptime = SETTING_LOOPTIME_DEFAULT,
@ -125,8 +125,8 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
#endif
.gyro_notch_hz = SETTING_GYRO_NOTCH_HZ_DEFAULT,
.gyro_notch_cutoff = SETTING_GYRO_NOTCH_CUTOFF_DEFAULT,
.gyro_stage2_lowpass_hz = SETTING_GYRO_STAGE2_LOWPASS_HZ_DEFAULT,
.gyro_stage2_lowpass_type = SETTING_GYRO_STAGE2_LOWPASS_TYPE_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,
.gyroDynamicLpfMinHz = SETTING_GYRO_DYN_LPF_MIN_HZ_DEFAULT,
.gyroDynamicLpfMaxHz = SETTING_GYRO_DYN_LPF_MAX_HZ_DEFAULT,
@ -287,10 +287,10 @@ static void gyroInitFilters(void)
notchFilter1ApplyFn = nullFilterApply;
//First gyro LPF running at full gyro frequency 8kHz
initGyroFilter(&gyroLpfApplyFn, gyroLpfState, gyroConfig()->gyro_soft_lpf_type, gyroConfig()->gyro_soft_lpf_hz, getGyroLooptime());
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_stage2_lowpass_type, gyroConfig()->gyro_stage2_lowpass_hz, getLooptime());
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) {
@ -577,11 +577,11 @@ int16_t gyroRateDps(int axis)
}
void gyroUpdateDynamicLpf(float cutoffFreq) {
if (gyroConfig()->gyro_soft_lpf_type == FILTER_PT1) {
if (gyroConfig()->gyro_main_lpf_type == FILTER_PT1) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
pt1FilterUpdateCutoff(&gyroLpf2State[axis].pt1, cutoffFreq);
}
} else if (gyroConfig()->gyro_soft_lpf_type == FILTER_BIQUAD) {
} else if (gyroConfig()->gyro_main_lpf_type == FILTER_BIQUAD) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterUpdate(&gyroLpf2State[axis].biquad, cutoffFreq, getLooptime(), BIQUAD_Q, FILTER_LPF);
}

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

@ -63,15 +63,15 @@ typedef struct gyroConfig_s {
uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
uint16_t looptime; // imu loop time in us
uint8_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
uint8_t gyro_soft_lpf_hz;
uint8_t gyro_soft_lpf_type;
uint8_t gyro_anti_aliasing_lpf_hz;
uint8_t gyro_anti_aliasing_lpf_type;
#ifdef USE_DUAL_GYRO
uint8_t gyro_to_use;
#endif
uint16_t gyro_notch_hz;
uint16_t gyro_notch_cutoff;
uint16_t gyro_stage2_lowpass_hz;
uint8_t gyro_stage2_lowpass_type;
uint16_t gyro_main_lpf_hz;
uint8_t gyro_main_lpf_type;
uint8_t useDynamicLpf;
uint16_t gyroDynamicLpfMinHz;
uint16_t gyroDynamicLpfMaxHz;

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

@ -60,7 +60,7 @@ void targetConfiguration(void)
gyroConfigMutable()->looptime = 1000;
gyroConfigMutable()->gyro_lpf = 0; // 256 Hz
gyroConfigMutable()->gyro_soft_lpf_hz = 90;
gyroConfigMutable()->gyro_main_lpf_hz = 90;
gyroConfigMutable()->gyro_notch_hz = 150;
gyroConfigMutable()->gyro_notch_cutoff = 80;