diff --git a/klippy/extras/ads1220.py b/klippy/extras/ads1220.py index 5e9ef72ba..fda584ac3 100644 --- a/klippy/extras/ads1220.py +++ b/klippy/extras/ads1220.py @@ -110,7 +110,7 @@ class ADS1220: self.query_ads1220_cmd = self.mcu.lookup_command( "query_ads1220 oid=%c rest_ticks=%u", cq=cmdqueue) self.attach_probe_cmd = self.mcu.lookup_command( - "ads1220_attach_load_cell_probe oid=%c load_cell_probe_oid=%c") + "ads1220_attach_trigger_analog oid=%c trigger_analog_oid=%c") self.ffreader.setup_query_command("query_ads1220_status oid=%c", oid=self.oid, cq=cmdqueue) @@ -129,8 +129,8 @@ class ADS1220: def add_client(self, callback): self.batch_bulk.add_client(callback) - def attach_load_cell_probe(self, load_cell_probe_oid): - self.attach_probe_cmd.send([self.oid, load_cell_probe_oid]) + def attach_trigger_analog(self, trigger_analog_oid): + self.attach_probe_cmd.send([self.oid, trigger_analog_oid]) # Measurement decoding def _convert_samples(self, samples): diff --git a/klippy/extras/hx71x.py b/klippy/extras/hx71x.py index a7f49f8ad..60fe6047c 100644 --- a/klippy/extras/hx71x.py +++ b/klippy/extras/hx71x.py @@ -66,7 +66,7 @@ class HX71xBase: self.query_hx71x_cmd = self.mcu.lookup_command( "query_hx71x oid=%c rest_ticks=%u") self.attach_probe_cmd = self.mcu.lookup_command( - "hx71x_attach_load_cell_probe oid=%c load_cell_probe_oid=%c") + "hx71x_attach_trigger_analog oid=%c trigger_analog_oid=%c") self.ffreader.setup_query_command("query_hx71x_status oid=%c", oid=self.oid, cq=self.mcu.alloc_command_queue()) @@ -87,8 +87,8 @@ class HX71xBase: def add_client(self, callback): self.batch_bulk.add_client(callback) - def attach_load_cell_probe(self, load_cell_probe_oid): - self.attach_probe_cmd.send([self.oid, load_cell_probe_oid]) + def attach_trigger_analog(self, trigger_analog_oid): + self.attach_probe_cmd.send([self.oid, trigger_analog_oid]) # Measurement decoding def _convert_samples(self, samples): diff --git a/klippy/extras/load_cell_probe.py b/klippy/extras/load_cell_probe.py index 9e5b0475e..9414ffc54 100644 --- a/klippy/extras/load_cell_probe.py +++ b/klippy/extras/load_cell_probe.py @@ -295,9 +295,9 @@ class LoadCellProbeConfigHelper: return sos_filter.to_fixed_32((1. / counts_per_gram), Q2_INT_BITS) -# McuLoadCellProbe is the interface to `load_cell_probe` on the MCU +# MCU_trigger_analog is the interface to `trigger_analog` on the MCU # This also manages the SosFilter so all commands use one command queue -class McuLoadCellProbe: +class MCU_trigger_analog: WATCHDOG_MAX = 3 ERROR_SAFETY_RANGE = mcu.MCU_trsync.REASON_COMMS_TIMEOUT + 1 ERROR_OVERFLOW = mcu.MCU_trsync.REASON_COMMS_TIMEOUT + 2 @@ -325,27 +325,27 @@ class McuLoadCellProbe: def _config_commands(self): self._sos_filter.create_filter() self._mcu.add_config_cmd( - "config_load_cell_probe oid=%d sos_filter_oid=%d" % ( + "config_trigger_analog oid=%d sos_filter_oid=%d" % ( self._oid, self._sos_filter.get_oid())) def _build_config(self): # Lookup commands self._query_cmd = self._mcu.lookup_query_command( - "load_cell_probe_query_state oid=%c", - "load_cell_probe_state oid=%c is_homing_trigger=%c " + "trigger_analog_query_state oid=%c", + "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( - "load_cell_probe_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", cq=self._cmd_queue) self._home_cmd = self._mcu.lookup_command( - "load_cell_probe_home oid=%c trsync_oid=%c trigger_reason=%c" + "trigger_analog_home oid=%c trsync_oid=%c trigger_reason=%c" " error_reason=%c clock=%u rest_ticks=%u timeout=%u", cq=self._cmd_queue) # the sensor data stream is connected on the MCU at the ready event def _on_connect(self): - self._sensor.attach_load_cell_probe(self._oid) + self._sensor.attach_trigger_analog(self._oid) def get_oid(self): return self._oid @@ -387,30 +387,30 @@ class McuLoadCellProbe: return self._mcu.clock_to_print_time(trigger_ticks) -# Execute probing moves using the McuLoadCellProbe +# Execute probing moves using the MCU_trigger_analog class LoadCellProbingMove: ERROR_MAP = { mcu.MCU_trsync.REASON_COMMS_TIMEOUT: "Communication timeout during " "homing", - McuLoadCellProbe.ERROR_SAFETY_RANGE: "Load Cell Probe Error: load " - "exceeds safety limit", - McuLoadCellProbe.ERROR_OVERFLOW: "Load Cell Probe Error: fixed point " - "math overflow", - McuLoadCellProbe.ERROR_WATCHDOG: "Load Cell Probe Error: timed out " - "waiting for sensor data" + MCU_trigger_analog.ERROR_SAFETY_RANGE: "Load Cell Probe Error: load " + "exceeds safety limit", + MCU_trigger_analog.ERROR_OVERFLOW: "Load Cell Probe Error: fixed point " + "math overflow", + MCU_trigger_analog.ERROR_WATCHDOG: "Load Cell Probe Error: timed out " + "waiting for sensor data" } - def __init__(self, config, mcu_load_cell_probe, param_helper, + def __init__(self, config, mcu_trigger_analog, param_helper, continuous_tare_filter_helper, config_helper): self._printer = config.get_printer() - self._mcu_load_cell_probe = mcu_load_cell_probe + self._mcu_trigger_analog = mcu_trigger_analog self._param_helper = param_helper self._continuous_tare_filter_helper = continuous_tare_filter_helper self._config_helper = config_helper - self._mcu = mcu_load_cell_probe.get_mcu() - self._load_cell = mcu_load_cell_probe.get_load_cell() + self._mcu = mcu_trigger_analog.get_mcu() + self._load_cell = mcu_trigger_analog.get_load_cell() self._z_min_position = probe.lookup_minimum_z(config) - self._dispatch = mcu_load_cell_probe.get_dispatch() + self._dispatch = mcu_trigger_analog.get_dispatch() probe.LookupZSteppers(config, self._dispatch.add_stepper) # internal state tracking self._tare_counts = 0 @@ -436,12 +436,12 @@ class LoadCellProbingMove: tare_counts = np.average(np.array(tare_samples)[:, 2].astype(float)) # update sos_filter with any gcode parameter changes self._continuous_tare_filter_helper.update_from_command(gcmd) - self._mcu_load_cell_probe.set_endstop_range(tare_counts, gcmd) + self._mcu_trigger_analog.set_endstop_range(tare_counts, gcmd) def _home_start(self, print_time): # start trsync trigger_completion = self._dispatch.start(print_time) - self._mcu_load_cell_probe.home_start(print_time) + self._mcu_trigger_analog.home_start(print_time) return trigger_completion def home_start(self, print_time, sample_time, sample_count, rest_time, @@ -453,7 +453,7 @@ class LoadCellProbingMove: # trigger has happened, now to find out why... res = self._dispatch.stop() # clear the homing state so it stops processing samples - self._last_trigger_time = self._mcu_load_cell_probe.clear_home() + self._last_trigger_time = self._mcu_trigger_analog.clear_home() if self._mcu.is_fileoutput(): self._last_trigger_time = home_end_time if res >= mcu.MCU_trsync.REASON_COMMS_TIMEOUT: @@ -468,7 +468,7 @@ class LoadCellProbingMove: def get_steppers(self): return self._dispatch.get_steppers() - # Probe towards z_min until the load_cell_probe on the MCU triggers + # Probe towards z_min until the trigger_analog on the MCU triggers def probing_move(self, gcmd): # do not permit probing if the load cell is not calibrated if not self._load_cell.is_calibrated(): @@ -627,11 +627,11 @@ class LoadCellPrinterProbe: self._param_helper = probe.ProbeParameterHelper(config) self._cmd_helper = probe.ProbeCommandHelper(config, self) self._probe_offsets = probe.ProbeOffsetsHelper(config) - self._mcu_load_cell_probe = McuLoadCellProbe(config, self._load_cell, + self._mcu_trigger_analog = MCU_trigger_analog(config, self._load_cell, continuous_tare_filter_helper.get_sos_filter(), config_helper, trigger_dispatch) load_cell_probing_move = LoadCellProbingMove(config, - self._mcu_load_cell_probe, self._param_helper, + self._mcu_trigger_analog, self._param_helper, continuous_tare_filter_helper, config_helper) self._tapping_move = TappingMove(config, load_cell_probing_move, config_helper) diff --git a/src/Kconfig b/src/Kconfig index 6740946d8..8013b40f0 100644 --- a/src/Kconfig +++ b/src/Kconfig @@ -177,13 +177,13 @@ 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 WANT_LOAD_CELL_PROBE +config WANT_TRIGGER_ANALOG bool depends on WANT_HX71X || WANT_ADS1220 default y config NEED_SOS_FILTER bool - depends on WANT_LOAD_CELL_PROBE + depends on WANT_TRIGGER_ANALOG default y menu "Optional features (to reduce code size)" depends on HAVE_LIMITED_CODE_SIZE diff --git a/src/Makefile b/src/Makefile index 974204bc5..dc6d4db52 100644 --- a/src/Makefile +++ b/src/Makefile @@ -28,4 +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 +src-$(CONFIG_WANT_TRIGGER_ANALOG) += trigger_analog.c diff --git a/src/load_cell_probe.c b/src/load_cell_probe.c deleted file mode 100644 index 48da77937..000000000 --- a/src/load_cell_probe.c +++ /dev/null @@ -1,298 +0,0 @@ -// 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 ((1L << 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 = 1L << 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 deleted file mode 100644 index e67c16e55..000000000 --- a/src/load_cell_probe.h +++ /dev/null @@ -1,10 +0,0 @@ -#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 diff --git a/src/sensor_ads1220.c b/src/sensor_ads1220.c index f51dc355a..93d52b6ae 100644 --- a/src/sensor_ads1220.c +++ b/src/sensor_ads1220.c @@ -4,16 +4,16 @@ // // This file may be distributed under the terms of the GNU GPLv3 license. +#include +#include "basecmd.h" // oid_alloc #include "board/irq.h" // irq_disable #include "board/gpio.h" // gpio_out_write #include "board/misc.h" // timer_read_time -#include "basecmd.h" // oid_alloc #include "command.h" // DECL_COMMAND #include "sched.h" // sched_add_timer #include "sensor_bulk.h" // sensor_bulk_report -#include "load_cell_probe.h" // load_cell_probe_report_sample #include "spicmds.h" // spidev_transfer -#include +#include "trigger_analog.h" // trigger_analog_update struct ads1220_adc { struct timer timer; @@ -22,7 +22,7 @@ struct ads1220_adc { struct spidev_s *spi; uint8_t pending_flag, data_count; struct sensor_bulk sb; - struct load_cell_probe *lce; + struct trigger_analog *ta; }; // Flag types @@ -97,8 +97,8 @@ ads1220_read_adc(struct ads1220_adc *ads1220, uint8_t oid) counts |= 0xFF000000; // endstop is optional, report if enabled and no errors - if (ads1220->lce) { - load_cell_probe_report_sample(ads1220->lce, counts); + if (ads1220->ta) { + trigger_analog_update(ads1220->ta, counts); } add_sample(ads1220, oid, counts); @@ -119,13 +119,13 @@ DECL_COMMAND(command_config_ads1220, "config_ads1220 oid=%c" " spi_oid=%c data_ready_pin=%u"); void -ads1220_attach_load_cell_probe(uint32_t *args) { +ads1220_attach_trigger_analog(uint32_t *args) { uint8_t oid = args[0]; struct ads1220_adc *ads1220 = oid_lookup(oid, command_config_ads1220); - ads1220->lce = load_cell_probe_oid_lookup(args[1]); + ads1220->ta = trigger_analog_oid_lookup(args[1]); } -DECL_COMMAND(ads1220_attach_load_cell_probe, - "ads1220_attach_load_cell_probe oid=%c load_cell_probe_oid=%c"); +DECL_COMMAND(ads1220_attach_trigger_analog, + "ads1220_attach_trigger_analog oid=%c trigger_analog_oid=%c"); // start/stop capturing ADC data void diff --git a/src/sensor_hx71x.c b/src/sensor_hx71x.c index 74575eec1..2c09e97af 100644 --- a/src/sensor_hx71x.c +++ b/src/sensor_hx71x.c @@ -4,17 +4,17 @@ // // This file may be distributed under the terms of the GNU GPLv3 license. +#include +#include #include "autoconf.h" // CONFIG_MACH_AVR +#include "basecmd.h" // oid_alloc #include "board/gpio.h" // gpio_out_write #include "board/irq.h" // irq_poll #include "board/misc.h" // timer_read_time -#include "basecmd.h" // oid_alloc #include "command.h" // DECL_COMMAND #include "sched.h" // sched_add_timer #include "sensor_bulk.h" // sensor_bulk_report -#include "load_cell_probe.h" // load_cell_probe_report_sample -#include -#include +#include "trigger_analog.h" // trigger_analog_update struct hx71x_adc { struct timer timer; @@ -25,7 +25,7 @@ struct hx71x_adc { struct gpio_in dout; // pin used to receive data from the hx71x struct gpio_out sclk; // pin used to generate clock for the hx71x struct sensor_bulk sb; - struct load_cell_probe *lce; + struct trigger_analog *ta; }; enum { @@ -178,8 +178,8 @@ hx71x_read_adc(struct hx71x_adc *hx71x, uint8_t oid) } // probe is optional, report if enabled - if (hx71x->last_error == 0 && hx71x->lce) { - load_cell_probe_report_sample(hx71x->lce, counts); + if (hx71x->last_error == 0 && hx71x->ta) { + trigger_analog_update(hx71x->ta, counts); } // Add measurement to buffer @@ -206,13 +206,13 @@ DECL_COMMAND(command_config_hx71x, "config_hx71x oid=%c gain_channel=%c" " dout_pin=%u sclk_pin=%u"); void -hx71x_attach_load_cell_probe(uint32_t *args) { +hx71x_attach_trigger_analog(uint32_t *args) { uint8_t oid = args[0]; struct hx71x_adc *hx71x = oid_lookup(oid, command_config_hx71x); - hx71x->lce = load_cell_probe_oid_lookup(args[1]); + hx71x->ta = trigger_analog_oid_lookup(args[1]); } -DECL_COMMAND(hx71x_attach_load_cell_probe, "hx71x_attach_load_cell_probe oid=%c" - " load_cell_probe_oid=%c"); +DECL_COMMAND(hx71x_attach_trigger_analog, "hx71x_attach_trigger_analog oid=%c" + " trigger_analog_oid=%c"); // start/stop capturing ADC data void diff --git a/src/trigger_analog.c b/src/trigger_analog.c new file mode 100644 index 000000000..9c0220e19 --- /dev/null +++ b/src/trigger_analog.c @@ -0,0 +1,294 @@ +// Support homing/probing "trigger" notification from analog sensors +// +// Copyright (C) 2025 Gareth Farrington +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include // abs +#include "basecmd.h" // oid_alloc +#include "board/misc.h" // timer_read_time +#include "command.h" // DECL_COMMAND +#include "sched.h" // shutdown +#include "sos_filter.h" // fixedQ12_t +#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 + +// Flags +enum {FLAG_IS_HOMING = 1 << 0 + , FLAG_IS_HOMING_TRIGGER = 1 << 1 + , FLAG_AWAIT_HOMING = 1 << 2 +}; + +// Endstop Structure +struct trigger_analog { + 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 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) +{ + return !!(mask & ta->flags); +} + +static inline void +set_flag(uint8_t mask, struct trigger_analog *ta) +{ + ta->flags |= mask; +} + +static inline void +clear_flag(uint8_t mask, struct trigger_analog *ta) +{ + ta->flags &= ~mask; +} + +void +try_trigger(struct trigger_analog *ta, uint32_t ticks) +{ + uint8_t is_homing_triggered = is_flag_set(FLAG_IS_HOMING_TRIGGER, ta); + if (!is_homing_triggered) { + // the first triggering sample when homing sets the trigger time + ta->trigger_ticks = ticks; + // this flag latches until a reset, disabling further triggering + set_flag(FLAG_IS_HOMING_TRIGGER, ta); + trsync_do_trigger(ta->ts, ta->trigger_reason); + } +} + +void +trigger_error(struct trigger_analog *ta, uint8_t error_code) +{ + trsync_do_trigger(ta->ts, ta->error_reason + error_code); +} + +// Used by Sensors to report new raw ADC sample +void +trigger_analog_update(struct trigger_analog *ta, const int32_t sample) +{ + // only process samples when homing + uint8_t is_homing = is_flag_set(FLAG_IS_HOMING, ta); + if (!is_homing) { + return; + } + + // save new sample + uint32_t ticks = timer_read_time(); + ta->last_sample_ticks = ticks; + ta->watchdog_count = 0; + + // do not trigger before homing start time + uint8_t await_homing = is_flag_set(FLAG_AWAIT_HOMING, ta); + if (await_homing && timer_is_before(ticks, ta->homing_start_time)) { + return; + } + clear_flag(FLAG_AWAIT_HOMING, ta); + + // check for safety limit violations + const uint8_t is_safety_trigger = sample <= ta->safety_counts_min + || sample >= ta->safety_counts_max; + // too much force, this is an error while homing + if (is_safety_trigger) { + trigger_error(ta, ERROR_SAFETY_RANGE); + 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 + const fixedQ16_t filtered_grams = sosfilt(ta->sf, (fixedQ16_t)raw_grams); + + // update trigger state + if (abs(filtered_grams) >= ta->trigger_grams_fixed) { + try_trigger(ta, ta->last_sample_ticks); + } +} + +// Timer callback that monitors for timeouts +static uint_fast8_t +watchdog_event(struct timer *t) +{ + struct trigger_analog *ta = container_of(t, struct trigger_analog, time); + uint8_t is_homing = is_flag_set(FLAG_IS_HOMING, ta); + uint8_t is_homing_trigger = is_flag_set(FLAG_IS_HOMING_TRIGGER, ta); + // the watchdog stops when not homing or when trsync becomes triggered + if (!is_homing || is_homing_trigger) { + return SF_DONE; + } + + if (ta->watchdog_count > ta->watchdog_max) { + trigger_error(ta, ERROR_WATCHDOG); + } + ta->watchdog_count += 1; + + // A sample was recently delivered, continue monitoring + ta->time.waketime += ta->rest_ticks; + 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"); + +// Lookup a trigger_analog +struct trigger_analog * +trigger_analog_oid_lookup(uint8_t oid) +{ + return oid_lookup(oid, command_config_trigger_analog); +} + +// Set the triggering range and tare value +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]); +} +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"); + +// Home an axis +void +command_trigger_analog_home(uint32_t *args) +{ + struct trigger_analog *ta = trigger_analog_oid_lookup(args[0]); + sched_del_timer(&ta->time); + // clear the homing trigger flag + clear_flag(FLAG_IS_HOMING_TRIGGER, ta); + clear_flag(FLAG_IS_HOMING, ta); + ta->trigger_ticks = 0; + ta->ts = NULL; + // 0 samples indicates homing is finished + if (args[3] == 0) { + // Disable end stop checking + return; + } + ta->ts = trsync_oid_lookup(args[1]); + ta->trigger_reason = args[2]; + ta->error_reason = args[3]; + ta->time.waketime = args[4]; + ta->homing_start_time = args[4]; + ta->rest_ticks = args[5]; + ta->watchdog_max = args[6]; + ta->watchdog_count = 0; + ta->time.func = watchdog_event; + set_flag(FLAG_IS_HOMING, ta); + set_flag(FLAG_AWAIT_HOMING, ta); + sched_add_timer(&ta->time); +} +DECL_COMMAND(command_trigger_analog_home, + "trigger_analog_home oid=%c trsync_oid=%c trigger_reason=%c" + " error_reason=%c clock=%u rest_ticks=%u timeout=%u"); + +void +command_trigger_analog_query_state(uint32_t *args) +{ + uint8_t oid = args[0]; + struct trigger_analog *ta = trigger_analog_oid_lookup(args[0]); + sendf("trigger_analog_state oid=%c is_homing_trigger=%c trigger_ticks=%u" + , oid + , is_flag_set(FLAG_IS_HOMING_TRIGGER, ta) + , ta->trigger_ticks); +} +DECL_COMMAND(command_trigger_analog_query_state + , "trigger_analog_query_state oid=%c"); diff --git a/src/trigger_analog.h b/src/trigger_analog.h new file mode 100644 index 000000000..9867095e8 --- /dev/null +++ b/src/trigger_analog.h @@ -0,0 +1,9 @@ +#ifndef __TRIGGER_ANALOG_H +#define __TRIGGER_ANALOG_H + +#include // uint8_t + +struct trigger_analog *trigger_analog_oid_lookup(uint8_t oid); +void trigger_analog_update(struct trigger_analog *ta, int32_t sample); + +#endif // trigger_analog.h