Add filament_motion_sensor

docs/Config_Reference.md

@@ -3452,6 +3452,34 @@ information.
 #   provided.
 ```
 
+## [filament_motion_sensor]
+
+Filament Motion Sensor. Support for filament insert and runout
+detection using an encoder that toggles the output pin during filament
+movement through the sensor.
+
+See the [command reference](G-Codes.md#filament-sensor) for more
+information.
+
+```
+[filament_motion_sensor my_sensor]
+detection_length: 7.0
+#   The minimum length of filament pulled through the sensor to trigger
+#   a state change on the switch_pin
+#   Default is 7 mm.
+extruder:
+#   The name of the extruder section this sensor is associated with.
+#   This parameter must be provided.
+switch_pin:
+#pause_on_runout:
+#runout_gcode:
+#insert_gcode:
+#event_delay:
+#pause_delay:
+#   See the "filament_switch_sensor" section for a description of the
+#   above parameters.
+```
+
 ## [tsl1401cl_filament_width_sensor]
 
 TSLl401CL Based Filament Width Sensor. See the

docs/G-Codes.md

@@ -574,7 +574,7 @@ enabled:
 ## Filament Sensor
 
 The following command is available when the
-[filament_switch_sensor config section](Config_Reference.md#filament_switch_sensor)
+[filament_switch_sensor or filament_motion_sensor config section](Config_Reference.md#filament_switch_sensor)
 is enabled.
 - `QUERY_FILAMENT_SENSOR SENSOR=<sensor_name>`: Queries the current
   status of the filament sensor. The data displayed on the terminal

klippy/chelper/__init__.py

@@ -35,6 +35,10 @@
         , int32_t set_next_step_dir_msgtag);
     void stepcompress_free(struct stepcompress *sc);
     int stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock);
+    int stepcompress_set_last_position(struct stepcompress *sc
+        , int64_t last_position);
+    int64_t stepcompress_find_past_position(struct stepcompress *sc
+        , uint64_t clock);
     int stepcompress_queue_msg(struct stepcompress *sc
         , uint32_t *data, int len);
 

klippy/chelper/stepcompress.c

@@ -14,6 +14,7 @@
 // This code is written in C (instead of python) for processing
 // efficiency - the repetitive integer math is vastly faster in C.
 
+#include <math.h> // sqrt
 #include <stddef.h> // offsetof
 #include <stdint.h> // uint32_t
 #include <stdio.h> // fprintf
@@ -42,6 +43,25 @@ struct stepcompress {
     // Step+dir+step filter
     uint64_t next_step_clock;
     int next_step_dir;
+    // History tracking
+    int64_t last_position;
+    struct list_head history_list;
+};
+
+struct step_move {
+    uint32_t interval;
+    uint16_t count;
+    int16_t add;
+};
+
+#define HISTORY_EXPIRE (30.0)
+
+struct history_move {
+    struct list_node node;
+    uint64_t first_clock, last_clock;
+    int64_t start_position;
+    int step_count;
+    struct step_move sm;
 };
 
 
@@ -84,12 +104,6 @@ minmax_point(struct stepcompress *sc, uint32_t *pos)
 // using 11 works well in practice.
 #define QUADRATIC_DEV 11
 
-struct step_move {
-    uint32_t interval;
-    uint16_t count;
-    int16_t add;
-};
-
 // Find a 'step_move' that covers a series of step times
 static struct step_move
 compress_bisect_add(struct stepcompress *sc)
@@ -237,6 +251,7 @@ stepcompress_alloc(uint32_t oid)
     struct stepcompress *sc = malloc(sizeof(*sc));
     memset(sc, 0, sizeof(*sc));
     list_init(&sc->msg_queue);
+    list_init(&sc->history_list);
     sc->oid = oid;
     sc->sdir = -1;
     return sc;
@@ -254,6 +269,20 @@ stepcompress_fill(struct stepcompress *sc, uint32_t max_error
     sc->set_next_step_dir_msgtag = set_next_step_dir_msgtag;
 }
 
+// Helper to free items from the history_list
+static void
+free_history(struct stepcompress *sc, uint64_t end_clock)
+{
+    while (!list_empty(&sc->history_list)) {
+        struct history_move *hm = list_last_entry(
+            &sc->history_list, struct history_move, node);
+        if (hm->last_clock > end_clock)
+            break;
+        list_del(&hm->node);
+        free(hm);
+    }
+}
+
 // Free memory associated with a 'stepcompress' object
 void __visible
 stepcompress_free(struct stepcompress *sc)
@@ -262,6 +291,7 @@ stepcompress_free(struct stepcompress *sc)
         return;
     free(sc->queue);
     message_queue_free(&sc->msg_queue);
+    free_history(sc, UINT64_MAX);
     free(sc);
 }
 
@@ -283,6 +313,9 @@ calc_last_step_print_time(struct stepcompress *sc)
 {
     double lsc = sc->last_step_clock;
     sc->last_step_print_time = sc->mcu_time_offset + (lsc - .5) / sc->mcu_freq;
+
+    if (lsc > sc->mcu_freq * HISTORY_EXPIRE)
+        free_history(sc, lsc - sc->mcu_freq * HISTORY_EXPIRE);
 }
 
 // Set the conversion rate of 'print_time' to mcu clock
@@ -295,6 +328,39 @@ stepcompress_set_time(struct stepcompress *sc
     calc_last_step_print_time(sc);
 }
 
+// Maximium clock delta between messages in the queue
+#define CLOCK_DIFF_MAX (3<<28)
+
+// Helper to create a queue_step command from a 'struct step_move'
+static void
+add_move(struct stepcompress *sc, uint64_t first_clock, struct step_move *move)
+{
+    int32_t addfactor = move->count*(move->count-1)/2;
+    uint32_t ticks = move->add*addfactor + move->interval*(move->count-1);
+    uint64_t last_clock = first_clock + ticks;
+
+    // Create and queue a queue_step command
+    uint32_t msg[5] = {
+        sc->queue_step_msgtag, sc->oid, move->interval, move->count, move->add
+    };
+    struct queue_message *qm = message_alloc_and_encode(msg, 5);
+    qm->min_clock = qm->req_clock = sc->last_step_clock;
+    if (move->count == 1 && first_clock >= sc->last_step_clock + CLOCK_DIFF_MAX)
+        qm->req_clock = first_clock;
+    list_add_tail(&qm->node, &sc->msg_queue);
+    sc->last_step_clock = last_clock;
+
+    // Create and store move in history tracking
+    struct history_move *hm = malloc(sizeof(*hm));
+    hm->first_clock = first_clock;
+    hm->last_clock = last_clock;
+    hm->start_position = sc->last_position;
+    hm->step_count = sc->sdir ? move->count : -move->count;
+    sc->last_position += hm->step_count;
+    memcpy(&hm->sm, move, sizeof(hm->sm));
+    list_add_head(&hm->node, &sc->history_list);
+}
+
 // Convert previously scheduled steps into commands for the mcu
 static int
 queue_flush(struct stepcompress *sc, uint64_t move_clock)
@@ -307,15 +373,7 @@ queue_flush(struct stepcompress *sc, uint64_t move_clock)
         if (ret)
             return ret;
 
-        uint32_t msg[5] = {
-            sc->queue_step_msgtag, sc->oid, move.interval, move.count, move.add
-        };
-        struct queue_message *qm = message_alloc_and_encode(msg, 5);
-        qm->min_clock = qm->req_clock = sc->last_step_clock;
-        int32_t addfactor = move.count*(move.count-1)/2;
-        uint32_t ticks = move.add*addfactor + move.interval*move.count;
-        sc->last_step_clock += ticks;
-        list_add_tail(&qm->node, &sc->msg_queue);
+        add_move(sc, sc->last_step_clock + move.interval, &move);
 
         if (sc->queue_pos + move.count >= sc->queue_next) {
             sc->queue_pos = sc->queue_next = sc->queue;
@@ -331,14 +389,8 @@ queue_flush(struct stepcompress *sc, uint64_t move_clock)
 static int
 stepcompress_flush_far(struct stepcompress *sc, uint64_t abs_step_clock)
 {
-    uint32_t msg[5] = {
-        sc->queue_step_msgtag, sc->oid, abs_step_clock - sc->last_step_clock,
-        1, 0
-    };
-    struct queue_message *qm = message_alloc_and_encode(msg, 5);
-    qm->min_clock = sc->last_step_clock;
-    sc->last_step_clock = qm->req_clock = abs_step_clock;
-    list_add_tail(&qm->node, &sc->msg_queue);
+    struct step_move move = { abs_step_clock - sc->last_step_clock, 1, 0 };
+    add_move(sc, abs_step_clock, &move);
     calc_last_step_print_time(sc);
     return 0;
 }
@@ -349,10 +401,10 @@ set_next_step_dir(struct stepcompress *sc, int sdir)
 {
     if (sc->sdir == sdir)
         return 0;
-    sc->sdir = sdir;
     int ret = queue_flush(sc, UINT64_MAX);
     if (ret)
         return ret;
+    sc->sdir = sdir;
     uint32_t msg[3] = {
         sc->set_next_step_dir_msgtag, sc->oid, sdir ^ sc->invert_sdir
     };
@@ -362,9 +414,6 @@ set_next_step_dir(struct stepcompress *sc, int sdir)
     return 0;
 }
 
-// Maximium clock delta between messages in the queue
-#define CLOCK_DIFF_MAX (3<<28)
-
 // Slow path for queue_append() - handle next step far in future
 static int
 queue_append_far(struct stepcompress *sc)
@@ -502,6 +551,46 @@ stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock)
     return 0;
 }
 
+// Set last_position in the stepcompress object
+int __visible
+stepcompress_set_last_position(struct stepcompress *sc, int64_t last_position)
+{
+    int ret = stepcompress_flush(sc, UINT64_MAX);
+    if (ret)
+        return ret;
+    sc->last_position = last_position;
+    return 0;
+}
+
+// Search history of moves to find a past position at a given clock
+int64_t __visible
+stepcompress_find_past_position(struct stepcompress *sc, uint64_t clock)
+{
+    int64_t last_position = sc->last_position;
+    struct history_move *hm;
+    list_for_each_entry(hm, &sc->history_list, node) {
+        if (clock < hm->first_clock) {
+            last_position = hm->start_position;
+            continue;
+        }
+        if (clock >= hm->last_clock)
+            return hm->start_position + hm->step_count;
+        int32_t interval = hm->sm.interval, add = hm->sm.add;
+        int32_t ticks = (int32_t)(clock - hm->first_clock) + interval, offset;
+        if (!add) {
+            offset = ticks / interval;
+        } else {
+            // Solve for "count" using quadratic formula
+            double a = .5 * add, b = interval - .5 * add, c = -ticks;
+            offset = (sqrt(b*b - 4*a*c) - b) / (2. * a);
+        }
+        if (hm->step_count < 0)
+            return hm->start_position - offset;
+        return hm->start_position + offset;
+    }
+    return last_position;
+}
+
 // Queue an mcu command to go out in order with stepper commands
 int __visible
 stepcompress_queue_msg(struct stepcompress *sc, uint32_t *data, int len)

klippy/extras/filament_motion_sensor.py

@@ -0,0 +1,82 @@
+# Filament Motion Sensor Module
+#
+# Copyright (C) 2021 Joshua Wherrett <thejoshw.code@gmail.com>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import logging
+from . import filament_switch_sensor
+
+CHECK_RUNOUT_TIMEOUT = .250
+
+class MotionHelper:
+    def __init__(self, config):
+        # Read config
+        self.printer = config.get_printer()
+        self.extruder_name = config.get("extruder", None)
+        self.detection_length = config.getfloat(
+                'detection_length', 7., above=0.)
+        # Get printer objects
+        self.reactor = self.printer.get_reactor()
+        self.runout_helper = filament_switch_sensor.RunoutHelper(config)
+        self.extruder = None
+        self.mcu = None
+        # Initialise internal state
+        self.filament_runout_pos = None
+        # Register commands and event handlers
+        self.printer.register_event_handler("klippy:ready",
+                self._handle_ready)
+        self.printer.register_event_handler("idle_timeout:printing",
+                self._handle_printing)
+        self.printer.register_event_handler("idle_timeout:ready",
+                self._handle_not_printing)
+        self.printer.register_event_handler("idle_timeout:idle",
+                self._handle_not_printing)
+    def _update_filament_runout_pos(self, eventtime=None):
+        if eventtime == None:
+            eventtime = self.reactor.monotonic()
+        self.filament_runout_pos = (
+                self._get_extruder_pos(eventtime) +
+                self.detection_length)
+    def _handle_ready(self):
+        self.extruder = self.printer.lookup_object(self.extruder_name)
+        self.mcu = self.extruder.stepper.get_mcu()
+        self._update_filament_runout_pos()
+        self._extruder_pos_update_timer = self.reactor.register_timer(
+                self._extruder_pos_update_event)
+    def _handle_printing(self, print_time):
+        self.reactor.update_timer(self._extruder_pos_update_timer,
+                self.reactor.NOW)
+    def _handle_not_printing(self, print_time):
+        self.reactor.update_timer(self._extruder_pos_update_timer,
+                self.reactor.NEVER)
+    def _get_extruder_pos(self, eventtime=None):
+        if eventtime == None:
+            eventtime = self.reactor.monotonic()
+        print_time = self.mcu.estimated_print_time(eventtime)
+        return self.extruder.find_past_position(print_time)
+    def _extruder_pos_update_event(self, eventtime):
+        extruder_pos = self._get_extruder_pos(eventtime)
+        # Check for filament runout
+        self.runout_helper.note_filament_present(
+                extruder_pos < self.filament_runout_pos)
+        return eventtime + CHECK_RUNOUT_TIMEOUT
+    def encoder_event(self, eventtime, state):
+        if self.mcu != None:
+            self._update_filament_runout_pos(eventtime)
+            # Check for filament insertion
+            # Filament is always assumed to be present on an encoder event
+            self.runout_helper.note_filament_present(True)
+
+class EncoderSensor:
+    def __init__(self, config):
+        printer = config.get_printer()
+        # This sensor behaves like a button
+        buttons = printer.load_object(config, 'buttons')
+        switch_pin = config.get('switch_pin')
+        buttons.register_buttons([switch_pin], self._button_handler)
+        self.motion_helper = MotionHelper(config)
+    def _button_handler(self, eventtime, state):
+        self.motion_helper.encoder_event(eventtime, state)
+
+def load_config_prefix(config):
+    return EncoderSensor(config)

klippy/kinematics/extruder.py

@@ -152,6 +152,10 @@ def move(self, print_time, move):
                           move.start_pos[3], 0., 0.,
                           1., pressure_advance, 0.,
                           start_v, cruise_v, accel)
+    def find_past_position(self, print_time):
+        mcu = self.stepper.get_mcu()
+        clock = mcu.print_time_to_clock(print_time)
+        return self.stepper.get_past_commanded_position(clock)
     def cmd_M104(self, gcmd, wait=False):
         # Set Extruder Temperature
         temp = gcmd.get_float('S', 0.)
@@ -220,6 +224,8 @@ def update_move_time(self, flush_time):
         pass
     def check_move(self, move):
         raise move.move_error("Extrude when no extruder present")
+    def find_past_position(self, print_time):
+        return 0.
     def calc_junction(self, prev_move, move):
         return move.max_cruise_v2
     def get_name(self):