load_cell_probe: Create load_cell_probe MCU object

Implement MCU features that enable using an adc to stop an axis

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

4 files changed, 314 insertions, 1 deletions

diff --git a/src/Kconfig b/src/Kconfig
index fba367d3..6740946d 100644
--- a/src/Kconfig
+++ b/src/Kconfig
@@ -177,10 +177,14 @@ 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
+config WANT_LOAD_CELL_PROBE
     bool
     depends on WANT_HX71X || WANT_ADS1220
     default y
+config NEED_SOS_FILTER
+    bool
+    depends on WANT_LOAD_CELL_PROBE
+    default y
 menu "Optional features (to reduce code size)"
     depends on HAVE_LIMITED_CODE_SIZE
 config WANT_ADC
diff --git a/src/Makefile b/src/Makefile
index 75f2c3b3..974204bc 100644
--- a/src/Makefile
+++ b/src/Makefile
@@ -28,3 +28,4 @@ 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-$(CONFIG_WANT_LOAD_CELL_PROBE) += load_cell_probe.c
diff --git a/src/load_cell_probe.c b/src/load_cell_probe.c
new file mode 100644
index 00000000..49dfbe52
--- /dev/null
+++ b/src/load_cell_probe.c
@@ -0,0 +1,298 @@
+// Load Cell based end stops.
+//
+// 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 "trsync.h" // trsync_do_trigger
+#include "board/misc.h" // timer_read_time
+#include "sos_filter.h" // fixedQ12_t
+#include "load_cell_probe.h" //load_cell_probe_report_sample
+#include <stdint.h> // int32_t
+#include <stdlib.h> // abs
+
+// 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 ((1 << FIXEDQ16) - 1)
+#define ERROR_SAFETY_RANGE 0
+#define ERROR_OVERFLOW 1
+#define ERROR_WATCHDOG 2
+
+// Flags
+enum {FLAG_IS_HOMING = 1 << 0
+    , FLAG_IS_HOMING_TRIGGER = 1 << 1
+    , FLAG_AWAIT_HOMING = 1 << 2
+    };
+
+// Endstop Structure
+struct load_cell_probe {
+    struct timer time;
+    uint32_t trigger_grams, 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;
+    uint8_t flags, trigger_reason, error_reason, watchdog_max
+            , watchdog_count;
+    fixedQ16_t trigger_grams_fixed;
+    fixedQ2_t grams_per_count;
+    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 load_cell_probe *lce, const int32_t counts) {
+    // tearing ensures readings are referenced to 0.0g
+    const int32_t delta = counts - lce->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)lce->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 load_cell_probe *lce)
+{
+    return !!(mask & lce->flags);
+}
+
+static inline void
+set_flag(uint8_t mask, struct load_cell_probe *lce)
+{
+    lce->flags |= mask;
+}
+
+static inline void
+clear_flag(uint8_t mask, struct load_cell_probe *lce)
+{
+    lce->flags &= ~mask;
+}
+
+void
+try_trigger(struct load_cell_probe *lce, uint32_t ticks)
+{
+    uint8_t is_homing_triggered = is_flag_set(FLAG_IS_HOMING_TRIGGER, lce);
+    if (!is_homing_triggered) {
+        // the first triggering sample when homing sets the trigger time
+        lce->trigger_ticks = ticks;
+        // this flag latches until a reset, disabling further triggering
+        set_flag(FLAG_IS_HOMING_TRIGGER, lce);
+        trsync_do_trigger(lce->ts, lce->trigger_reason);
+    }
+}
+
+void
+trigger_error(struct load_cell_probe *lce, uint8_t error_code)
+{
+    trsync_do_trigger(lce->ts, lce->error_reason + error_code);
+}
+
+// Used by Sensors to report new raw ADC sample
+void
+load_cell_probe_report_sample(struct load_cell_probe *lce
+                                , const int32_t sample)
+{
+    // only process samples when homing
+    uint8_t is_homing = is_flag_set(FLAG_IS_HOMING, lce);
+    if (!is_homing) {
+        return;
+    }
+
+    // save new sample
+    uint32_t ticks = timer_read_time();
+    lce->last_sample_ticks = ticks;
+    lce->watchdog_count = 0;
+
+    // do not trigger before homing start time
+    uint8_t await_homing = is_flag_set(FLAG_AWAIT_HOMING, lce);
+    if (await_homing && timer_is_before(ticks, lce->homing_start_time)) {
+        return;
+    }
+    clear_flag(FLAG_AWAIT_HOMING, lce);
+
+    // check for safety limit violations
+    const uint8_t is_safety_trigger = sample <= lce->safety_counts_min
+                                        || sample >= lce->safety_counts_max;
+    // too much force, this is an error while homing
+    if (is_safety_trigger) {
+        trigger_error(lce, ERROR_SAFETY_RANGE);
+        return;
+    }
+
+    // convert sample to grams
+    const fixedQ48_t raw_grams = counts_to_grams(lce, sample);
+    if (overflows_int32(raw_grams)) {
+        trigger_error(lce, ERROR_OVERFLOW);
+        return;
+    }
+
+    // perform filtering
+    const fixedQ16_t filtered_grams = sosfilt(lce->sf, (fixedQ16_t)raw_grams);
+
+    // update trigger state
+    if (abs(filtered_grams) >= lce->trigger_grams_fixed) {
+        try_trigger(lce, lce->last_sample_ticks);
+    }
+}
+
+// Timer callback that monitors for timeouts
+static uint_fast8_t
+watchdog_event(struct timer *t)
+{
+    struct load_cell_probe *lce = container_of(t, struct load_cell_probe
+                                        , time);
+    uint8_t is_homing = is_flag_set(FLAG_IS_HOMING, lce);
+    uint8_t is_homing_trigger = is_flag_set(FLAG_IS_HOMING_TRIGGER, lce);
+    // the watchdog stops when not homing or when trsync becomes triggered
+    if (!is_homing || is_homing_trigger) {
+        return SF_DONE;
+    }
+
+    if (lce->watchdog_count > lce->watchdog_max) {
+        trigger_error(lce, ERROR_WATCHDOG);
+    }
+    lce->watchdog_count += 1;
+
+    // A sample was recently delivered, continue monitoring
+    lce->time.waketime += lce->rest_ticks;
+    return SF_RESCHEDULE;
+}
+
+static void
+set_endstop_range(struct load_cell_probe *lce
+                , 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 = 1 << FIXEDQ2_FRAC_BITS;
+    if (grams_per_count < 0 || grams_per_count >= one) {
+        shutdown("grams_per_count is invalid");
+    }
+    lce->safety_counts_min = safety_counts_min;
+    lce->safety_counts_max = safety_counts_max;
+    lce->tare_counts = tare_counts;
+    lce->trigger_grams = trigger_grams;
+    lce->trigger_grams_fixed = trigger_grams << FIXEDQ16_FRAC_BITS;
+    lce->grams_per_count = grams_per_count;
+}
+
+// Create a load_cell_probe
+void
+command_config_load_cell_probe(uint32_t *args)
+{
+    struct load_cell_probe *lce = oid_alloc(args[0]
+                            , command_config_load_cell_probe, sizeof(*lce));
+    lce->flags = 0;
+    lce->trigger_ticks = 0;
+    lce->watchdog_max = 0;
+    lce->watchdog_count = 0;
+    lce->sf = sos_filter_oid_lookup(args[1]);
+    set_endstop_range(lce, 0, 0, 0, 0, 0);
+}
+DECL_COMMAND(command_config_load_cell_probe, "config_load_cell_probe"
+                                               " oid=%c sos_filter_oid=%c");
+
+// Lookup a load_cell_probe
+struct load_cell_probe *
+load_cell_probe_oid_lookup(uint8_t oid)
+{
+    return oid_lookup(oid, command_config_load_cell_probe);
+}
+
+// Set the triggering range and tare value
+void
+command_load_cell_probe_set_range(uint32_t *args)
+{
+    struct load_cell_probe *lce = load_cell_probe_oid_lookup(args[0]);
+    set_endstop_range(lce, args[1], args[2], args[3], args[4]
+                , (fixedQ16_t)args[5]);
+}
+DECL_COMMAND(command_load_cell_probe_set_range, "load_cell_probe_set_range"
+    " oid=%c safety_counts_min=%i safety_counts_max=%i tare_counts=%i"
+    " trigger_grams=%u grams_per_count=%i");
+
+// Home an axis
+void
+command_load_cell_probe_home(uint32_t *args)
+{
+    struct load_cell_probe *lce = load_cell_probe_oid_lookup(args[0]);
+    sched_del_timer(&lce->time);
+    // clear the homing trigger flag
+    clear_flag(FLAG_IS_HOMING_TRIGGER, lce);
+    clear_flag(FLAG_IS_HOMING, lce);
+    lce->trigger_ticks = 0;
+    lce->ts = NULL;
+    // 0 samples indicates homing is finished
+    if (args[3] == 0) {
+        // Disable end stop checking
+        return;
+    }
+    lce->ts = trsync_oid_lookup(args[1]);
+    lce->trigger_reason = args[2];
+    lce->error_reason = args[3];
+    lce->time.waketime = args[4];
+    lce->homing_start_time = args[4];
+    lce->rest_ticks = args[5];
+    lce->watchdog_max = args[6];
+    lce->watchdog_count = 0;
+    lce->time.func = watchdog_event;
+    set_flag(FLAG_IS_HOMING, lce);
+    set_flag(FLAG_AWAIT_HOMING, lce);
+    sched_add_timer(&lce->time);
+}
+DECL_COMMAND(command_load_cell_probe_home,
+             "load_cell_probe_home oid=%c trsync_oid=%c trigger_reason=%c"
+             " error_reason=%c clock=%u rest_ticks=%u timeout=%u");
+
+void
+command_load_cell_probe_query_state(uint32_t *args)
+{
+    uint8_t oid = args[0];
+    struct load_cell_probe *lce = load_cell_probe_oid_lookup(args[0]);
+    sendf("load_cell_probe_state oid=%c is_homing_trigger=%c trigger_ticks=%u"
+            , oid
+            , is_flag_set(FLAG_IS_HOMING_TRIGGER, lce)
+            , lce->trigger_ticks);
+}
+DECL_COMMAND(command_load_cell_probe_query_state
+                , "load_cell_probe_query_state oid=%c");
diff --git a/src/load_cell_probe.h b/src/load_cell_probe.h
new file mode 100644
index 00000000..e67c16e5
--- /dev/null
+++ b/src/load_cell_probe.h
@@ -0,0 +1,10 @@
+#ifndef __LOAD_CELL_PROBE_H
+#define __LOAD_CELL_PROBE_H
+
+#include <stdint.h> // uint8_t
+
+struct load_cell_probe *load_cell_probe_oid_lookup(uint8_t oid);
+void load_cell_probe_report_sample(struct load_cell_probe *lce
+                        , int32_t sample);
+
+#endif // load_cell_probe.h