3D-printing/klipper
1
0
Fork 0
mirror of https://github.com/Klipper3d/klipper.git synced 2026-02-07 16:50:54 -07:00
Code Issues Projects Releases Packages Wiki Activity Actions 4

sos_filter: Consistently use "frac_bits" instead of "int_bits"

Browse source 
Internally describe the Qx.y format using the number of fractional
bits.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor 2026-01-10 19:17:05 -05:00
parent 3b5045ed9e
commit 109f13c797
3 changed files with 43 additions and 46 deletions
Download patch file Download diff file Expand all files Collapse all files

18
klippy/extras/load_cell_probe.py
View file

@ -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

37
klippy/extras/sos_filter.py
View file

@ -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])

34
src/sos_filter.c
View file

@ -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");