diff --git a/src/config/config.h b/src/config/config.h index 16c940a3a..e4b79260f 100644 --- a/src/config/config.h +++ b/src/config/config.h @@ -160,8 +160,7 @@ // at eliminating propwash. Motor noise related to rpm is known to have a quadratic relationship with increasing throttle. While a quadratic curve // could have been selected for this feature, a faster moving parabolic one was selected in its place as the goal is not to follow motor noise, but // to get the filter out of the way as fast as possible in the interest of better performance and handling through reduced D filter latency when you need it most. -#define DTERM_DYNAMIC_LPF -#define DTERM_DYNAMIC_EXPO 1.0f +#define DTERM_DYNAMIC_TYPE FILTER_LP_PT1 #define DTERM_DYNAMIC_FREQ_MIN 70 #define DTERM_DYNAMIC_FREQ_MAX 260 diff --git a/src/core/profile.c b/src/core/profile.c index 4724015b7..4b0d93ac7 100644 --- a/src/core/profile.c +++ b/src/core/profile.c @@ -203,17 +203,10 @@ const profile_t default_profile = { }, }, -#ifdef DTERM_DYNAMIC_LPF - .dterm_dynamic_enable = 1, -#else - .dterm_dynamic_enable = 0, -#endif -#ifdef DTERM_DYNAMIC_FREQ_MIN + .dterm_dynamic_type = DTERM_DYNAMIC_TYPE, .dterm_dynamic_min = DTERM_DYNAMIC_FREQ_MIN, -#endif -#ifdef DTERM_DYNAMIC_FREQ_MAX .dterm_dynamic_max = DTERM_DYNAMIC_FREQ_MAX, -#endif + #ifdef GYRO_DYNAMIC_NOTCH .gyro_dynamic_notch_enable = 1, #else diff --git a/src/core/profile.h b/src/core/profile.h index ea139788e..7ef5bc3f4 100644 --- a/src/core/profile.h +++ b/src/core/profile.h @@ -9,7 +9,7 @@ #define OSD_NUMBER_ELEMENTS 32 -#define PROFILE_VERSION MAKE_SEMVER(0, 2, 5) +#define PROFILE_VERSION MAKE_SEMVER(0, 2, 6) // Rates typedef enum { @@ -327,7 +327,7 @@ typedef struct { typedef struct { profile_filter_parameter_t gyro[FILTER_MAX_SLOTS]; profile_filter_parameter_t dterm[FILTER_MAX_SLOTS]; - uint8_t dterm_dynamic_enable; + filter_type_t dterm_dynamic_type; float dterm_dynamic_min; float dterm_dynamic_max; uint8_t gyro_dynamic_notch_enable; @@ -337,7 +337,7 @@ typedef struct { START_STRUCT(profile_filter_t) \ ARRAY_MEMBER(gyro, FILTER_MAX_SLOTS, profile_filter_parameter_t) \ ARRAY_MEMBER(dterm, FILTER_MAX_SLOTS, profile_filter_parameter_t) \ - MEMBER(dterm_dynamic_enable, uint8_t) \ + MEMBER(dterm_dynamic_type, uint8_t) \ MEMBER(dterm_dynamic_min, float) \ MEMBER(dterm_dynamic_max, float) \ MEMBER(gyro_dynamic_notch_enable, uint8_t) \ diff --git a/src/flight/filter.h b/src/flight/filter.h index 23e28333a..a0a8286ff 100644 --- a/src/flight/filter.h +++ b/src/flight/filter.h @@ -9,6 +9,8 @@ typedef enum { FILTER_LP_PT1, FILTER_LP_PT2, FILTER_LP_PT3, + + FILTER_MAX } __attribute__((__packed__)) filter_type_t; typedef struct { diff --git a/src/flight/pid.c b/src/flight/pid.c index 9c9d1cd8d..a8268a0c9 100644 --- a/src/flight/pid.c +++ b/src/flight/pid.c @@ -42,7 +42,7 @@ static vec3_t last_error2; static filter_t filter[FILTER_MAX_SLOTS]; static filter_state_t filter_state[FILTER_MAX_SLOTS][3]; -static filter_lp_pt1 dynamic_filter; +static filter_t dynamic_filter; static filter_state_t dynamic_filter_state[3]; static filter_lp_pt1 rx_filter; @@ -50,15 +50,10 @@ static filter_state_t rx_filter_state[3]; void pid_init() { filter_lp_pt1_init(&rx_filter, rx_filter_state, 3, state.rx_filter_hz); - for (uint8_t i = 0; i < FILTER_MAX_SLOTS; i++) { filter_init(profile.filter.dterm[i].type, &filter[i], filter_state[i], 3, profile.filter.dterm[i].cutoff_freq); } - - if (profile.filter.dterm_dynamic_enable) { - // zero out filter, freq will be updated later on - filter_lp_pt1_init(&dynamic_filter, dynamic_filter_state, 3, DTERM_DYNAMIC_FREQ_MAX); - } + filter_init(profile.filter.dterm_dynamic_type, &dynamic_filter, dynamic_filter_state, 3, DTERM_DYNAMIC_FREQ_MAX); } // (iwindup = 0 windup is not allowed) (iwindup = 1 windup is allowed) @@ -92,11 +87,7 @@ static inline float pid_compute_iterm_windup(uint8_t x, float pid_output) { static inline float pid_filter_dterm(uint8_t x, float dterm) { dterm = filter_step(profile.filter.dterm[0].type, &filter[0], &filter_state[0][x], dterm); dterm = filter_step(profile.filter.dterm[1].type, &filter[1], &filter_state[1][x], dterm); - - if (profile.filter.dterm_dynamic_enable) { - dterm = filter_lp_pt1_step(&dynamic_filter, &dynamic_filter_state[x], dterm); - } - + dterm = filter_step(profile.filter.dterm_dynamic_type, &dynamic_filter, &dynamic_filter_state[x], dterm); return dterm; } @@ -147,9 +138,15 @@ static inline float pid_tda_compensation() { // output: state.pidoutput.axis[] = change required from motors void pid_calc() { filter_lp_pt1_coeff(&rx_filter, state.rx_filter_hz); + filter_coeff(profile.filter.dterm[0].type, &filter[0], profile.filter.dterm[0].cutoff_freq); filter_coeff(profile.filter.dterm[1].type, &filter[1], profile.filter.dterm[1].cutoff_freq); + const float dynamic_throttle = state.throttle + state.throttle * (1.0f - state.throttle); + const float dterm_dynamic_raw_freq = mapf(dynamic_throttle, 0.0f, 1.0f, profile.filter.dterm_dynamic_min, profile.filter.dterm_dynamic_max); + const float dterm_dynamic_freq = constrain(dterm_dynamic_raw_freq, profile.filter.dterm_dynamic_min, profile.filter.dterm_dynamic_max); + filter_coeff(profile.filter.dterm_dynamic_type, &dynamic_filter, dterm_dynamic_freq); + static vec3_t pid_output = {.roll = 0, .pitch = 0, .yaw = 0}; const float v_compensation = pid_voltage_compensation(); const float tda_compensation = pid_tda_compensation(); @@ -158,14 +155,6 @@ void pid_calc() { const float *stick_accelerator = profile.pid.stick_rates[stick_boost_profile].accelerator.axis; const float *stick_transition = profile.pid.stick_rates[stick_boost_profile].transition.axis; - if (profile.filter.dterm_dynamic_enable) { - float dynamic_throttle = state.throttle + state.throttle * (1 - state.throttle) * DTERM_DYNAMIC_EXPO; - float d_term_dynamic_freq = mapf(dynamic_throttle, 0.0f, 1.0f, profile.filter.dterm_dynamic_min, profile.filter.dterm_dynamic_max); - d_term_dynamic_freq = constrain(d_term_dynamic_freq, profile.filter.dterm_dynamic_min, profile.filter.dterm_dynamic_max); - - filter_lp_pt1_coeff(&dynamic_filter, d_term_dynamic_freq); - } - // rotates errors ierror = vec3_rotate(ierror, state.gyro_delta_angle); diff --git a/src/osd/render.c b/src/osd/render.c index dc6a04bf1..9cf19a17d 100644 --- a/src/osd/render.c +++ b/src/osd/render.c @@ -96,6 +96,13 @@ static const char *on_off_labels[] = { " ON", }; +static const char *filter_type_labels[] = { + "NONE", + " PT1", + " PT2", + " PT3", +}; + #pragma GCC diagnostic ignored "-Wmissing-braces" static const vec3_t rate_defaults[RATE_MODE_ACTUAL + 1][3] = { @@ -815,16 +822,9 @@ void osd_display() { osd_menu_start(); osd_menu_header("GYRO FILTERS"); - const char *filter_type_labels[] = { - "NONE", - " PT1", - " PT2", - " PT3", - }; - osd_menu_select(4, 4, "PASS 1 TYPE"); if (osd_menu_select_enum(18, 4, profile.filter.gyro[0].type, filter_type_labels)) { - profile.filter.gyro[0].type = osd_menu_adjust_int(profile.filter.gyro[0].type, 1, 0, FILTER_LP_PT3); + profile.filter.gyro[0].type = osd_menu_adjust_int(profile.filter.gyro[0].type, 1, 0, FILTER_MAX - 1); osd_state.reboot_fc_requested = 1; } @@ -835,7 +835,7 @@ void osd_display() { osd_menu_select(4, 6, "PASS 2 TYPE"); if (osd_menu_select_enum(18, 6, profile.filter.gyro[1].type, filter_type_labels)) { - profile.filter.gyro[1].type = osd_menu_adjust_int(profile.filter.gyro[1].type, 1, 0, FILTER_LP_PT3); + profile.filter.gyro[1].type = osd_menu_adjust_int(profile.filter.gyro[1].type, 1, 0, FILTER_MAX - 1); osd_state.reboot_fc_requested = 1; } @@ -859,16 +859,9 @@ void osd_display() { osd_menu_start(); osd_menu_header("D-TERM FILTERS"); - const char *filter_type_labels[] = { - "NONE", - " PT1", - " PT2", - " PT3", - }; - osd_menu_select(4, 3, "PASS 1 TYPE"); if (osd_menu_select_enum(18, 3, profile.filter.dterm[0].type, filter_type_labels)) { - profile.filter.dterm[0].type = osd_menu_adjust_int(profile.filter.dterm[0].type, 1, 0, FILTER_LP_PT2); + profile.filter.dterm[0].type = osd_menu_adjust_int(profile.filter.dterm[0].type, 1, 0, FILTER_MAX - 1); osd_state.reboot_fc_requested = 1; } @@ -879,7 +872,7 @@ void osd_display() { osd_menu_select(4, 5, "PASS 2 TYPE"); if (osd_menu_select_enum(18, 5, profile.filter.dterm[1].type, filter_type_labels)) { - profile.filter.dterm[1].type = osd_menu_adjust_int(profile.filter.dterm[1].type, 1, 0, FILTER_LP_PT2); + profile.filter.dterm[1].type = osd_menu_adjust_int(profile.filter.dterm[1].type, 1, 0, FILTER_MAX - 1); osd_state.reboot_fc_requested = 1; } @@ -888,9 +881,9 @@ void osd_display() { profile.filter.dterm[1].cutoff_freq = osd_menu_adjust_float(profile.filter.dterm[1].cutoff_freq, 10, 50, 500); } - osd_menu_select(4, 7, "DYNAMIC"); - if (osd_menu_select_enum(18, 7, profile.filter.dterm_dynamic_enable, on_off_labels)) { - profile.filter.dterm_dynamic_enable = osd_menu_adjust_int(profile.filter.dterm_dynamic_enable, 1, 0, 1); + osd_menu_select(4, 7, "DYNAMIC TYPE"); + if (osd_menu_select_enum(18, 7, profile.filter.dterm_dynamic_type, filter_type_labels)) { + profile.filter.dterm_dynamic_type = osd_menu_adjust_int(profile.filter.dterm_dynamic_type, 1, 0, FILTER_MAX - 1); osd_state.reboot_fc_requested = 1; }