sos_filter: Second Order Sections MCU Filter

This is an implementation of the SOS fliltering algorithm that runs on the MCU.

The filter opperates on data in fixed point format to avoid use of the FPU as klipper does not support FPU usage.

This host object handles duties of initalizing and resetting the filter so client dont have to declare their own commands for these opperations. Clients can select how many integer bits they want to use for both the filter coefficients and the filters output value. An arbitrary number of filter sections can be configured. Filters can be designed on the fly with the SciPy library or loaded from another source.

Signed-off-by: Gareth Farrington <gareth@waves.ky>

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

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

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

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