diff --git a/klippy/extras/load_cell_probe.py b/klippy/extras/load_cell_probe.py index 56992e9f8..2d5fce81e 100644 --- a/klippy/extras/load_cell_probe.py +++ b/klippy/extras/load_cell_probe.py @@ -210,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_29_FRAC_BITS, Q16_15_FRAC_BITS) + Q2_29_FRAC_BITS) sf.set_filter_design(design) return sf @@ -280,7 +280,7 @@ 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.29 fixed point + # calculate 1/counts_per_gram 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 @@ -288,7 +288,7 @@ class LoadCellProbeConfigHelper: # 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_29_FRAC_BITS) + return 1. / counts_per_gram # MCU_trigger_analog is the interface to `trigger_analog` on the MCU @@ -330,9 +330,8 @@ class MCU_trigger_analog: "trigger_analog_state oid=%c is_homing_trigger=%c " "trigger_ticks=%u", oid=self._oid, cq=self._cmd_queue) self._set_range_cmd = self._mcu.lookup_command( - "trigger_analog_set_range" - " oid=%c safety_counts_min=%i safety_counts_max=%i tare_counts=%i" - " trigger_grams=%u grams_per_count=%i", cq=self._cmd_queue) + "trigger_analog_set_range oid=%c safety_counts_min=%i" + " safety_counts_max=%i trigger_value=%i", cq=self._cmd_queue) self._home_cmd = self._mcu.lookup_command( "trigger_analog_home oid=%c trsync_oid=%c trigger_reason=%c" " error_reason=%c clock=%u rest_ticks=%u timeout=%u", @@ -359,10 +358,13 @@ class MCU_trigger_analog: self._load_cell.tare(tare_counts) # update internal tare value safety_min, safety_max = self._config_helper.get_safety_range(gcmd) - args = [self._oid, safety_min, safety_max, int(tare_counts), - self._config_helper.get_trigger_force_grams(gcmd), - self._config_helper.get_grams_per_count()] - self._set_range_cmd.send(args) + trigger_val = self._config_helper.get_trigger_force_grams(gcmd) + tval32 = sos_filter.to_fixed_32(trigger_val, Q16_15_FRAC_BITS) + self._set_range_cmd.send([self._oid, safety_min, safety_max, tval32]) + gpc = self._config_helper.get_grams_per_count() + Q17_14_FRAC_BITS = 14 + self._sos_filter.set_offset_scale(int(-tare_counts), gpc, + Q17_14_FRAC_BITS, Q16_15_FRAC_BITS) self._sos_filter.reset_filter() def home_start(self, print_time): diff --git a/klippy/extras/sos_filter.py b/klippy/extras/sos_filter.py index 021dfff28..6456eaa5c 100644 --- a/klippy/extras/sos_filter.py +++ b/klippy/extras/sos_filter.py @@ -67,17 +67,20 @@ class DigitalFilter: 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_frac_bits=29, value_frac_bits=16): + def __init__(self, mcu, cmd_queue, max_sections, coeff_frac_bits=29): self._mcu = mcu self._cmd_queue = cmd_queue self._oid = self._mcu.create_oid() self._max_sections = max_sections self._coeff_frac_bits = coeff_frac_bits - self._value_frac_bits = value_frac_bits + self._value_frac_bits = self._scale_frac_bits = 0 self._design = None - self._set_section_cmd = self._set_state_cmd = self._set_active_cmd =None + self._offset = 0 + self._scale = 1 + self._set_section_cmd = self._set_state_cmd = None + self._set_active_cmd = self._set_offset_scale_cmd = None self._last_sent_coeffs = [None] * self._max_sections + self._last_sent_offset_scale = None self._mcu.add_config_cmd("config_sos_filter oid=%d max_sections=%d" % (self._oid, self._max_sections)) self._mcu.register_config_callback(self._build_config) @@ -95,6 +98,9 @@ class MCU_SosFilter: self._set_state_cmd = self._mcu.lookup_command( "sos_filter_set_state oid=%c section_idx=%c state0=%i state1=%i", cq=self._cmd_queue) + self._set_offset_scale_cmd = self._mcu.lookup_command( + "sos_filter_set_offset_scale oid=%c offset=%i" + " scale=%i scale_frac_bits=%c", cq=self._cmd_queue) self._set_active_cmd = self._mcu.lookup_command( "sos_filter_set_active oid=%c n_sections=%c coeff_frac_bits=%c", cq=self._cmd_queue) @@ -142,6 +148,15 @@ class MCU_SosFilter: sos_state.append(fixed_state) return sos_state + # Set conversion of a raw value 1 to a 1.0 value processed by sos filter + def set_offset_scale(self, offset, scale, scale_frac_bits=0, + value_frac_bits=0): + self._offset = offset + self._value_frac_bits = value_frac_bits + scale_mult = scale * float(1 << value_frac_bits) + self._scale = to_fixed_32(scale_mult, scale_frac_bits) + self._scale_frac_bits = scale_frac_bits + # Change the filter coefficients and state at runtime def set_filter_design(self, design): self._design = design @@ -170,6 +185,11 @@ class MCU_SosFilter: # Send section initial states for i, state in enumerate(sos_state): self._set_state_cmd.send([self._oid, i, state[0], state[1]]) + # Send offset/scale (if they have changed) + args = (self._oid, self._offset, self._scale, self._scale_frac_bits) + if args != self._last_sent_offset_scale: + self._set_offset_scale_cmd.send(args) + self._last_sent_offset_scale = args # Activate filter self._set_active_cmd.send([self._oid, num_sections, self._coeff_frac_bits]) diff --git a/src/sos_filter.c b/src/sos_filter.c index 60c6abf4c..c11ae3df8 100644 --- a/src/sos_filter.c +++ b/src/sos_filter.c @@ -20,7 +20,8 @@ struct sos_filter_section { }; struct sos_filter { - uint8_t max_sections, n_sections, coeff_frac_bits; + uint8_t max_sections, n_sections, coeff_frac_bits, scale_frac_bits; + int32_t offset, scale; // filter composed of second order sections struct sos_filter_section filter[0]; }; @@ -55,9 +56,19 @@ fixed_mul(int32_t coeff, int32_t value, uint_fast8_t frac_bits, int32_t *res) int sos_filter_apply(struct sos_filter *sf, int32_t *pvalue) { - int32_t cur_val = *pvalue; - uint_fast8_t cfb = sf->coeff_frac_bits; + int32_t raw_val = *pvalue; + + // Apply offset and scale + int32_t offset = sf->offset, offset_val = raw_val + offset, cur_val; + if ((offset >= 0) != (offset_val >= raw_val)) + // Overflow + return -1; + int ret = fixed_mul(sf->scale, offset_val, sf->scale_frac_bits, &cur_val); + if (ret) + return -1; + // foreach section + uint_fast8_t cfb = sf->coeff_frac_bits; 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 @@ -144,6 +155,18 @@ command_sos_filter_set_state(uint32_t *args) DECL_COMMAND(command_sos_filter_set_state , "sos_filter_set_state oid=%c section_idx=%c state0=%i state1=%i"); +// Set incoming sample offset/scaling +void +command_trigger_analog_set_offset_scale(uint32_t *args) +{ + struct sos_filter *sf = sos_filter_oid_lookup(args[0]); + sf->offset = args[1]; + sf->scale = args[2]; + sf->scale_frac_bits = args[3] & 0x3f; +} +DECL_COMMAND(command_trigger_analog_set_offset_scale, + "sos_filter_set_offset_scale oid=%c offset=%i scale=%i scale_frac_bits=%c"); + // Set one section of the filter void command_sos_filter_activate(uint32_t *args) diff --git a/src/trigger_analog.c b/src/trigger_analog.c index 4a8419d3b..df64a4f40 100644 --- a/src/trigger_analog.c +++ b/src/trigger_analog.c @@ -13,25 +13,6 @@ #include "trigger_analog.h" // trigger_analog_update #include "trsync.h" // trsync_do_trigger -// Q2.29 -typedef int32_t fixedQ2_t; -#define FIXEDQ2 2 -#define FIXEDQ2_FRAC_BITS ((32 - FIXEDQ2) - 1) - -// Q32.29 - a Q2.29 value stored in int64 -typedef int64_t fixedQ32_t; -#define FIXEDQ32_FRAC_BITS FIXEDQ2_FRAC_BITS - -// Q16.15 -typedef int32_t fixedQ16_t; -#define FIXEDQ16 16 -#define FIXEDQ16_FRAC_BITS ((32 - FIXEDQ16) - 1) - -// Q48.15 - a Q16.15 value stored in int64 -typedef int64_t fixedQ48_t; -#define FIXEDQ48_FRAC_BITS FIXEDQ16_FRAC_BITS - -#define MAX_TRIGGER_GRAMS ((1L << FIXEDQ16) - 1) #define ERROR_SAFETY_RANGE 0 #define ERROR_OVERFLOW 1 #define ERROR_WATCHDOG 2 @@ -45,42 +26,15 @@ enum {FLAG_IS_HOMING = 1 << 0 // Endstop Structure struct trigger_analog { struct timer time; - uint32_t trigger_grams, trigger_ticks, last_sample_ticks, rest_ticks; + uint32_t trigger_ticks, last_sample_ticks, rest_ticks; uint32_t homing_start_time; struct trsync *ts; - int32_t safety_counts_min, safety_counts_max, tare_counts; + int32_t safety_counts_min, safety_counts_max; uint8_t flags, trigger_reason, error_reason, watchdog_max, watchdog_count; - fixedQ16_t trigger_grams_fixed; - fixedQ2_t grams_per_count; + int32_t trigger_value; struct sos_filter *sf; }; -static inline uint8_t -overflows_int32(int64_t value) { - return value > (int64_t)INT32_MAX || value < (int64_t)INT32_MIN; -} - -// returns the integer part of a fixedQ48_t -static inline int64_t -round_fixedQ48(const int64_t fixed_value) { - return fixed_value >> FIXEDQ48_FRAC_BITS; -} - -// Convert sensor counts to grams -static inline fixedQ48_t -counts_to_grams(struct trigger_analog *ta, const int32_t counts) { - // tearing ensures readings are referenced to 0.0g - const int32_t delta = counts - ta->tare_counts; - // convert sensor counts to grams by multiplication: 124 * 0.051 = 6.324 - // this optimizes to single cycle SMULL instruction - const fixedQ32_t product = (int64_t)delta * (int64_t)ta->grams_per_count; - // after multiplication there are 30 fraction bits, reduce to 15 - // caller verifies this wont overflow a 32bit int when truncated - const fixedQ48_t grams = product >> - (FIXEDQ32_FRAC_BITS - FIXEDQ48_FRAC_BITS); - return grams; -} - static inline uint8_t is_flag_set(const uint8_t mask, struct trigger_analog *ta) { @@ -149,23 +103,16 @@ trigger_analog_update(struct trigger_analog *ta, const int32_t sample) return; } - // convert sample to grams - const fixedQ48_t raw_grams = counts_to_grams(ta, sample); - if (overflows_int32(raw_grams)) { - trigger_error(ta, ERROR_OVERFLOW); - return; - } - // perform filtering - int32_t filtered_grams = raw_grams; - int ret = sos_filter_apply(ta->sf, &filtered_grams); + int32_t filtered_value = sample; + int ret = sos_filter_apply(ta->sf, &filtered_value); if (ret) { trigger_error(ta, ERROR_OVERFLOW); return; } // update trigger state - if (abs(filtered_grams) >= ta->trigger_grams_fixed) { + if (abs(filtered_value) >= ta->trigger_value) { try_trigger(ta, ta->last_sample_ticks); } } @@ -192,43 +139,13 @@ watchdog_event(struct timer *t) return SF_RESCHEDULE; } -static void -set_endstop_range(struct trigger_analog *ta - , int32_t safety_counts_min, int32_t safety_counts_max - , int32_t tare_counts, uint32_t trigger_grams - , fixedQ2_t grams_per_count) -{ - if (!(safety_counts_max >= safety_counts_min)) { - shutdown("Safety range reversed"); - } - if (trigger_grams > MAX_TRIGGER_GRAMS) { - shutdown("trigger_grams too large"); - } - // grams_per_count must be a positive fraction in Q2 format - const fixedQ2_t one = 1L << FIXEDQ2_FRAC_BITS; - if (grams_per_count < 0 || grams_per_count >= one) { - shutdown("grams_per_count is invalid"); - } - ta->safety_counts_min = safety_counts_min; - ta->safety_counts_max = safety_counts_max; - ta->tare_counts = tare_counts; - ta->trigger_grams = trigger_grams; - ta->trigger_grams_fixed = trigger_grams << FIXEDQ16_FRAC_BITS; - ta->grams_per_count = grams_per_count; -} - // Create a trigger_analog void command_config_trigger_analog(uint32_t *args) { struct trigger_analog *ta = oid_alloc( args[0], command_config_trigger_analog, sizeof(*ta)); - ta->flags = 0; - ta->trigger_ticks = 0; - ta->watchdog_max = 0; - ta->watchdog_count = 0; ta->sf = sos_filter_oid_lookup(args[1]); - set_endstop_range(ta, 0, 0, 0, 0, 0); } DECL_COMMAND(command_config_trigger_analog , "config_trigger_analog oid=%c sos_filter_oid=%c"); @@ -245,12 +162,12 @@ void command_trigger_analog_set_range(uint32_t *args) { struct trigger_analog *ta = trigger_analog_oid_lookup(args[0]); - set_endstop_range(ta, args[1], args[2], args[3], args[4] - , (fixedQ16_t)args[5]); + ta->safety_counts_min = args[1]; + ta->safety_counts_max = args[2]; + ta->trigger_value = args[3]; } DECL_COMMAND(command_trigger_analog_set_range, "trigger_analog_set_range" - " oid=%c safety_counts_min=%i safety_counts_max=%i tare_counts=%i" - " trigger_grams=%u grams_per_count=%i"); + " oid=%c safety_counts_min=%i safety_counts_max=%i trigger_value=%i"); // Home an axis void