Added LULU filtering as an option

Fixed not initializing window size

Update LICENSE

Restored licence after thinking about it

Update src/flight/filter.c

Thanks for the suggestion!

Co-authored-by: Hugo Chiang <[email protected]>

Update src/flight/filter.c

Co-authored-by: Hugo Chiang <[email protected]>

Implemented changes as requested by Hanfer

lulu: use union, add coeff function, optimize for constant substitution

Author: pjpei

src/flight/filter.c

@@ -90,6 +90,92 @@ float filter_lp_pt3_step(filter_lp_pt3 *filter, filter_state_t *state, float in)
   return state->delay_element[0];
 }
 
+void filter_lp_lulu_coeff(filter_lp_lulu *filter, float hz) {
+  if (filter->hz == hz && filter->sample_period_us == state.looptime_autodetect) {
+    return;
+  }
+  filter->hz = hz;
+  filter->sample_period_us = state.looptime_autodetect;
+
+  // The window value is half the wavelength of the wave that it filters.  So if the wavelength of the cutoff frequency is 2 samples, the N value should be 1.  If the wavelength is 4, N should be 2.  Etc.
+  float cutoff_wave_length = 1.0f / hz / 4.0f;
+  float loop_wave_length = state.looptime_autodetect * 1e-6f;
+  uint8_t window_half_length = cutoff_wave_length / loop_wave_length;
+
+  filter->num_samples = constrain(window_half_length, 1, 12);
+  filter->window_size = filter->num_samples * 2 + 1;
+}
+
+void filter_lp_lulu_init(filter_lp_lulu *filter, filter_state_t *state, uint8_t count, float hz) {
+  filter_lp_lulu_coeff(filter, hz);
+  filter_init_state(state, count);
+}
+
+static float fix_road(float *series, float *series_b, uint8_t index, uint8_t filter_n, uint8_t window_size) {
+  for (uint32_t N = 1; N <= filter_n; N++) {
+    const uint32_t index_neg = (index + window_size - 2 * N) % window_size;
+
+    float prev_val = series[index_neg];
+    float prev_val_b = series_b[index_neg];
+
+    uint32_t cur_index = (index_neg + 1) % window_size;
+    uint32_t index_pos = (cur_index + N) % window_size;
+    for (uint32_t i = window_size - 2 * N; i < window_size - N; i++) {
+      const float cur_val = series[cur_index];
+      const float next_val = series[index_pos];
+      if (prev_val < cur_val && cur_val > next_val) {
+        series[cur_index] = max(prev_val, next_val);
+      }
+      prev_val = cur_val;
+
+      const float cur_val_b = series_b[cur_index];
+      const float next_val_b = series_b[index_pos];
+      if (prev_val_b < cur_val_b && cur_val_b > next_val_b) {
+        series_b[cur_index] = max(prev_val_b, next_val_b);
+      }
+      prev_val_b = cur_val_b;
+
+      cur_index = (cur_index + 1) % window_size;
+      index_pos = (index_pos + 1) % window_size;
+    }
+
+    prev_val = series[index_neg];
+    prev_val_b = series_b[index_neg];
+
+    cur_index = (index_neg + 1) % window_size;
+    index_pos = (cur_index + N) % window_size;
+    for (uint32_t i = window_size - 2 * N; i < window_size - N; i++) {
+      const float cur_val = series[cur_index];
+      const float next_val = series[index_pos];
+      if (prev_val > cur_val && cur_val < next_val) {
+        series[cur_index] = min(prev_val, next_val);
+      }
+      prev_val = cur_val;
+
+      const float cur_val_b = series_b[cur_index];
+      const float next_val_b = series_b[index_pos];
+      if (prev_val_b > cur_val_b && cur_val_b < next_val_b) {
+        series_b[cur_index] = min(prev_val_b, next_val_b);
+      }
+      prev_val_b = cur_val_b;
+
+      cur_index = (cur_index + 1) % window_size;
+      index_pos = (index_pos + 1) % window_size;
+    }
+  }
+
+  const uint8_t final_index = (index + window_size - filter_n) % window_size;
+  return (series[final_index] - series_b[final_index]) / 2;
+}
+
+float filter_lp_lulu_step(filter_lp_lulu *filter, filter_state_t *state, float in) {
+  const uint8_t window_index = state->window_buf_index;
+  state->window_buf_index = (window_index + 1) % filter->window_size;
+  state->interim[window_index] = in;
+  state->interim_b[window_index] = -in;
+  return fix_road(state->interim, state->interim_b, window_index, filter->num_samples, filter->window_size);
+}
+
 void filter_biquad_notch_init(filter_biquad_notch_t *filter, filter_biquad_state_t *state, uint8_t count, float hz) {
   memset(filter, 0, sizeof(filter_biquad_notch_t));
   filter_biquad_notch_coeff(filter, hz);
@@ -190,6 +276,8 @@ void filter_init(filter_type_t type, filter_t *filter, filter_state_t *state, ui
   case FILTER_LP_PT3:
     filter_lp_pt3_init(&filter->lp_pt3, state, count, hz);
     break;
+  case FILTER_LP_LULU:
+    filter_lp_lulu_init(&filter->lp_lulu, state, count, hz);
   default:
     // no filter, do nothing
     break;
@@ -207,6 +295,9 @@ void filter_coeff(filter_type_t type, filter_t *filter, float hz) {
   case FILTER_LP_PT3:
     filter_lp_pt3_coeff(&filter->lp_pt3, hz);
     break;
+  case FILTER_LP_LULU:
+    filter_lp_lulu_coeff(&filter->lp_lulu, hz);
+    break;
   default:
     // no filter, do nothing
     break;
@@ -221,6 +312,8 @@ float filter_step(filter_type_t type, filter_t *filter, filter_state_t *state, f
     return filter_lp_pt2_step(&filter->lp_pt2, state, in);
   case FILTER_LP_PT3:
     return filter_lp_pt3_step(&filter->lp_pt3, state, in);
+  case FILTER_LP_LULU:
+    return filter_lp_lulu_step(&filter->lp_lulu, state, in);
   default:
     // no filter at all
     return in;

src/flight/filter.h

@@ -9,12 +9,20 @@ typedef enum {
   FILTER_LP_PT1,
   FILTER_LP_PT2,
   FILTER_LP_PT3,
+  FILTER_LP_LULU,
 
   FILTER_MAX
 } __attribute__((__packed__)) filter_type_t;
 
-typedef struct {
-  float delay_element[3];
+typedef union {
+  struct {
+    float delay_element[3];
+  };
+  struct {
+    float interim[32];
+    float interim_b[32];
+    uint8_t window_buf_index;
+  };
 } filter_state_t;
 
 typedef struct {
@@ -45,6 +53,15 @@ typedef struct {
   float alpha;
 } filter_lp_pt3;
 
+// Max N = 15
+typedef struct {
+  float hz;
+  uint32_t sample_period_us;
+
+  uint8_t window_size;
+  uint8_t num_samples;
+} filter_lp_lulu;
+
 typedef struct {
   float hz;
   uint32_t sample_period_us;
@@ -60,6 +77,7 @@ typedef union {
   filter_lp_pt1 lp_pt1;
   filter_lp_pt2 lp_pt2;
   filter_lp_pt3 lp_pt3;
+  filter_lp_lulu lp_lulu;
 } filter_t;
 
 typedef struct {
@@ -99,6 +117,10 @@ void filter_lp_pt3_init(filter_lp_pt3 *filter, filter_state_t *state, uint8_t co
 void filter_lp_pt3_coeff(filter_lp_pt3 *filter, float hz);
 float filter_lp_pt3_step(filter_lp_pt3 *filter, filter_state_t *state, float in);
 
+void filter_lp_lulu_init(filter_lp_lulu *filter, filter_state_t *state, uint8_t count, float hz);
+void filter_lp_lulu_coeff(filter_lp_lulu *filter, float hz);
+float filter_lp_lulu_step(filter_lp_lulu *filter, filter_state_t *state, float in);
+
 void filter_biquad_notch_init(filter_biquad_notch_t *filter, filter_biquad_state_t *state, uint8_t count, float hz);
 void filter_biquad_notch_coeff(filter_biquad_notch_t *filter, float hz);
 float filter_biquad_notch_step(filter_biquad_notch_t *filter, filter_biquad_state_t *state, float in);

src/osd/render.c

@@ -101,6 +101,7 @@ static const char *filter_type_labels[] = {
     " PT1",
     " PT2",
     " PT3",
+    " LULU",
 };
 
 #pragma GCC diagnostic ignored "-Wmissing-braces"