Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix/add USE_GYRO_LPF2 gating #935

Merged
merged 5 commits into from
Oct 26, 2023
Merged

fix/add USE_GYRO_LPF2 gating #935

Show file tree
Hide file tree
Changes from 4 commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
ce02255
add Gyro LPF2 type to OSD
nerdCopter Oct 25, 2023
974e2f1
fix/add USE_GYRO_LPF2 gating
nerdCopter Oct 25, 2023
9be0498
Merge remote-tracking branch 'emuflight/master' into 20231025_add_Gyr…
nerdCopter Oct 25, 2023
c36a851
Merge branch '20231025_add_Gyro_LPF2_type_to_OSD' into 20231025_fix_g…
nerdCopter Oct 25, 2023
2d2f647
Merge remote-tracking branch 'emuflight/master' into 20231025_fix_gat…
nerdCopter Oct 26, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions src/main/blackbox/blackbox.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1229,10 +1229,12 @@ static bool blackboxWriteSysinfo(void) {
BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz_roll", "%d", gyroConfig()->gyro_lowpass_hz[ROLL]);
BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz_pitch", "%d", gyroConfig()->gyro_lowpass_hz[PITCH]);
BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz_yaw", "%d", gyroConfig()->gyro_lowpass_hz[YAW]);
#ifdef USE_GYRO_LPF2
BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass2_type", "%d", gyroConfig()->gyro_lowpass2_type);
BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass2_hz_roll", "%d", gyroConfig()->gyro_lowpass2_hz[ROLL]);
BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass2_hz_pitch", "%d", gyroConfig()->gyro_lowpass2_hz[PITCH]);
BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass2_hz_yaw", "%d", gyroConfig()->gyro_lowpass2_hz[YAW]);
#endif
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz", "%d,%d", gyroConfig()->gyro_soft_notch_hz_1,
gyroConfig()->gyro_soft_notch_hz_2);
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff", "%d,%d", gyroConfig()->gyro_soft_notch_cutoff_1,
Expand Down
12 changes: 11 additions & 1 deletion src/main/cms/cms_menu_imu.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -493,9 +493,12 @@ static uint8_t gyroConfig_gyro_lowpass1_type;
static uint16_t gyroConfig_gyro_lowpass_hz_roll;
static uint16_t gyroConfig_gyro_lowpass_hz_pitch;
static uint16_t gyroConfig_gyro_lowpass_hz_yaw;
#ifdef USE_GYRO_LPF2
static uint8_t gyroConfig_gyro_lowpass2_type;
static uint16_t gyroConfig_gyro_lowpass2_hz_roll;
static uint16_t gyroConfig_gyro_lowpass2_hz_pitch;
static uint16_t gyroConfig_gyro_lowpass2_hz_yaw;
#endif
#ifdef USE_GYRO_DATA_ANALYSE
static uint16_t gyroConfig_gyro_q;
static uint8_t gyroConfig_gyro_notch_count;
Expand Down Expand Up @@ -524,9 +527,12 @@ static long cmsx_menuGyro_onEnter(void) {
gyroConfig_gyro_lowpass_hz_roll = gyroConfig()->gyro_lowpass_hz[ROLL];
gyroConfig_gyro_lowpass_hz_pitch = gyroConfig()->gyro_lowpass_hz[PITCH];
gyroConfig_gyro_lowpass_hz_yaw = gyroConfig()->gyro_lowpass_hz[YAW];
#ifdef USE_GYRO_LPF2
gyroConfig_gyro_lowpass2_type = gyroConfig()->gyro_lowpass2_type;
gyroConfig_gyro_lowpass2_hz_roll = gyroConfig()->gyro_lowpass2_hz[ROLL];
gyroConfig_gyro_lowpass2_hz_pitch = gyroConfig()->gyro_lowpass2_hz[PITCH];
gyroConfig_gyro_lowpass2_hz_yaw = gyroConfig()->gyro_lowpass2_hz[YAW];
#endif
#ifdef USE_GYRO_DATA_ANALYSE
gyroConfig_gyro_q = gyroConfig()->dyn_notch_q;
gyroConfig_gyro_notch_count = gyroConfig()->dyn_notch_count;
Expand Down Expand Up @@ -558,9 +564,12 @@ static long cmsx_menuGyro_onExit(const OSD_Entry *self) {
gyroConfigMutable()->gyro_lowpass_hz[ROLL] = gyroConfig_gyro_lowpass_hz_roll;
gyroConfigMutable()->gyro_lowpass_hz[PITCH] = gyroConfig_gyro_lowpass_hz_pitch;
gyroConfigMutable()->gyro_lowpass_hz[YAW] = gyroConfig_gyro_lowpass_hz_yaw;
#ifdef USE_GYRO_LPF2
gyroConfigMutable()->gyro_lowpass2_type = gyroConfig_gyro_lowpass2_type;
gyroConfigMutable()->gyro_lowpass2_hz[ROLL] = gyroConfig_gyro_lowpass2_hz_roll;
gyroConfigMutable()->gyro_lowpass2_hz[PITCH] = gyroConfig_gyro_lowpass2_hz_pitch;
gyroConfigMutable()->gyro_lowpass2_hz[YAW] = gyroConfig_gyro_lowpass2_hz_yaw;
#endif
#ifdef USE_GYRO_DATA_ANALYSE
gyroConfigMutable()->dyn_notch_q = gyroConfig_gyro_q;
gyroConfigMutable()->dyn_notch_count = gyroConfig_gyro_notch_count;
Expand Down Expand Up @@ -589,11 +598,12 @@ static long cmsx_menuGyro_onExit(const OSD_Entry *self) {
static OSD_Entry cmsx_menuFilterGlobalEntries[] = {
{ "-- FILTER GLB --", OME_Label, NULL, NULL, 0 },

{ "GYRO LPF TYPE", OME_TAB, NULL, &(OSD_TAB_t) { (uint8_t *) &gyroConfig_gyro_lowpass1_type, 4, cms_FilterType }, 0 },
{ "GYRO LPF ROLL", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_lowpass_hz_roll, 0, 16000, 1 }, 0 },
{ "GYRO LPF PITCH", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_lowpass_hz_pitch, 0, 16000, 1 }, 0 },
{ "GYRO LPF YAW", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_lowpass_hz_yaw, 0, 16000, 1 }, 0 },
{ "GYRO LPF TYPE", OME_TAB, NULL, &(OSD_TAB_t) { (uint8_t *) &gyroConfig_gyro_lowpass1_type, 4, cms_FilterType }, 0 },
#ifdef USE_GYRO_LPF2
{ "GYRO LPF2 TYPE", OME_TAB, NULL, &(OSD_TAB_t) { (uint8_t *) &gyroConfig_gyro_lowpass2_type, 4, cms_FilterType }, 0 },
{ "GYRO LPF2 ROLL", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_lowpass2_hz_roll, 0, 16000, 1 }, 0 },
{ "GYRO LPF2 PITCH", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_lowpass2_hz_pitch, 0, 16000, 1 }, 0 },
{ "GYRO LPF2 YAW", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_lowpass2_hz_yaw, 0, 16000, 1 }, 0 },
Expand Down
20 changes: 20 additions & 0 deletions src/main/interface/msp.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1194,11 +1194,21 @@ bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) {
sbufWriteU16(dst, gyroConfig()->gyro_lowpass_hz[ROLL]);
sbufWriteU16(dst, gyroConfig()->gyro_lowpass_hz[PITCH]);
sbufWriteU16(dst, gyroConfig()->gyro_lowpass_hz[YAW]);
#ifdef USE_GYRO_LPF2
sbufWriteU16(dst, gyroConfig()->gyro_lowpass2_hz[ROLL]);
sbufWriteU16(dst, gyroConfig()->gyro_lowpass2_hz[PITCH]);
sbufWriteU16(dst, gyroConfig()->gyro_lowpass2_hz[YAW]);
#else
sbufWriteU16(dst, 0);
sbufWriteU16(dst, 0);
sbufWriteU16(dst, 0);
#endif
sbufWriteU8(dst, gyroConfig()->gyro_lowpass_type);
#ifdef USE_GYRO_LPF2
sbufWriteU8(dst, gyroConfig()->gyro_lowpass2_type);
#else
sbufWriteU8(dst, 0);
#endif
sbufWriteU16(dst, currentPidProfile->dFilter[ROLL].dLpf2);
sbufWriteU16(dst, currentPidProfile->dFilter[PITCH].dLpf2);
sbufWriteU16(dst, currentPidProfile->dFilter[YAW].dLpf2);
Expand Down Expand Up @@ -1823,11 +1833,21 @@ mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) {
gyroConfigMutable()->gyro_lowpass_hz[ROLL] = sbufReadU16(src);
gyroConfigMutable()->gyro_lowpass_hz[PITCH] = sbufReadU16(src);
gyroConfigMutable()->gyro_lowpass_hz[YAW] = sbufReadU16(src);
#ifdef USE_GYRO_LPF2
gyroConfigMutable()->gyro_lowpass2_hz[ROLL] = sbufReadU16(src);
gyroConfigMutable()->gyro_lowpass2_hz[PITCH] = sbufReadU16(src);
gyroConfigMutable()->gyro_lowpass2_hz[YAW] = sbufReadU16(src);
#else
sbufReadU16(src);
sbufReadU16(src);
sbufReadU16(src);
#endif
gyroConfigMutable()->gyro_lowpass_type = sbufReadU8(src);
#ifdef USE_GYRO_LPF2
gyroConfigMutable()->gyro_lowpass2_type = sbufReadU8(src);
#else
sbufReadU8(src);
#endif
currentPidProfile->dFilter[ROLL].dLpf2 = sbufReadU16(src);
currentPidProfile->dFilter[PITCH].dLpf2 = sbufReadU16(src);
currentPidProfile->dFilter[YAW].dLpf2 = sbufReadU16(src);
Expand Down
2 changes: 2 additions & 0 deletions src/main/interface/settings.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -513,10 +513,12 @@ const clivalue_t valueTable[] = {
{ "gyro_lowpass_hz_pitch", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass_hz[PITCH]) },
{ "gyro_lowpass_hz_yaw", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass_hz[YAW]) },

#ifdef USE_GYRO_LPF2
{ "gyro_lowpass2_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_FILTER_TYPE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass2_type) },
{ "gyro_lowpass2_hz_roll", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass2_hz[ROLL]) },
{ "gyro_lowpass2_hz_pitch", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass2_hz[PITCH]) },
{ "gyro_lowpass2_hz_yaw", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass2_hz[YAW]) },
#endif

{ "gyro_abg_alpha", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_ABG_alpha) },
{ "gyro_abg_boost", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 2000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_ABG_boost) },
Expand Down
10 changes: 10 additions & 0 deletions src/main/sensors/gyro.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -158,8 +158,10 @@ typedef struct gyroSensor_s {
gyroLowpassFilter_t lowpassFilter[XYZ_AXIS_COUNT];

// lowpass2 gyro soft filter
#ifdef USE_GYRO_LPF2
filterApplyFnPtr lowpass2FilterApplyFn;
gyroLowpassFilter_t lowpass2Filter[XYZ_AXIS_COUNT];
#endif

// ABG filter
filterApplyFnPtr gyroABGFilterApplyFn;
Expand Down Expand Up @@ -236,10 +238,12 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyro_lowpass_hz[ROLL] = 0,
.gyro_lowpass_hz[PITCH] = 0,
.gyro_lowpass_hz[YAW] = 0,
#ifdef USE_GYRO_LPF2
.gyro_lowpass2_type = FILTER_PT1,
.gyro_lowpass2_hz[ROLL] = 0,
.gyro_lowpass2_hz[PITCH] = 0,
.gyro_lowpass2_hz[YAW] = 0,
#endif
.gyro_high_fsr = false,
.gyro_use_32khz = true,
.gyro_to_use = GYRO_CONFIG_USE_GYRO_DEFAULT,
Expand Down Expand Up @@ -289,10 +293,12 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyro_lowpass_hz[ROLL] = 115,
.gyro_lowpass_hz[PITCH] = 115,
.gyro_lowpass_hz[YAW] = 105,
#ifdef USE_GYRO_LPF2
.gyro_lowpass2_type = FILTER_PT1,
.gyro_lowpass2_hz[ROLL] = 0,
.gyro_lowpass2_hz[PITCH] = 0,
.gyro_lowpass2_hz[YAW] = 0,
#endif
.gyro_high_fsr = false,
.gyro_use_32khz = false,
.gyro_to_use = GYRO_CONFIG_USE_GYRO_DEFAULT,
Expand Down Expand Up @@ -749,13 +755,15 @@ void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type) {
lpfHz[PITCH] = gyroConfig()->gyro_lowpass_hz[PITCH];
lpfHz[YAW] = gyroConfig()->gyro_lowpass_hz[YAW];
break;
#ifdef USE_GYRO_LPF2
case FILTER_LOWPASS2:
lowpassFilterApplyFn = &gyroSensor->lowpass2FilterApplyFn;
lowpassFilter = gyroSensor->lowpass2Filter;
lpfHz[ROLL] = gyroConfig()->gyro_lowpass2_hz[ROLL];
lpfHz[PITCH] = gyroConfig()->gyro_lowpass2_hz[PITCH];
lpfHz[YAW] = gyroConfig()->gyro_lowpass2_hz[YAW];
break;
#endif
default:
return;
}
Expand Down Expand Up @@ -894,11 +902,13 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor) {
FILTER_LOWPASS,
gyroConfig()->gyro_lowpass_type
);
#ifdef USE_GYRO_LPF2
gyroInitLowpassFilterLpf(
gyroSensor,
FILTER_LOWPASS2,
gyroConfig()->gyro_lowpass2_type
);
#endif
gyroInitFilterNotch1(gyroSensor, gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1);
gyroInitFilterNotch2(gyroSensor, gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2);
#ifdef USE_GYRO_DATA_ANALYSE
Expand Down
4 changes: 4 additions & 0 deletions src/main/sensors/gyro.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -132,7 +132,9 @@ typedef struct gyroConfig_s {
uint8_t gyro_to_use;

uint16_t gyro_lowpass_hz[XYZ_AXIS_COUNT];
#ifdef USE_GYRO_LPF2
uint16_t gyro_lowpass2_hz[XYZ_AXIS_COUNT];
#endif

uint16_t gyro_ABG_alpha;
uint16_t gyro_ABG_boost;
Expand All @@ -147,7 +149,9 @@ typedef struct gyroConfig_s {

// Lowpass primary/secondary
uint8_t gyro_lowpass_type;
#ifdef USE_GYRO_LPF2
uint8_t gyro_lowpass2_type;
#endif

uint8_t yaw_spin_recovery;
int16_t yaw_spin_threshold;
Expand Down
2 changes: 2 additions & 0 deletions src/main/sensors/gyro_filter_impl.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,9 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor) {
#endif

// apply static notch filters and software lowpass filters
#ifdef USE_GYRO_LPF2
gyroADCf = gyroSensor->lowpass2FilterApplyFn((filter_t *)&gyroSensor->lowpass2Filter[axis], gyroADCf);
#endif
gyroADCf = gyroSensor->lowpassFilterApplyFn((filter_t *)&gyroSensor->lowpassFilter[axis], gyroADCf);
gyroADCf = gyroSensor->notchFilter1ApplyFn((filter_t *)&gyroSensor->notchFilter1[axis], gyroADCf);
gyroADCf = gyroSensor->notchFilter2ApplyFn((filter_t *)&gyroSensor->notchFilter2[axis], gyroADCf);
Expand Down
2 changes: 2 additions & 0 deletions src/main/target/NBDBBBLV2/config.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -168,9 +168,11 @@ void targetConfiguration(void)
gyroConfigMutable()->gyro_lowpass_hz[ROLL] = 200;
gyroConfigMutable()->gyro_lowpass_hz[PITCH] = 200;
gyroConfigMutable()->gyro_lowpass_hz[YAW] = 200;
#ifdef USE_GYRO_LPF2
gyroConfigMutable()->gyro_lowpass2_hz[ROLL] = 250;
gyroConfigMutable()->gyro_lowpass2_hz[PITCH] = 250;
gyroConfigMutable()->gyro_lowpass2_hz[YAW] = 250;
#endif
gyroConfigMutable()->yaw_spin_threshold = 1950;
rxConfigMutable()->mincheck = 1075;
rxConfigMutable()->maxcheck = 1900;
Expand Down
2 changes: 2 additions & 0 deletions src/main/target/NBDBBBLV3/config.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -168,9 +168,11 @@ void targetConfiguration(void)
gyroConfigMutable()->gyro_lowpass_hz[ROLL] = 200;
gyroConfigMutable()->gyro_lowpass_hz[PITCH] = 200;
gyroConfigMutable()->gyro_lowpass_hz[YAW] = 200;
#ifdef USE_GYRO_LPF2
gyroConfigMutable()->gyro_lowpass2_hz[ROLL] = 250;
gyroConfigMutable()->gyro_lowpass2_hz[PITCH] = 250;
gyroConfigMutable()->gyro_lowpass2_hz[YAW] = 250;
#endif
gyroConfigMutable()->yaw_spin_threshold = 1950;
rxConfigMutable()->mincheck = 1075;
rxConfigMutable()->maxcheck = 1900;
Expand Down
2 changes: 2 additions & 0 deletions src/main/target/NBDHMBF41S/config.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -114,7 +114,9 @@ void targetConfiguration(void)

gyroConfigMutable()->gyro_lowpass_type = FILTER_PT1;
gyroConfigMutable()->gyro_lowpass_hz = 200;
#ifdef USE_GYRO_LPF2
gyroConfigMutable()->gyro_lowpass2_hz = 200;
#endif
gyroConfigMutable()->yaw_spin_threshold = 1950;
gyroConfigMutable()->dyn_lpf_gyro_min_hz = 160;
gyroConfigMutable()->dyn_lpf_gyro_max_hz = 400;
Expand Down
2 changes: 2 additions & 0 deletions src/main/target/NBDHMBF4PRO/config.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -149,9 +149,11 @@ void targetConfiguration(void) {
gyroConfigMutable()->gyro_lowpass_hz[ROLL] = 200;
gyroConfigMutable()->gyro_lowpass_hz[PITCH] = 200;
gyroConfigMutable()->gyro_lowpass_hz[YAW] = 200;
#ifdef USE_GYRO_LPF2
gyroConfigMutable()->gyro_lowpass2_hz[ROLL] = 250;
gyroConfigMutable()->gyro_lowpass2_hz[PITCH] = 250;
gyroConfigMutable()->gyro_lowpass2_hz[YAW] = 250;
#endif
gyroConfigMutable()->yaw_spin_threshold = 1950;
// gyroConfigMutable()->dyn_lpf_gyro_min_hz = 200;
// gyroConfigMutable()->dyn_lpf_gyro_max_hz = 500;
Expand Down
2 changes: 2 additions & 0 deletions src/test/unit/sensor_gyro_unittest.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -116,7 +116,9 @@ TEST(SensorGyro, Update)
pgResetAll();
// turn off filters
gyroConfigMutable()->gyro_lowpass_hz[ROLL] = 0;
#ifdef USE_GYRO_LPF2
gyroConfigMutable()->gyro_lowpass2_hz[ROLL] = 0;
#endif
gyroConfigMutable()->gyro_soft_notch_hz_1 = 0;
gyroConfigMutable()->gyro_soft_notch_hz_2 = 0;
gyroInit();
Expand Down
Loading