diff --git a/src/flight/filter.c b/src/flight/filter.c index 38c9bc708..50aa13a3d 100644 --- a/src/flight/filter.c +++ b/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)); } diff --git a/src/flight/filter.h b/src/flight/filter.h index a54fb0703..23e28333a 100644 --- a/src/flight/filter.h +++ b/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(); diff --git a/src/flight/pid.c b/src/flight/pid.c index 63b7cd30c..f020ee63b 100644 --- a/src/flight/pid.c +++ b/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); } diff --git a/src/io/rgb_led.c b/src/io/rgb_led.c index 59306ee67..f02b19202 100644 --- a/src/io/rgb_led.c +++ b/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) diff --git a/src/io/vbat.c b/src/io/vbat.c index 249dcbbcd..5b3ee7dee 100644 --- a/src/io/vbat.c +++ b/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 { diff --git a/src/osd/render.c b/src/osd/render.c index 52106be83..586960bbb 100644 --- a/src/osd/render.c +++ b/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); diff --git a/src/util/util.h b/src/util/util.h index 5b203619a..2900eace5 100644 --- a/src/util/util.h +++ b/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)