move lpf calculations to macro

src/flight/filter.c

@@ -14,30 +14,6 @@
 #define ORDER2_CORRECTION 1.55377397403f
 #define ORDER3_CORRECTION 1.9614591767f
 
-// calculates the coefficient for lpf filter, times in the same units
-float lpfcalc(float sampleperiod, float filtertime) {
-  float ga = 1.0f - sampleperiod / filtertime;
-  if (ga > 1.0f)
-    ga = 1.0f;
-  if (ga < 0.0f)
-    ga = 0.0f;
-  return ga;
-}
-
-// calculates the coefficient for lpf filter
-float lpfcalc_hz(float sampleperiod, float filterhz) {
-  float ga = 1.0f - sampleperiod * filterhz;
-  if (ga > 1.0f)
-    ga = 1.0f;
-  if (ga < 0.0f)
-    ga = 0.0f;
-  return ga;
-}
-
-void lpf(float *out, float in, float coeff) {
-  *out = (*out) * coeff + in * (1 - coeff);
-}
-
 static void filter_init_state(filter_state_t *state, uint8_t count) {
   memset(state, 0, count * sizeof(filter_state_t));
 }

src/flight/filter.h

@@ -68,10 +68,20 @@ typedef struct {
   float v[2];
 } filter_hp_be;
 
-float lpfcalc(float sampleperiod, float filtertime);
-float lpfcalc_hz(float sampleperiod, float filterhz);
-
-void lpf(float *out, float in, float coeff);
+//(1 - alpha. filtertime = 1 / filter-cutoff-frequency) as long as filtertime > sampleperiod
+#define lpfcalc(sampleperiod, filtertime) ({                                                   \
+  const float _sampleperiod = sampleperiod;                                                    \
+  const float _filtertime = filtertime;                                                        \
+  constrain(1.0f - (6.0f * _sampleperiod) / (3.0f * _sampleperiod + _filtertime), 0.0f, 1.0f); \
+})
+#define lpfcalc_hz(sampleperiod, filter_hz) lpfcalc(sampleperiod, 1.0f / filter_hz)
+
+#define lpf(out, in, coeff)                                  \
+  ({                                                         \
+    typeof(coeff) _coeff = (coeff);                          \
+    typeof(in) _in = (in);                                   \
+    (*(out) = (*(out)) * (_coeff) + (_in) * (1 - (_coeff))); \
+  })
 
 void filter_global_init();
 

src/flight/pid.c

@@ -81,13 +81,13 @@ static inline float pid_compute_iterm_windup(uint8_t x, float pid_output) {
 #ifdef ITERM_RELAX //  Roll - Pitch  Setpoint based I term relax method
   static float avg_setpoint[3] = {0, 0, 0};
   if (x < 2) {
-    lpf(&avg_setpoint[x], state.setpoint.axis[x], FILTERCALC(state.looptime, 1.0f / (float)RELAX_FREQUENCY_HZ)); // 11 Hz filter
+    lpf(&avg_setpoint[x], state.setpoint.axis[x], lpfcalc(state.looptime, 1.0f / (float)RELAX_FREQUENCY_HZ)); // 11 Hz filter
     const float hpfSetpoint = fabsf(state.setpoint.axis[x] - avg_setpoint[x]);
     return max(1.0f - hpfSetpoint / RELAX_FACTOR, 0.0f);
   }
 #ifdef ITERM_RELAX_YAW
-  else {                                                                                                             // axis is yaw
-    lpf(&avg_setpoint[x], state.setpoint.axis[x], FILTERCALC(state.looptime, 1.0f / (float)RELAX_FREQUENCY_HZ_YAW)); // 25 Hz filter
+  else {                                                                                                          // axis is yaw
+    lpf(&avg_setpoint[x], state.setpoint.axis[x], lpfcalc(state.looptime, 1.0f / (float)RELAX_FREQUENCY_HZ_YAW)); // 25 Hz filter
     const float hpfSetpoint = fabsf(state.setpoint.axis[x] - avg_setpoint[x]);
     return max(1.0f - hpfSetpoint / RELAX_FACTOR_YAW, 0.0f);
   }

src/io/rgb_led.c

@@ -22,7 +22,7 @@
 // runs the update once every 16 loop times ( 16 mS )
 #define DOWNSAMPLE 16
 
-#define RGB_FILTER_TIME FILTERCALC(1000 * DOWNSAMPLE, RGB_FILTER_TIME_MICROSECONDS)
+#define RGB_FILTER_TIME lpfcalc(1000 * DOWNSAMPLE, RGB_FILTER_TIME_MICROSECONDS)
 #define RGB(r, g, b) ((((int)g & 0xff) << 16) | (((int)r & 0xff) << 8) | ((int)b & 0xff))
 
 #if (RGB_LED_NUMBER > 0)

src/io/vbat.c

@@ -56,14 +56,13 @@ static float vbat_auto_vdrop(float thrfilt, float tempvolt) {
         lastin[y] = vcomp[z];
       firstrun = 0;
     }
-    float ans;
     //  y(n) = x(n) - x(n-1) + R * y(n-1)
     //  out = in - lastin + coeff*lastout
     // hpf
-    ans = vcomp[z] - lastin[z] + FILTERCALC(1000 * 12, 6000e3) * lastout[z];
+    const float ans = vcomp[z] - lastin[z] + lpfcalc(1000 * 12, 6000e3) * lastout[z];
     lastin[z] = vcomp[z];
     lastout[z] = ans;
-    lpf(&score[z], ans * ans, FILTERCALC(1000 * 12, 60e6));
+    lpf(&score[z], ans * ans, lpfcalc(1000 * 12, 60e6));
     z++;
 
     if (z >= 12) {
@@ -88,12 +87,12 @@ void vbat_calc() {
 
   // read acd and scale based on processor voltage
   state.ibat = adc_read(ADC_CHAN_IBAT);
-  lpf(&state.ibat_filtered, state.ibat, FILTERCALC(1000, 5000e3));
+  lpf(&state.ibat_filtered, state.ibat, lpfcalc(1000, 5000e3));
 
   // li-ion battery model compensation time decay ( 18 seconds )
   state.vbat = adc_read(ADC_CHAN_VBAT);
   lpf(&state.vbat_filtered, state.vbat, 0.9968f);
-  lpf(&state.vbat_filtered_decay, state.vbat_filtered, FILTERCALC(1000, 18000e3));
+  lpf(&state.vbat_filtered_decay, state.vbat_filtered, lpfcalc(1000, 18000e3));
 
   state.vbat_cell_avg = state.vbat_filtered_decay / (float)state.lipo_cell_count;
 
@@ -115,7 +114,6 @@ void vbat_calc() {
   state.vbat_compensated = tempvolt + vdrop_factor * thrfilt;
   state.vbat_compensated_cell_avg = state.vbat_compensated / (float)state.lipo_cell_count;
 
-
   if (profile.voltage.use_filtered_voltage_for_warnings) {
     flags.lowbatt = state.vbat_cell_avg < profile.voltage.vbattlow ? 1 : 0;
   } else {

src/osd/render.c

@@ -293,7 +293,7 @@ static void print_osd_rssi(osd_element_t *el) {
   if (flags.failsafe)
     state.rx_rssi = 0.0f;
 
-  lpf(&rx_rssi_filt, state.rx_rssi, FILTERCALC(state.looptime * 1e6f * 133.0f, 2e6f)); // 2 second filtertime and 15hz refresh rate @4k, 30hz@ 8k loop
+  lpf(&rx_rssi_filt, state.rx_rssi, lpfcalc(state.looptime * 1e6f * 133.0f, 2e6f)); // 2 second filtertime and 15hz refresh rate @4k, 30hz@ 8k loop
 
   osd_start(osd_attr(el), el->pos_x, el->pos_y);
   osd_write_uint(rx_rssi_filt - 0.5f, 4);

src/util/util.h

@@ -46,10 +46,6 @@
     _a > _b ? _a : _b;  \
   })
 
-// this should be precalculated by the compiler when parameters are constant
-//(1 - alpha. filtertime = 1 / filter-cutoff-frequency) as long as filtertime > sampleperiod
-#define FILTERCALC(sampleperiod, filtertime) (1.0f - (6.0f * (float)(sampleperiod)) / (3.0f * (float)(sampleperiod) + (float)(filtertime)))
-
 #define MHZ_TO_HZ(mhz) (mhz * 1000000)
 
 #define MAKE_SEMVER(major, minor, patch) ((major << 16) | (minor << 8) | patch)