From 109f13c7977381dc0ce926e5842621260a5d2e1b Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Sat, 10 Jan 2026 19:17:05 -0500 Subject: [PATCH] sos_filter: Consistently use "frac_bits" instead of "int_bits" Internally describe the Qx.y format using the number of fractional bits. Signed-off-by: Kevin O'Connor --- klippy/extras/load_cell_probe.py | 18 +++++++--------- klippy/extras/sos_filter.py | 37 ++++++++++++++++---------------- src/sos_filter.c | 34 ++++++++++++++--------------- 3 files changed, 43 insertions(+), 46 deletions(-) diff --git a/klippy/extras/load_cell_probe.py b/klippy/extras/load_cell_probe.py index 3b08b4510..56992e9f8 100644 --- a/klippy/extras/load_cell_probe.py +++ b/klippy/extras/load_cell_probe.py @@ -10,10 +10,8 @@ from . import probe, sos_filter, load_cell, hx71x, ads1220 np = None # delay NumPy import until configuration time # constants for fixed point numbers -Q2_INT_BITS = 2 -Q2_FRAC_BITS = (32 - (1 + Q2_INT_BITS)) -Q16_INT_BITS = 16 -Q16_FRAC_BITS = (32 - (1 + Q16_INT_BITS)) +Q2_29_FRAC_BITS = 29 +Q16_15_FRAC_BITS = 15 class TapAnalysis: @@ -212,7 +210,7 @@ class ContinuousTareFilterHelper: def _create_filter(self, design, cmd_queue): sf = sos_filter.MCU_SosFilter(self._sensor.get_mcu(), cmd_queue, 4, - Q2_INT_BITS, Q16_INT_BITS) + Q2_29_FRAC_BITS, Q16_15_FRAC_BITS) sf.set_filter_design(design) return sf @@ -282,15 +280,15 @@ class LoadCellProbeConfigHelper: raise cmd_err("Load cell force_safety_limit exceeds sensor range!") return safety_min, safety_max - # calculate 1/counts_per_gram in Q2 fixed point + # calculate 1/counts_per_gram in Q2.29 fixed point def get_grams_per_count(self): counts_per_gram = self._load_cell.get_counts_per_gram() # The counts_per_gram could be so large that it becomes 0.0 when - # converted to Q2 format. This would mean the ADC range only measures a - # few grams which seems very unlikely. Treat this as an error: - if counts_per_gram >= 2**Q2_FRAC_BITS: + # converted to Q2.29 format. This would mean the ADC range only measures + # a few grams which seems very unlikely. Treat this as an error: + if counts_per_gram >= 2**Q2_29_FRAC_BITS: raise OverflowError("counts_per_gram value is too large to filter") - return sos_filter.to_fixed_32((1. / counts_per_gram), Q2_INT_BITS) + return sos_filter.to_fixed_32((1. / counts_per_gram), Q2_29_FRAC_BITS) # MCU_trigger_analog is the interface to `trigger_analog` on the MCU diff --git a/klippy/extras/sos_filter.py b/klippy/extras/sos_filter.py index 9c9cda499..021dfff28 100644 --- a/klippy/extras/sos_filter.py +++ b/klippy/extras/sos_filter.py @@ -6,18 +6,17 @@ MAX_INT32 = (2 ** 31) MIN_INT32 = -(2 ** 31) - 1 -def assert_is_int32(value, error): +def assert_is_int32(value, frac_bits): if value > MAX_INT32 or value < MIN_INT32: - raise OverflowError(error) + raise OverflowError("Fixed point Q%d.%d overflow" + % (31-frac_bits, frac_bits)) return value # convert a floating point value to a 32 bit fixed point representation # checks for overflow -def to_fixed_32(value, int_bits): - fractional_bits = (32 - (1 + int_bits)) - fixed_val = int(value * (2 ** fractional_bits)) - return assert_is_int32(fixed_val, "Fixed point Q%i overflow" - % (int_bits,)) +def to_fixed_32(value, frac_bits): + fixed_val = int(value * (2**frac_bits)) + return assert_is_int32(fixed_val, frac_bits) # Digital filter designer and container @@ -69,13 +68,13 @@ class MCU_SosFilter: # max_sections should be the largest number of sections you expect # to use at runtime. def __init__(self, mcu, cmd_queue, max_sections, - coeff_int_bits=2, value_int_bits=15): + coeff_frac_bits=29, value_frac_bits=16): self._mcu = mcu self._cmd_queue = cmd_queue self._oid = self._mcu.create_oid() self._max_sections = max_sections - self._coeff_int_bits = coeff_int_bits - self._value_int_bits = value_int_bits + self._coeff_frac_bits = coeff_frac_bits + self._value_frac_bits = value_frac_bits self._design = None self._set_section_cmd = self._set_state_cmd = self._set_active_cmd =None self._last_sent_coeffs = [None] * self._max_sections @@ -83,11 +82,11 @@ class MCU_SosFilter: % (self._oid, self._max_sections)) self._mcu.register_config_callback(self._build_config) - def _validate_int_bits(self, int_bits): - if int_bits < 1 or int_bits > 30: - raise ValueError("The number of integer bits (%i) must be a" - " value between 1 and 30" % (int_bits,)) - return int_bits + def _validate_frac_bits(self, frac_bits): + if frac_bits < 0 or frac_bits > 31: + raise ValueError("The number of fractional bits (%i) must be a" + " value between 0 and 31" % (frac_bits,)) + return frac_bits def _build_config(self): self._set_section_cmd = self._mcu.lookup_command( @@ -97,7 +96,7 @@ class MCU_SosFilter: "sos_filter_set_state oid=%c section_idx=%c state0=%i state1=%i", cq=self._cmd_queue) self._set_active_cmd = self._mcu.lookup_command( - "sos_filter_set_active oid=%c n_sections=%c coeff_int_bits=%c", + "sos_filter_set_active oid=%c n_sections=%c coeff_frac_bits=%c", cq=self._cmd_queue) def get_oid(self): @@ -117,7 +116,7 @@ class MCU_SosFilter: fixed_section = [] for col, coeff in enumerate(section): if col != 3: # omit column 3 - fixed_coeff = to_fixed_32(coeff, self._coeff_int_bits) + fixed_coeff = to_fixed_32(coeff, self._coeff_frac_bits) fixed_section.append(fixed_coeff) elif coeff != 1.0: # double check column 3 is always 1.0 raise ValueError("Coefficient 3 is expected to be 1.0" @@ -139,7 +138,7 @@ class MCU_SosFilter: % (nun_states,)) fixed_state = [] for col, value in enumerate(section): - fixed_state.append(to_fixed_32(value, self._value_int_bits)) + fixed_state.append(to_fixed_32(value, self._value_frac_bits)) sos_state.append(fixed_state) return sos_state @@ -173,4 +172,4 @@ class MCU_SosFilter: self._set_state_cmd.send([self._oid, i, state[0], state[1]]) # Activate filter self._set_active_cmd.send([self._oid, num_sections, - self._coeff_int_bits]) + self._coeff_frac_bits]) diff --git a/src/sos_filter.c b/src/sos_filter.c index 036c50ce5..60c6abf4c 100644 --- a/src/sos_filter.c +++ b/src/sos_filter.c @@ -21,7 +21,6 @@ struct sos_filter_section { struct sos_filter { uint8_t max_sections, n_sections, coeff_frac_bits; - uint32_t coeff_rounding; // filter composed of second order sections struct sos_filter_section filter[0]; }; @@ -33,15 +32,17 @@ overflows_int32(int64_t value) } // Multiply a coeff*value and shift result by coeff_frac_bits -static inline int -fixed_mul(struct sos_filter *sf, int32_t coeff, int32_t value, int32_t *res) +static int +fixed_mul(int32_t coeff, int32_t value, uint_fast8_t frac_bits, int32_t *res) { // 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; + int64_t result = (int64_t)coeff * (int64_t)value; + if (frac_bits) { + // round up at the last bit to be shifted away + result += 1 << (frac_bits - 1); + // shift the decimal right to discard the coefficient fractional bits + result >>= frac_bits; + } // truncate significant 32 bits *res = (int32_t)result; // check for overflow of int32_t @@ -55,17 +56,18 @@ int sos_filter_apply(struct sos_filter *sf, int32_t *pvalue) { int32_t cur_val = *pvalue; + uint_fast8_t cfb = sf->coeff_frac_bits; // 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 int32_t next_val, c1_cur, c2_cur, c3_next, c4_next; - int ret = fixed_mul(sf, section->coeff[0], cur_val, &next_val); + int ret = fixed_mul(section->coeff[0], cur_val, cfb, &next_val); next_val += section->state[0]; - ret |= fixed_mul(sf, section->coeff[1], cur_val, &c1_cur); - ret |= fixed_mul(sf, section->coeff[3], next_val, &c3_next); - ret |= fixed_mul(sf, section->coeff[2], cur_val, &c2_cur); - ret |= fixed_mul(sf, section->coeff[4], next_val, &c4_next); + ret |= fixed_mul(section->coeff[1], cur_val, cfb, &c1_cur); + ret |= fixed_mul(section->coeff[3], next_val, cfb, &c3_next); + ret |= fixed_mul(section->coeff[2], cur_val, cfb, &c2_cur); + ret |= fixed_mul(section->coeff[4], next_val, cfb, &c4_next); if (ret) // Overflow return -1; @@ -151,9 +153,7 @@ command_sos_filter_activate(uint32_t *args) if (n_sections > sf->max_sections) shutdown("Filter section index larger than max_sections"); sf->n_sections = n_sections; - uint8_t coeff_int_bits = args[2]; - sf->coeff_frac_bits = (31 - coeff_int_bits); - sf->coeff_rounding = (1 << (sf->coeff_frac_bits - 1)); + sf->coeff_frac_bits = args[2] & 0x3f; } DECL_COMMAND(command_sos_filter_activate - , "sos_filter_set_active oid=%c n_sections=%c coeff_int_bits=%c"); + , "sos_filter_set_active oid=%c n_sections=%c coeff_frac_bits=%c");