From e8ec1801ffd41be4a79e0871d43b9e084074151b Mon Sep 17 00:00:00 2001 From: Pascal Pieper Date: Fri, 20 Nov 2020 21:44:35 -0500 Subject: [PATCH] pwmcmds: Use move queue for hard PWM Signed-off-by: Pascal Pieper Signed-off-by: Kevin O'Connor --- docs/MCU_Commands.md | 4 +-- klippy/mcu.py | 5 ++-- src/pwmcmds.c | 67 ++++++++++++++++++++++++++++++++++++-------- 3 files changed, 61 insertions(+), 15 deletions(-) diff --git a/docs/MCU_Commands.md b/docs/MCU_Commands.md index 133476d8..536d1ce4 100644 --- a/docs/MCU_Commands.md +++ b/docs/MCU_Commands.md @@ -197,8 +197,8 @@ only of interest to developers looking to gain insight into Klipper. same 'oid' parameter must have been issued during micro-controller configuration. -* `schedule_pwm_out oid=%c clock=%u value=%hu` : Schedules a change to - a hardware PWM output pin. See the 'schedule_digital_out' and +* `queue_pwm_out oid=%c clock=%u value=%hu` : Schedules a change to a + hardware PWM output pin. See the 'schedule_digital_out' and 'config_pwm_out' commands for more info. * `schedule_soft_pwm_out oid=%c clock=%u on_ticks=%u` : Schedules a diff --git a/klippy/mcu.py b/klippy/mcu.py index 0cd6a22e..2bf3200c 100644 --- a/klippy/mcu.py +++ b/klippy/mcu.py @@ -208,6 +208,7 @@ class MCU_pwm: % (self._pin, cycle_ticks, self._start_value * self._pwm_max)) return + self._mcu.request_move_queue_slot() self._oid = self._mcu.create_oid() self._mcu.add_config_cmd( "config_pwm_out oid=%d pin=%s cycle_ticks=%d value=%d" @@ -217,11 +218,11 @@ class MCU_pwm: self._shutdown_value * self._pwm_max, self._mcu.seconds_to_clock(self._max_duration))) svalue = int(self._start_value * self._pwm_max + 0.5) - self._mcu.add_config_cmd("schedule_pwm_out oid=%d clock=%d value=%d" + self._mcu.add_config_cmd("queue_pwm_out oid=%d clock=%d value=%d" % (self._oid, self._last_clock, svalue), on_restart=True) self._set_cmd = self._mcu.lookup_command( - "schedule_pwm_out oid=%c clock=%u value=%hu", cq=cmd_queue) + "queue_pwm_out oid=%c clock=%u value=%hu", cq=cmd_queue) return # Software PWM if self._shutdown_value not in [0., 1.]: diff --git a/src/pwmcmds.c b/src/pwmcmds.c index fd380f8b..d214a95a 100644 --- a/src/pwmcmds.c +++ b/src/pwmcmds.c @@ -6,6 +6,8 @@ #include "basecmd.h" // oid_alloc #include "board/gpio.h" // struct gpio_pwm +#include "board/irq.h" // irq_disable +#include "board/misc.h" // timer_is_before #include "command.h" // DECL_COMMAND #include "sched.h" // sched_add_timer @@ -13,7 +15,14 @@ struct pwm_out_s { struct timer timer; struct gpio_pwm pin; uint32_t max_duration; - uint16_t value, default_value; + uint16_t default_value; + struct move_queue_head mq; +}; + +struct pwm_move { + struct move_node node; + uint32_t waketime; + uint16_t value; }; static uint_fast8_t @@ -25,12 +34,32 @@ pwm_end_event(struct timer *timer) static uint_fast8_t pwm_event(struct timer *timer) { + // Apply next update and remove it from queue struct pwm_out_s *p = container_of(timer, struct pwm_out_s, timer); - gpio_pwm_write(p->pin, p->value); - if (p->value == p->default_value || !p->max_duration) - return SF_DONE; - p->timer.waketime += p->max_duration; - p->timer.func = pwm_end_event; + struct move_node *mn = move_queue_pop(&p->mq); + struct pwm_move *m = container_of(mn, struct pwm_move, node); + uint16_t value = m->value; + gpio_pwm_write(p->pin, value); + move_free(m); + + // Check if more updates queued + if (move_queue_empty(&p->mq)) { + if (value == p->default_value || !p->max_duration) + return SF_DONE; + + // Start the safety timeout + p->timer.waketime += p->max_duration; + p->timer.func = pwm_end_event; + return SF_RESCHEDULE; + } + + // Schedule next update + struct move_node *nn = move_queue_first(&p->mq); + uint32_t wake = container_of(nn, struct pwm_move, node)->waketime; + if (value != p->default_value && p->max_duration + && timer_is_before(p->timer.waketime + p->max_duration, wake)) + shutdown("Scheduled pwm event will exceed max_duration"); + p->timer.waketime = wake; return SF_RESCHEDULE; } @@ -43,23 +72,37 @@ command_config_pwm_out(uint32_t *args) p->pin = pin; p->default_value = args[4]; p->max_duration = args[5]; + p->timer.func = pwm_event; + move_queue_setup(&p->mq, sizeof(struct pwm_move)); } DECL_COMMAND(command_config_pwm_out, "config_pwm_out oid=%c pin=%u cycle_ticks=%u value=%hu" " default_value=%hu max_duration=%u"); void -command_schedule_pwm_out(uint32_t *args) +command_queue_pwm_out(uint32_t *args) { struct pwm_out_s *p = oid_lookup(args[0], command_config_pwm_out); + struct pwm_move *m = move_alloc(); + m->waketime = args[1]; + m->value = args[2]; + + irq_disable(); + int need_add_timer = move_queue_push(&m->node, &p->mq); + irq_enable(); + if (!need_add_timer) + return; + + // queue was empty and a timer needs to be added sched_del_timer(&p->timer); + if (p->timer.func == pwm_end_event + && timer_is_before(p->timer.waketime, m->waketime)) + shutdown("Scheduled pwm event will exceed max_duration"); p->timer.func = pwm_event; - p->timer.waketime = args[1]; - p->value = args[2]; + p->timer.waketime = m->waketime; sched_add_timer(&p->timer); } -DECL_COMMAND(command_schedule_pwm_out, - "schedule_pwm_out oid=%c clock=%u value=%hu"); +DECL_COMMAND(command_queue_pwm_out, "queue_pwm_out oid=%c clock=%u value=%hu"); void pwm_shutdown(void) @@ -68,6 +111,8 @@ pwm_shutdown(void) struct pwm_out_s *p; foreach_oid(i, p, command_config_pwm_out) { gpio_pwm_write(p->pin, p->default_value); + p->timer.func = pwm_event; + move_queue_clear(&p->mq); } } DECL_SHUTDOWN(pwm_shutdown);