filter: precompute order correction
#define M_PI_F 3.14159265358979323846f
+// equation is 1 / sqrtf(powf(2, 1.0f / ORDER) - 1);
+#define ORDER1_CORRECTION 1
+#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;
@@ -46,8 +51,7 @@ void filter_lp_pt1_init(filter_lp_pt1 *filter, filter_state_t *state, uint8_t co
}
void filter_lp_pt1_coeff(filter_lp_pt1 *filter, float hz) {
- const float cutoff = 1 / sqrtf(powf(2, 1.0f / 1.0f) - 1);
- const float rc = 1 / (2 * cutoff * M_PI_F * hz);
+ const float rc = 1 / (2 * ORDER1_CORRECTION * M_PI_F * hz);
filter->alpha = state.looptime / (rc + state.looptime);
}
@@ -63,8 +67,7 @@ void filter_lp_pt2_init(filter_lp_pt2 *filter, filter_state_t *state, uint8_t co
}
void filter_lp_pt2_coeff(filter_lp_pt2 *filter, float hz) {
- const float cutoff = 1 / sqrtf(powf(2, 1.0f / 2.0f) - 1);
- const float rc = 1 / (2 * cutoff * M_PI_F * hz);
+ const float rc = 1 / (2 * ORDER2_CORRECTION * M_PI_F * hz);
filter->alpha = state.looptime / (rc + state.looptime);
}
@@ -81,8 +84,7 @@ 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) {
- const float cutoff = 1 / sqrtf(powf(2, 1.0f / 3.0f) - 1);
- const float rc = 1 / (2 * cutoff * M_PI_F * hz);
+ const float rc = 1 / (2 * ORDER3_CORRECTION * M_PI_F * hz);
filter->alpha = state.looptime / (rc + state.looptime);
}
