diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/Kconfig | 4 | ||||
| -rw-r--r-- | src/Makefile | 1 | ||||
| -rw-r--r-- | src/sos_filter.c | 158 | ||||
| -rw-r--r-- | src/sos_filter.h | 12 |
4 files changed, 175 insertions, 0 deletions
diff --git a/src/Kconfig b/src/Kconfig index 05b9cb42..fba367d3 100644 --- a/src/Kconfig +++ b/src/Kconfig @@ -177,6 +177,10 @@ config NEED_SENSOR_BULK depends on WANT_ADXL345 || WANT_LIS2DW || WANT_MPU9250 || WANT_ICM20948 \ || WANT_HX71X || WANT_ADS1220 || WANT_LDC1612 || WANT_SENSOR_ANGLE default y +config NEED_SOS_FILTER + bool + depends on WANT_HX71X || WANT_ADS1220 + default y menu "Optional features (to reduce code size)" depends on HAVE_LIMITED_CODE_SIZE config WANT_ADC diff --git a/src/Makefile b/src/Makefile index ebebf14d..75f2c3b3 100644 --- a/src/Makefile +++ b/src/Makefile @@ -27,3 +27,4 @@ src-$(CONFIG_WANT_ADS1220) += sensor_ads1220.c src-$(CONFIG_WANT_LDC1612) += sensor_ldc1612.c src-$(CONFIG_WANT_SENSOR_ANGLE) += sensor_angle.c src-$(CONFIG_NEED_SENSOR_BULK) += sensor_bulk.c +src-$(CONFIG_NEED_SOS_FILTER) += sos_filter.c diff --git a/src/sos_filter.c b/src/sos_filter.c new file mode 100644 index 00000000..7272241e --- /dev/null +++ b/src/sos_filter.c @@ -0,0 +1,158 @@ +// Second Order sections Filter implementation using Fixed Point math +// +// Copyright (C) 2025 Gareth Farrington <gareth@waves.ky> +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include "basecmd.h" // oid_alloc +#include "command.h" // DECL_COMMAND +#include "sched.h" // shutdown +#include "sos_filter.h" // sos_filter + +typedef int32_t fixedQ_coeff_t; +typedef int32_t fixedQ_value_t; + +// filter strucutre sizes +#define SECTION_WIDTH 5 +#define STATE_WIDTH 2 + +struct sos_filter_section { + // filter composed of second order sections + fixedQ_coeff_t coeff[SECTION_WIDTH]; // aka sos + fixedQ_value_t state[STATE_WIDTH]; // aka zi +}; + +struct sos_filter { + uint8_t max_sections, n_sections, coeff_frac_bits, is_active; + uint32_t coeff_rounding; + // filter composed of second order sections + struct sos_filter_section filter[0]; +}; + +inline uint8_t +overflows_int32(int64_t value) { + return value > (int64_t)INT32_MAX || value < (int64_t)INT32_MIN; +} + +// Multiply a coefficient in fixedQ_coeff_t by a value fixedQ_value_t +static inline fixedQ_value_t +fixed_mul(struct sos_filter *sf, const fixedQ_coeff_t coeff + , const fixedQ_value_t value) { + // This optimizes to single cycle SMULL on Arm Coretex M0+ + int64_t product = (int64_t)coeff * (int64_t)value; + // round up at the last bit to be shifted away + product += sf->coeff_rounding; + // shift the decimal right to discard the coefficient fractional bits + int64_t result = product >> sf->coeff_frac_bits; + // check for overflow of int32_t + if (overflows_int32(result)) { + shutdown("fixed_mul: overflow"); + } + // truncate significant 32 bits + return (fixedQ_value_t)result; +} + +// Apply the sosfilt algorithm to a new datapoint +// returns the fixedQ_value_t filtered value +int32_t +sosfilt(struct sos_filter *sf, const int32_t unfiltered_value) { + if (!sf->is_active) { + shutdown("sos_filter not property initialized"); + } + + // an empty filter performs no filtering + if (sf->n_sections == 0) { + return unfiltered_value; + } + + fixedQ_value_t cur_val = unfiltered_value; + // foreach section + for (int section_idx = 0; section_idx < sf->n_sections; section_idx++) { + struct sos_filter_section *section = &(sf->filter[section_idx]); + // apply the section's filter coefficients to input + fixedQ_value_t next_val = fixed_mul(sf, section->coeff[0], cur_val); + next_val += section->state[0]; + section->state[0] = fixed_mul(sf, section->coeff[1], cur_val) + - fixed_mul(sf, section->coeff[3], next_val) + + (section->state[1]); + section->state[1] = fixed_mul(sf, section->coeff[2], cur_val) + - fixed_mul(sf, section->coeff[4], next_val); + cur_val = next_val; + } + + return (int32_t)cur_val; +} + +// Create an sos_filter +void +command_config_sos_filter(uint32_t *args) +{ + uint32_t max_sections = args[1]; + uint32_t size = offsetof(struct sos_filter, filter[max_sections]); + struct sos_filter *sf = oid_alloc(args[0] + , command_config_sos_filter, size); + sf->max_sections = max_sections; + sf->is_active = 0; +} +DECL_COMMAND(command_config_sos_filter, "config_sos_filter oid=%c" + " max_sections=%u"); + +// Lookup an sos_filter +struct sos_filter * +sos_filter_oid_lookup(uint8_t oid) +{ + return oid_lookup(oid, command_config_sos_filter); +} + +// Set one section of the filter +void +command_sos_filter_set_section(uint32_t *args) +{ + struct sos_filter *sf = sos_filter_oid_lookup(args[0]); + // setting a section marks the filter as inactive + sf->is_active = 0; + uint8_t section_idx = args[1]; + // copy section data + const uint8_t arg_base = 2; + for (uint8_t i = 0; i < SECTION_WIDTH; i++) { + sf->filter[section_idx].coeff[i] = args[i + arg_base]; + } +} +DECL_COMMAND(command_sos_filter_set_section + , "sos_filter_set_section oid=%c section_idx=%c" + " sos0=%i sos1=%i sos2=%i sos3=%i sos4=%i"); + +// Set the state of one section of the filter +void +command_sos_filter_set_state(uint32_t *args) +{ + struct sos_filter *sf = sos_filter_oid_lookup(args[0]); + // setting a section's state marks the filter as inactive + sf->is_active = 0; + // copy state data + uint8_t section_idx = args[1]; + const uint8_t arg_base = 2; + sf->filter[section_idx].state[0] = args[0 + arg_base]; + sf->filter[section_idx].state[1] = args[1 + arg_base]; +} +DECL_COMMAND(command_sos_filter_set_state + , "sos_filter_set_state oid=%c section_idx=%c state0=%i state1=%i"); + +// Set one section of the filter +void +command_sos_filter_activate(uint32_t *args) +{ + struct sos_filter *sf = sos_filter_oid_lookup(args[0]); + uint8_t n_sections = args[1]; + if (n_sections > sf->max_sections) { + shutdown("Filter section count larger than max_sections"); + } + sf->n_sections = n_sections; + const uint8_t coeff_int_bits = args[2]; + sf->coeff_frac_bits = (31 - coeff_int_bits); + sf->coeff_rounding = (1 << (sf->coeff_frac_bits - 1)); + // mark filter as ready to use + sf->is_active = 1; +} +DECL_COMMAND(command_sos_filter_activate + , "sos_filter_set_active oid=%c n_sections=%c coeff_int_bits=%c"); diff --git a/src/sos_filter.h b/src/sos_filter.h new file mode 100644 index 00000000..6c215dda --- /dev/null +++ b/src/sos_filter.h @@ -0,0 +1,12 @@ +#ifndef __SOS_FILTER_H +#define __SOS_FILTER_H + +#include <stdint.h> + +struct sos_filter; + +int32_t sosfilt(struct sos_filter *sf + , const int32_t unfiltered_value); +struct sos_filter *sos_filter_oid_lookup(uint8_t oid); + +#endif // sos_filter.h |