make dynamic dterm filter type configurable

Author: bkleiner

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
 

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

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)                        \

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 {

src/flight/pid.c

@@ -42,23 +42,18 @@ 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;
 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);
 

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;
     }