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 +// +// 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 // int32_t +#include // 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 // 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