From a38437f378ece33cb762b482120be629c78d3540 Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Sun, 5 Mar 2017 15:00:15 -0500 Subject: [PATCH] stepper: Introduce stepper_get_position command and remove from endstop.c Move the logic to calculate and report the stepper's current position from endstop.c to stepper.c. This localizes the stepper code into stepper.c. Signed-off-by: Kevin O'Connor --- docs/Firmware_Commands.md | 6 ++++++ klippy/mcu.py | 19 +++++++++++-------- klippy/serialhdl.py | 11 ++++++----- src/endstop.c | 8 +++----- src/stepper.c | 32 +++++++++++++++++++++++++++----- src/stepper.h | 6 +----- 6 files changed, 54 insertions(+), 28 deletions(-) diff --git a/docs/Firmware_Commands.md b/docs/Firmware_Commands.md index a0f9a65c..63647c39 100644 --- a/docs/Firmware_Commands.md +++ b/docs/Firmware_Commands.md @@ -251,6 +251,12 @@ Stepper commands so that the next step is relative to the supplied 'clock' time. The host usually only sends this command at the start of a print. +* stepper_get_position oid=%c : This command causes the firmware to + generate a "stepper_position" response message with the stepper's + current position. The position is the total number of steps + generated with dir=1 minus the total number of steps generated with + dir=0. + * end_stop_home oid=%c clock=%u rest_ticks=%u pin_value=%c : This command is used during stepper "homing" operations. To use this command a 'config_end_stop' command with the same 'oid' parameter diff --git a/klippy/mcu.py b/klippy/mcu.py index e4d17898..c7fde801 100644 --- a/klippy/mcu.py +++ b/klippy/mcu.py @@ -43,6 +43,8 @@ class MCU_stepper: "set_next_step_dir oid=%c dir=%c") self._reset_cmd = mcu.lookup_command( "reset_step_clock oid=%c clock=%u") + self._get_position_cmd = mcu.lookup_command( + "stepper_get_position oid=%c") ffi_main, self.ffi_lib = chelper.get_ffi() self._stepqueue = ffi_main.gc(self.ffi_lib.stepcompress_alloc( max_error, self._step_cmd.msgid @@ -51,13 +53,9 @@ class MCU_stepper: self.print_to_mcu_time = mcu.print_to_mcu_time def get_oid(self): return self._oid - def get_invert_dir(self): - return self._invert_dir def set_position(self, pos): self._mcu_position_offset += self.commanded_position - pos self.commanded_position = pos - def set_mcu_position(self, pos): - self._mcu_position_offset = pos - self.commanded_position def get_mcu_position(self): return self.commanded_position + self._mcu_position_offset def note_homing_start(self, homing_clock): @@ -71,6 +69,14 @@ class MCU_stepper: ret = self.ffi_lib.stepcompress_reset(self._stepqueue, 0) if ret: raise error("Internal error in stepcompress") + def note_homing_triggered(self): + params = self._mcu.serial.send_with_response( + self._get_position_cmd.encode(self._oid), + 'stepper_position', self._oid) + pos = params['pos'] + if self._invert_dir: + pos = -pos + self._mcu_position_offset = pos - self.commanded_position def reset_step_clock(self, mcu_time): clock = int(mcu_time * self._mcu_freq) ret = self.ffi_lib.stepcompress_reset(self._stepqueue, clock) @@ -188,10 +194,7 @@ class MCU_endstop: if not self._homing: return False if not self._last_state.get('homing', 0): - pos = self._last_state.get('pos', 0) - if self._stepper.get_invert_dir(): - pos = -pos - self._stepper.set_mcu_position(pos) + self._stepper.note_homing_triggered() self._homing = False return False if (self._mcu.serial.get_clock(last_sent_time) diff --git a/klippy/serialhdl.py b/klippy/serialhdl.py index 8112d703..6ccd9636 100644 --- a/klippy/serialhdl.py +++ b/klippy/serialhdl.py @@ -171,8 +171,8 @@ class SerialReader: def encode_and_send(self, data, minclock, reqclock, cq): self.ffi_lib.serialqueue_encode_and_send( self.serialqueue, cq, data, len(data), minclock, reqclock) - def send_with_response(self, cmd, name): - src = SerialRetryCommand(self, cmd, name) + def send_with_response(self, cmd, name, oid=None): + src = SerialRetryCommand(self, cmd, name, oid) return src.get_response() def alloc_command_queue(self): return self.ffi_main.gc(self.ffi_lib.serialqueue_alloc_commandqueue(), @@ -235,17 +235,18 @@ class SerialReader: class SerialRetryCommand: TIMEOUT_TIME = 5.0 RETRY_TIME = 0.500 - def __init__(self, serial, cmd, name): + def __init__(self, serial, cmd, name, oid=None): self.serial = serial self.cmd = cmd self.name = name + self.oid = oid self.response = None self.min_query_time = self.serial.reactor.monotonic() - self.serial.register_callback(self.handle_callback, self.name) + self.serial.register_callback(self.handle_callback, self.name, self.oid) self.send_timer = self.serial.reactor.register_timer( self.send_event, self.serial.reactor.NOW) def unregister(self): - self.serial.unregister_callback(self.name) + self.serial.unregister_callback(self.name, self.oid) self.serial.reactor.unregister_timer(self.send_timer) def send_event(self, eventtime): if self.response is not None: diff --git a/src/endstop.c b/src/endstop.c index 3701f642..913bb466 100644 --- a/src/endstop.c +++ b/src/endstop.c @@ -42,7 +42,7 @@ void command_config_end_stop(uint32_t *args) { struct end_stop *e = oid_alloc(args[0], command_config_end_stop, sizeof(*e)); - struct stepper *s = oid_lookup(args[3], command_config_stepper); + struct stepper *s = stepper_oid_lookup(args[3]); e->time.func = end_stop_event; e->stepper = s; e->pin = gpio_in_setup(args[1], args[2]); @@ -73,14 +73,12 @@ static void end_stop_report(uint8_t oid, struct end_stop *e) { irq_disable(); - uint32_t position = stepper_get_position(e->stepper); uint8_t eflags = e->flags; e->flags &= ~ESF_REPORT; irq_enable(); - sendf("end_stop_state oid=%c homing=%c pin=%c pos=%i" - , oid, !!(eflags & ESF_HOMING), gpio_in_read(e->pin) - , position - STEPPER_POSITION_BIAS); + sendf("end_stop_state oid=%c homing=%c pin=%c" + , oid, !!(eflags & ESF_HOMING), gpio_in_read(e->pin)); } void diff --git a/src/stepper.c b/src/stepper.c index 39fdab54..b196dc61 100644 --- a/src/stepper.c +++ b/src/stepper.c @@ -46,6 +46,8 @@ struct stepper { uint8_t flags : 8; }; +enum { POSITION_BIAS=0x40000000 }; + enum { SF_LAST_DIR=1<<0, SF_NEXT_DIR=1<<1, SF_INVERT_STEP=1<<2, SF_HAVE_ADD=1<<3, SF_LAST_RESET=1<<4, SF_NO_NEXT_CHECK=1<<5 }; @@ -152,18 +154,25 @@ command_config_stepper(uint32_t *args) s->step_pin = gpio_out_setup(args[1], s->flags & SF_INVERT_STEP ? 1 : 0); s->dir_pin = gpio_out_setup(args[2], 0); s->min_stop_interval = args[3]; - s->position = -STEPPER_POSITION_BIAS; + s->position = -POSITION_BIAS; move_request_size(sizeof(struct stepper_move)); } DECL_COMMAND(command_config_stepper, "config_stepper oid=%c step_pin=%c dir_pin=%c" " min_stop_interval=%u invert_step=%c"); +// Return the 'struct stepper' for a given stepper oid +struct stepper * +stepper_oid_lookup(uint8_t oid) +{ + return oid_lookup(oid, command_config_stepper); +} + // Schedule a set of steps with a given timing void command_queue_step(uint32_t *args) { - struct stepper *s = oid_lookup(args[0], command_config_stepper); + struct stepper *s = stepper_oid_lookup(args[0]); struct stepper_move *m = move_alloc(); m->interval = args[1]; m->count = args[2]; @@ -204,7 +213,7 @@ DECL_COMMAND(command_queue_step, void command_set_next_step_dir(uint32_t *args) { - struct stepper *s = oid_lookup(args[0], command_config_stepper); + struct stepper *s = stepper_oid_lookup(args[0]); uint8_t nextdir = args[1] ? SF_NEXT_DIR : 0; irq_disable(); s->flags = (s->flags & ~SF_NEXT_DIR) | nextdir; @@ -216,7 +225,7 @@ DECL_COMMAND(command_set_next_step_dir, "set_next_step_dir oid=%c dir=%c"); void command_reset_step_clock(uint32_t *args) { - struct stepper *s = oid_lookup(args[0], command_config_stepper); + struct stepper *s = stepper_oid_lookup(args[0]); uint32_t waketime = args[1]; irq_disable(); if (s->count) @@ -228,7 +237,7 @@ command_reset_step_clock(uint32_t *args) DECL_COMMAND(command_reset_step_clock, "reset_step_clock oid=%c clock=%u"); // Return the current stepper position. Caller must disable irqs. -uint32_t +static uint32_t stepper_get_position(struct stepper *s) { uint32_t position = s->position; @@ -241,6 +250,19 @@ stepper_get_position(struct stepper *s) return position; } +// Report the current position of the stepper +void +command_stepper_get_position(uint32_t *args) +{ + uint8_t oid = args[0]; + struct stepper *s = stepper_oid_lookup(oid); + irq_disable(); + uint32_t position = stepper_get_position(s); + irq_enable(); + sendf("stepper_position oid=%c pos=%i", oid, position - POSITION_BIAS); +} +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 diff --git a/src/stepper.h b/src/stepper.h index f4019ae5..8e27b789 100644 --- a/src/stepper.h +++ b/src/stepper.h @@ -3,12 +3,8 @@ #include // uint8_t -enum { STEPPER_POSITION_BIAS=0x40000000 }; - uint_fast8_t stepper_event(struct timer *t); -void command_config_stepper(uint32_t *args); -struct stepper; -uint32_t stepper_get_position(struct stepper *s); +struct stepper *stepper_oid_lookup(uint8_t oid); void stepper_stop(struct stepper *s); #endif // stepper.h