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

Author: bkleiner

src/flight/filter.c

@@ -90,98 +90,90 @@ float filter_lp_pt3_step(filter_lp_pt3 *filter, filter_state_t *state, float in)
   return state->delay_element[0];
 }
 
-void filter_lp_lulu_init(filter_t *filter, filter_state_t *filter_state, uint8_t count, float hz) {
-  //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.
+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;
-  uint32_t window_half_length = cutoff_wave_length / loop_wave_length;
-  
-  filter->lp_lulu.num_samples = constrain(window_half_length, 1, 12);
-  filter->lp_lulu.window_size = filter->lp_lulu.num_samples * 2 + 1;
-  filter_init_state(filter_state, count);
+  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;
 }
 
-static float fix_road(float *series, float *series_b, int32_t index, int32_t filter_n, int32_t window_size) {
-  float cur_val = 0;
-  float cur_val_b = 0;
-  for (int32_t N = 1; N <= filter_n; N++) {
-    int32_t index_neg = (index + window_size - 2 * N) % window_size;
-    int32_t cur_index = (index_neg + 1) % window_size;
+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];
-    int32_t index_pos = (cur_index + N) % window_size;
-    for (int32_t i = window_size - 2 * N; i < window_size - N; i++) {
-      if (index_pos >= window_size) {
-        index_pos = 0;
-      }
-      if (cur_index >= window_size) {
-        cur_index = 0;
-      }
-
-      cur_val = series[cur_index];
-      cur_val_b = series_b[cur_index];
-      float next_val = series[index_pos];
-      float next_val_b = series_b[index_pos];
 
+    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) {
-        float maxValue = max(prev_val, next_val);
-        series[cur_index] = maxValue;
+        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) {
-        float maxValue = max(prev_val_b, next_val_b);
-        series_b[cur_index] = maxValue;
+        series_b[cur_index] = max(prev_val_b, next_val_b);
       }
-      prev_val = cur_val;
       prev_val_b = cur_val_b;
-      cur_index++;
-      index_pos++;
+
+      cur_index = (cur_index + 1) % window_size;
+      index_pos = (index_pos + 1) % window_size;
     }
 
-    cur_index = (index_neg + 1) % window_size;
     prev_val = series[index_neg];
     prev_val_b = series_b[index_neg];
-    index_pos = (cur_index + N) % window_size;
-    for (int32_t i = window_size - 2 * N; i < window_size - N; i++) {
-      if (index_pos >= window_size) {
-          index_pos = 0;
-      }
-      if (cur_index >= window_size) {
-          cur_index = 0;
-      }
-
-      cur_val = series[cur_index];
-      cur_val_b = series_b[cur_index];
-      float next_val = series[index_pos];
-      float next_val_b = series_b[index_pos];
 
+    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) {
-          float minValue = min(prev_val, next_val);
-          series[cur_index] = minValue;
+        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) {
-          float minValue = min(prev_val_b, next_val_b);
-          series_b[cur_index] = minValue;
+        series_b[cur_index] = min(prev_val_b, next_val_b);
       }
-      prev_val = cur_val;
       prev_val_b = cur_val_b;
-      cur_index++;
-      index_pos++;
+
+      cur_index = (cur_index + 1) % window_size;
+      index_pos = (index_pos + 1) % window_size;
     }
   }
-  int32_t final_index = (index + window_size - filter_n) % window_size;
-  cur_val = series[final_index];
-  cur_val_b = series_b[final_index];
-  return (cur_val - cur_val_b) / 2;
+
+  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_t *filter, filter_state_t *filter_state, float in) {
-  int32_t window_index = filter_state->lulu_state.window_buf_index;
-  filter_state->lulu_state.window_buf_index = (window_index + 1) % filter->lp_lulu.window_size;
-  filter_state->lulu_state.interim[window_index] = in;
-  filter_state->lulu_state.interim_b[window_index] = -in;
-  return fix_road(filter_state->lulu_state.interim, filter_state->lulu_state.interim_b, window_index, filter->lp_lulu.num_samples, filter->lp_lulu.window_size);
+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) {
@@ -303,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;

src/flight/filter.h

@@ -12,18 +12,17 @@ typedef enum {
   FILTER_LP_LULU,
 } __attribute__((__packed__)) filter_type_t;
 
-typedef struct {
+typedef union {
+  struct {
+    float delay_element[3];
+  };
+  struct {
     float interim[32];
     float interim_b[32];
-    int32_t window_buf_index;
-} filter_lulu_state_t;
-
-typedef struct {
-  float delay_element[3];
-  filter_lulu_state_t lulu_state;
+    uint8_t window_buf_index;
+  };
 } filter_state_t;
 
-
 typedef struct {
   float x1;
   float x2;
@@ -52,10 +51,13 @@ typedef struct {
   float alpha;
 } filter_lp_pt3;
 
-//Max N = 15
+// Max N = 15
 typedef struct {
-    int window_size;
-    int num_samples;
+  float hz;
+  uint32_t sample_period_us;
+
+  uint8_t window_size;
+  uint8_t num_samples;
 } filter_lp_lulu;
 
 typedef struct {
@@ -113,8 +115,9 @@ 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_t *filter, filter_state_t *state, uint8_t count, float hz);
-float filter_lp_lulu_step(filter_t *filter, filter_state_t *lulu_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);

src/flight/sixaxis.c

@@ -42,7 +42,6 @@ static float rot_mat[3][3] = {
 
 static filter_t filter[FILTER_MAX_SLOTS];
 static filter_state_t filter_state[FILTER_MAX_SLOTS][3];
-static filter_lulu_state_t lulu_filter_state_gyro[3];
 
 static sdft_t gyro_sdft[SDFT_AXES];
 static filter_biquad_notch_t notch_filter[SDFT_AXES][SDFT_PEAKS];