From 05c2d51a1239a13a7dfeec9cfbb1ea38b7b32aa0 Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Thu, 4 Feb 2021 10:07:13 -0500 Subject: [PATCH] trsync: Introduce new "trigger synchronization" support Separate out the stepper stopping code from endstop.c into its own trsync.c code file. Signed-off-by: Kevin O'Connor --- klippy/mcu.py | 67 +++++++++-------- src/Makefile | 3 +- src/endstop.c | 89 +++++----------------- src/stepper.c | 29 ++++++-- src/stepper.h | 2 - src/trsync.c | 200 ++++++++++++++++++++++++++++++++++++++++++++++++++ src/trsync.h | 19 +++++ 7 files changed, 299 insertions(+), 110 deletions(-) create mode 100644 src/trsync.c create mode 100644 src/trsync.h diff --git a/klippy/mcu.py b/klippy/mcu.py index 3f88c3e5..dc0011c9 100644 --- a/klippy/mcu.py +++ b/klippy/mcu.py @@ -18,10 +18,10 @@ class MCU_endstop: self._pullup = pin_params['pullup'] self._invert = pin_params['invert'] self._reactor = mcu.get_printer().get_reactor() - self._oid = self._home_cmd = self._requery_cmd = self._query_cmd = None + self._oid = self._home_cmd = self._query_cmd = None + self._ts_oid = self._trsync_start_cmd = self._trsync_trigger_cmd = None self._mcu.register_config_callback(self._build_config) - self._min_query_time = self._last_sent_time = 0. - self._next_query_print_time = self._end_home_time = 0. + self._min_query_time = self._last_sent_time = self._end_home_time = 0. self._trigger_completion = self._home_completion = None def get_mcu(self): return self._mcu @@ -34,23 +34,31 @@ class MCU_endstop: def get_steppers(self): return list(self._steppers) def _build_config(self): + # Setup config + self._ts_oid = self._mcu.create_oid() + self._mcu.add_config_cmd("config_trsync oid=%d" % (self._ts_oid,)) + self._mcu.add_config_cmd("trsync_trigger oid=%d reason=0" + % (self._ts_oid,), on_restart=True) self._oid = self._mcu.create_oid() - self._mcu.add_config_cmd( - "config_endstop oid=%d pin=%s pull_up=%d stepper_count=%d" % ( - self._oid, self._pin, self._pullup, len(self._steppers))) + self._mcu.add_config_cmd("config_endstop oid=%d pin=%s pull_up=%d" + % (self._oid, self._pin, self._pullup)) self._mcu.add_config_cmd( "endstop_home oid=%d clock=0 sample_ticks=0 sample_count=0" - " rest_ticks=0 pin_value=0" % (self._oid,), on_restart=True) - for i, s in enumerate(self._steppers): - self._mcu.add_config_cmd( - "endstop_set_stepper oid=%d pos=%d stepper_oid=%d" % ( - self._oid, i, s.get_oid()), is_init=True) + " rest_ticks=0 pin_value=0 trsync_oid=0 trigger_reason=0" + % (self._oid,), on_restart=True) + # Lookup commands cmd_queue = self._mcu.alloc_command_queue() + self._trsync_start_cmd = self._mcu.lookup_command( + "trsync_start oid=%c report_clock=%u report_ticks=%u" + " expire_reason=%c", cq=cmd_queue) + self._trsync_trigger_cmd = self._mcu.lookup_command( + "trsync_trigger oid=%c reason=%c", cq=cmd_queue) + self._stepper_stop_cmd = self._mcu.lookup_command( + "stepper_stop_on_trigger oid=%c trsync_oid=%c", cq=cmd_queue) self._home_cmd = self._mcu.lookup_command( "endstop_home oid=%c clock=%u sample_ticks=%u sample_count=%c" - " rest_ticks=%u pin_value=%c", cq=cmd_queue) - self._requery_cmd = self._mcu.lookup_command( - "endstop_query_state oid=%c", cq=cmd_queue) + " rest_ticks=%u pin_value=%c trsync_oid=%c trigger_reason=%c", + cq=cmd_queue) self._query_cmd = self._mcu.lookup_query_command( "endstop_query_state oid=%c", "endstop_state oid=%c homing=%c next_clock=%u pin_value=%c", @@ -59,24 +67,28 @@ class MCU_endstop: triggered=True): clock = self._mcu.print_time_to_clock(print_time) rest_ticks = self._mcu.print_time_to_clock(print_time+rest_time) - clock - self._next_query_print_time = print_time + self.RETRY_QUERY self._min_query_time = self._reactor.monotonic() self._last_sent_time = 0. self._home_end_time = self._reactor.NEVER self._trigger_completion = self._reactor.completion() - self._mcu.register_response(self._handle_endstop_state, - "endstop_state", self._oid) + self._mcu.register_response(self._handle_trsync_state, + "trsync_state", self._ts_oid) + report_ticks = self._mcu.seconds_to_clock(0.100) + self._trsync_start_cmd.send([self._ts_oid, clock, report_ticks, 0], + reqclock=clock) + for s in self._steppers: + self._stepper_stop_cmd.send([s.get_oid(), self._ts_oid]) self._home_cmd.send( [self._oid, clock, self._mcu.seconds_to_clock(sample_time), - sample_count, rest_ticks, triggered ^ self._invert], - reqclock=clock) + sample_count, rest_ticks, triggered ^ self._invert, + self._ts_oid, 0], reqclock=clock) self._home_completion = self._reactor.register_callback( self._home_retry) return self._trigger_completion - def _handle_endstop_state(self, params): - logging.debug("endstop_state %s", params) + def _handle_trsync_state(self, params): + logging.debug("trsync_state %s", params) if params['#sent_time'] >= self._min_query_time: - if params['homing']: + if params['can_trigger']: self._last_sent_time = params['#sent_time'] else: self._min_query_time = self._reactor.NEVER @@ -93,17 +105,12 @@ class MCU_endstop: last = self._mcu.estimated_print_time(self._last_sent_time) if last > self._home_end_time or self._mcu.is_shutdown(): return False - # Check for resend - eventtime = self._reactor.monotonic() - est_print_time = self._mcu.estimated_print_time(eventtime) - if est_print_time >= self._next_query_print_time: - self._next_query_print_time = est_print_time + self.RETRY_QUERY - self._requery_cmd.send([self._oid]) def home_wait(self, home_end_time): self._home_end_time = home_end_time did_trigger = self._home_completion.wait() - self._mcu.register_response(None, "endstop_state", self._oid) - self._home_cmd.send([self._oid, 0, 0, 0, 0, 0]) + self._trsync_trigger_cmd.send([self._ts_oid, 0]) + self._mcu.register_response(None, "trsync_state", self._ts_oid) + self._home_cmd.send([self._oid, 0, 0, 0, 0, 0, 0, 0]) for s in self._steppers: s.note_homing_end(did_trigger=did_trigger) if not self._trigger_completion.test(): diff --git a/src/Makefile b/src/Makefile index 2f2ab521..50f893c7 100644 --- a/src/Makefile +++ b/src/Makefile @@ -1,7 +1,8 @@ # Main code build rules src-y += sched.c command.c basecmd.c debugcmds.c -src-$(CONFIG_HAVE_GPIO) += initial_pins.c gpiocmds.c stepper.c endstop.c +src-$(CONFIG_HAVE_GPIO) += initial_pins.c gpiocmds.c stepper.c endstop.c \ + trsync.c src-$(CONFIG_HAVE_GPIO_ADC) += adccmds.c src-$(CONFIG_HAVE_GPIO_SPI) += spicmds.c thermocouple.c src-$(CONFIG_HAVE_GPIO_I2C) += i2ccmds.c diff --git a/src/endstop.c b/src/endstop.c index 96f8e805..77915398 100644 --- a/src/endstop.c +++ b/src/endstop.c @@ -1,6 +1,6 @@ // Handling of end stops. // -// Copyright (C) 2016-2019 Kevin O'Connor +// Copyright (C) 2016-2021 Kevin O'Connor // // This file may be distributed under the terms of the GNU GPLv3 license. @@ -9,30 +9,17 @@ #include "board/irq.h" // irq_disable #include "command.h" // DECL_COMMAND #include "sched.h" // struct timer -#include "stepper.h" // stepper_stop +#include "trsync.h" // trsync_do_trigger struct endstop { struct timer time; struct gpio_in pin; uint32_t rest_time, sample_time, nextwake; - uint8_t flags, stepper_count, sample_count, trigger_count; - struct stepper *steppers[0]; + struct trsync *ts; + uint8_t flags, sample_count, trigger_count, trigger_reason; }; -enum { ESF_PIN_HIGH=1<<0, ESF_HOMING=1<<1, ESF_REPORT=1<<2 }; - -static struct task_wake endstop_wake; - -static void -stop_steppers(struct endstop *e) -{ - e->flags = ESF_REPORT; - uint8_t count = e->stepper_count; - while (count--) - if (e->steppers[count]) - stepper_stop(e->steppers[count]); - sched_wake_task(&endstop_wake); -} +enum { ESF_PIN_HIGH=1<<0, ESF_HOMING=1<<1 }; static uint_fast8_t endstop_oversample_event(struct timer *t); @@ -68,7 +55,7 @@ endstop_oversample_event(struct timer *t) } uint8_t count = e->trigger_count - 1; if (!count) { - stop_steppers(e); + trsync_do_trigger(e->ts, e->trigger_reason); return SF_DONE; } e->trigger_count = count; @@ -79,28 +66,10 @@ endstop_oversample_event(struct timer *t) void command_config_endstop(uint32_t *args) { - uint8_t stepper_count = args[3]; - struct endstop *e = oid_alloc( - args[0], command_config_endstop - , sizeof(*e) + sizeof(e->steppers[0]) * stepper_count); + struct endstop *e = oid_alloc(args[0], command_config_endstop, sizeof(*e)); e->pin = gpio_in_setup(args[1], args[2]); - e->stepper_count = stepper_count; - e->sample_count = 1; } -DECL_COMMAND(command_config_endstop, - "config_endstop oid=%c pin=%c pull_up=%c stepper_count=%c"); - -void -command_endstop_set_stepper(uint32_t *args) -{ - struct endstop *e = oid_lookup(args[0], command_config_endstop); - uint8_t pos = args[1]; - if (pos >= e->stepper_count) - shutdown("Set stepper past maximum stepper count"); - e->steppers[pos] = stepper_oid_lookup(args[2]); -} -DECL_COMMAND(command_endstop_set_stepper, - "endstop_set_stepper oid=%c pos=%c stepper_oid=%c"); +DECL_COMMAND(command_config_endstop, "config_endstop oid=%c pin=%c pull_up=%c"); // Home an axis void @@ -113,6 +82,7 @@ command_endstop_home(uint32_t *args) e->sample_count = args[3]; if (!e->sample_count) { // Disable end stop checking + e->ts = NULL; e->flags = 0; return; } @@ -120,45 +90,26 @@ command_endstop_home(uint32_t *args) e->time.func = endstop_event; e->trigger_count = e->sample_count; e->flags = ESF_HOMING | (args[5] ? ESF_PIN_HIGH : 0); + e->ts = trsync_oid_lookup(args[6]); + e->trigger_reason = args[7]; sched_add_timer(&e->time); } DECL_COMMAND(command_endstop_home, "endstop_home oid=%c clock=%u sample_ticks=%u sample_count=%c" - " rest_ticks=%u pin_value=%c"); - -static void -endstop_report(uint8_t oid, struct endstop *e) -{ - irq_disable(); - uint8_t eflags = e->flags; - e->flags &= ~ESF_REPORT; - uint32_t nextwake = e->nextwake; - irq_enable(); - - sendf("endstop_state oid=%c homing=%c next_clock=%u pin_value=%c" - , oid, !!(eflags & ESF_HOMING), nextwake, gpio_in_read(e->pin)); -} + " rest_ticks=%u pin_value=%c trsync_oid=%c trigger_reason=%c"); void command_endstop_query_state(uint32_t *args) { uint8_t oid = args[0]; struct endstop *e = oid_lookup(oid, command_config_endstop); - endstop_report(oid, e); + + irq_disable(); + uint8_t eflags = e->flags; + uint32_t nextwake = e->nextwake; + irq_enable(); + + sendf("endstop_state oid=%c homing=%c next_clock=%u pin_value=%c" + , oid, !!(eflags & ESF_HOMING), nextwake, gpio_in_read(e->pin)); } DECL_COMMAND(command_endstop_query_state, "endstop_query_state oid=%c"); - -void -endstop_task(void) -{ - if (!sched_check_wake(&endstop_wake)) - return; - uint8_t oid; - struct endstop *e; - foreach_oid(oid, e, command_config_endstop) { - if (!(e->flags & ESF_REPORT)) - continue; - endstop_report(oid, e); - } -} -DECL_TASK(endstop_task); diff --git a/src/stepper.c b/src/stepper.c index 83da362d..8c822d66 100644 --- a/src/stepper.c +++ b/src/stepper.c @@ -1,6 +1,6 @@ // Handling of stepper drivers. // -// Copyright (C) 2016-2019 Kevin O'Connor +// Copyright (C) 2016-2021 Kevin O'Connor // // This file may be distributed under the terms of the GNU GPLv3 license. @@ -11,7 +11,8 @@ #include "board/misc.h" // timer_is_before #include "command.h" // DECL_COMMAND #include "sched.h" // struct timer -#include "stepper.h" // command_config_stepper +#include "stepper.h" // stepper_event +#include "trsync.h" // trsync_add_signal DECL_CONSTANT("STEP_DELAY", CONFIG_STEP_DELAY); @@ -44,6 +45,7 @@ struct stepper { struct gpio_out step_pin, dir_pin; uint32_t position; struct move_queue_head mq; + struct trsync_signal stop_signal; // gcc (pre v6) does better optimization when uint8_t are bitfields uint8_t flags : 8; }; @@ -192,7 +194,7 @@ DECL_COMMAND(command_config_stepper, "config_stepper oid=%c step_pin=%c dir_pin=%c invert_step=%c"); // Return the 'struct stepper' for a given stepper oid -struct stepper * +static struct stepper * stepper_oid_lookup(uint8_t oid) { return oid_lookup(oid, command_config_stepper); @@ -290,11 +292,11 @@ command_stepper_get_position(uint32_t *args) } DECL_COMMAND(command_stepper_get_position, "stepper_get_position oid=%c"); -// Stop all moves for a given stepper (used in end stop homing). IRQs -// must be off. -void -stepper_stop(struct stepper *s) +// Stop all moves for a given stepper (caller must disable IRQs) +static void +stepper_stop(struct trsync_signal *tss, uint8_t reason) { + struct stepper *s = container_of(tss, struct stepper, stop_signal); sched_del_timer(&s->time); s->next_step_time = 0; s->position = -stepper_get_position(s); @@ -309,6 +311,17 @@ stepper_stop(struct stepper *s) } } +// Set the stepper to stop on a "trigger event" (used in homing) +void +command_stepper_stop_on_trigger(uint32_t *args) +{ + struct stepper *s = stepper_oid_lookup(args[0]); + struct trsync *ts = trsync_oid_lookup(args[1]); + trsync_add_signal(ts, &s->stop_signal, stepper_stop); +} +DECL_COMMAND(command_stepper_stop_on_trigger, + "stepper_stop_on_trigger oid=%c trsync_oid=%c"); + void stepper_shutdown(void) { @@ -316,7 +329,7 @@ stepper_shutdown(void) struct stepper *s; foreach_oid(i, s, command_config_stepper) { move_queue_clear(&s->mq); - stepper_stop(s); + stepper_stop(&s->stop_signal, 0); } } DECL_SHUTDOWN(stepper_shutdown); diff --git a/src/stepper.h b/src/stepper.h index 8e27b789..db29eed0 100644 --- a/src/stepper.h +++ b/src/stepper.h @@ -4,7 +4,5 @@ #include // uint8_t uint_fast8_t stepper_event(struct timer *t); -struct stepper *stepper_oid_lookup(uint8_t oid); -void stepper_stop(struct stepper *s); #endif // stepper.h diff --git a/src/trsync.c b/src/trsync.c new file mode 100644 index 00000000..3bf7aa66 --- /dev/null +++ b/src/trsync.c @@ -0,0 +1,200 @@ +// Handling of synchronized "trigger" dispatch +// +// Copyright (C) 2016-2021 Kevin O'Connor +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include "basecmd.h" // oid_alloc +#include "board/gpio.h" // struct gpio +#include "board/irq.h" // irq_disable +#include "board/misc.h" // timer_read_time +#include "command.h" // DECL_COMMAND +#include "sched.h" // struct timer +#include "trsync.h" // trsync_do_trigger + +struct trsync { + struct timer report_time, expire_time; + uint32_t report_ticks; + struct trsync_signal *signals; + uint8_t flags, trigger_reason, expire_reason; +}; + +enum { TSF_CAN_TRIGGER=1<<0, TSF_REPORT=1<<2 }; + +static struct task_wake trsync_wake; + +// Activate a trigger (caller must disable IRQs) +void +trsync_do_trigger(struct trsync *ts, uint8_t reason) +{ + uint8_t flags = ts->flags; + if (!(flags & TSF_CAN_TRIGGER)) + return; + ts->trigger_reason = reason; + ts->flags = (flags & ~TSF_CAN_TRIGGER) | TSF_REPORT; + // Dispatch signals + while (ts->signals) { + struct trsync_signal *tss = ts->signals; + trsync_callback_t func = tss->func; + ts->signals = tss->next; + tss->next = NULL; + tss->func = NULL; + func(tss, reason); + } + sched_wake_task(&trsync_wake); +} + +// Timeout handler +static uint_fast8_t +trsync_expire_event(struct timer *t) +{ + struct trsync *ts = container_of(t, struct trsync, expire_time); + trsync_do_trigger(ts, ts->expire_reason); + return SF_DONE; +} + +// Report handler +static uint_fast8_t +trsync_report_event(struct timer *t) +{ + struct trsync *ts = container_of(t, struct trsync, report_time); + ts->flags |= TSF_REPORT; + sched_wake_task(&trsync_wake); + ts->report_time.waketime += ts->report_ticks; + return SF_RESCHEDULE; +} + +void +command_config_trsync(uint32_t *args) +{ + struct trsync *ts = oid_alloc(args[0], command_config_trsync, sizeof(*ts)); + ts->report_time.func = trsync_report_event; + ts->expire_time.func = trsync_expire_event; +} +DECL_COMMAND(command_config_trsync, "config_trsync oid=%c"); + +// Return the 'struct trsync' for a given trsync oid +struct trsync * +trsync_oid_lookup(uint8_t oid) +{ + return oid_lookup(oid, command_config_trsync); +} + +// Add a callback to invoke on a trigger +void +trsync_add_signal(struct trsync *ts, struct trsync_signal *tss + , trsync_callback_t func) +{ + irqstatus_t flag = irq_save(); + if (tss->func || !func) + shutdown("Can't add signal that is already active"); + tss->func = func; + tss->next = ts->signals; + ts->signals = tss; + irq_restore(flag); +} + +// Disable trigger and unregister any signal handlers (caller must disable IRQs) +static void +trsync_clear(struct trsync *ts) +{ + sched_del_timer(&ts->report_time); + sched_del_timer(&ts->expire_time); + struct trsync_signal *tss = ts->signals; + while (tss) { + struct trsync_signal *next = tss->next; + tss->func = NULL; + tss->next = NULL; + tss = next; + } + ts->signals = NULL; + ts->flags = ts->trigger_reason = ts->expire_reason = 0; +} + +void +command_trsync_start(uint32_t *args) +{ + struct trsync *ts = trsync_oid_lookup(args[0]); + irq_disable(); + trsync_clear(ts); + ts->flags = TSF_CAN_TRIGGER; + ts->report_time.waketime = args[1]; + ts->report_ticks = args[2]; + if (ts->report_ticks) + sched_add_timer(&ts->report_time); + ts->expire_reason = args[3]; + irq_enable(); +} +DECL_COMMAND(command_trsync_start, + "trsync_start oid=%c report_clock=%u report_ticks=%u" + " expire_reason=%c"); + +void +command_trsync_set_timeout(uint32_t *args) +{ + struct trsync *ts = trsync_oid_lookup(args[0]); + irq_disable(); + uint8_t flags = ts->flags; + if (flags & TSF_CAN_TRIGGER) { + sched_del_timer(&ts->expire_time); + ts->expire_time.waketime = args[1]; + sched_add_timer(&ts->expire_time); + } + irq_enable(); +} +DECL_COMMAND(command_trsync_set_timeout, "trsync_set_timeout oid=%c clock=%u"); + +static void +trsync_report(uint8_t oid, uint8_t flags, uint8_t reason, uint32_t clock) +{ + sendf("trsync_state oid=%c can_trigger=%c trigger_reason=%c clock=%u" + , oid, !!(flags & TSF_CAN_TRIGGER), reason, clock); +} + +void +command_trsync_trigger(uint32_t *args) +{ + uint8_t oid = args[0]; + struct trsync *ts = trsync_oid_lookup(oid); + irq_disable(); + trsync_do_trigger(ts, args[1]); + sched_del_timer(&ts->report_time); + sched_del_timer(&ts->expire_time); + ts->flags = 0; + uint8_t trigger_reason = ts->trigger_reason; + irq_enable(); + trsync_report(oid, 0, trigger_reason, 0); +} +DECL_COMMAND(command_trsync_trigger, "trsync_trigger oid=%c reason=%c"); + +void +trsync_task(void) +{ + if (!sched_check_wake(&trsync_wake)) + return; + uint8_t oid; + struct trsync *ts; + foreach_oid(oid, ts, command_config_trsync) { + if (!(ts->flags & TSF_REPORT)) + continue; + uint32_t time = timer_read_time(); + irq_disable(); + uint8_t trigger_reason = ts->trigger_reason, flags = ts->flags; + ts->flags = flags & ~TSF_REPORT; + irq_enable(); + + trsync_report(oid, flags, trigger_reason, time); + } +} +DECL_TASK(trsync_task); + +void +trsync_shutdown(void) +{ + uint8_t oid; + struct trsync *ts; + foreach_oid(oid, ts, command_config_trsync) { + trsync_clear(ts); + } +} +DECL_SHUTDOWN(trsync_shutdown); diff --git a/src/trsync.h b/src/trsync.h new file mode 100644 index 00000000..c7cc9b54 --- /dev/null +++ b/src/trsync.h @@ -0,0 +1,19 @@ +#ifndef __TRSYNC_H +#define __TRSYNC_H + +#include // uint16_t + +struct trsync_signal; +typedef void (*trsync_callback_t)(struct trsync_signal *tss, uint8_t reason); + +struct trsync_signal { + struct trsync_signal *next; + trsync_callback_t func; +}; + +struct trsync *trsync_oid_lookup(uint8_t oid); +void trsync_do_trigger(struct trsync *ts, uint8_t reason); +void trsync_add_signal(struct trsync *ts, struct trsync_signal *tss + , trsync_callback_t func); + +#endif // trsync.h