sos_filter: Move offset/scale support from trigger_analog.c to sos_filter.c

Support offsetting and scaling the initial raw value prior to
processing in the sos_filter.

Remove that support from trigger_analog.c .

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

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):

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

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)

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