klipper update

klippy/chelper/__init__.py

@@ -0,0 +1,304 @@
+# Wrapper around C helper code
+#
+# Copyright (C) 2016-2021  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import os, logging
+import cffi
+
+
+######################################################################
+# c_helper.so compiling
+######################################################################
+
+GCC_CMD = "gcc"
+COMPILE_ARGS = ("-Wall -g -O2 -shared -fPIC"
+                " -flto -fwhole-program -fno-use-linker-plugin"
+                " -o %s %s")
+SSE_FLAGS = "-mfpmath=sse -msse2"
+SOURCE_FILES = [
+    'pyhelper.c', 'serialqueue.c', 'stepcompress.c', 'itersolve.c', 'trapq.c',
+    'pollreactor.c', 'msgblock.c', 'trdispatch.c',
+    'kin_cartesian.c', 'kin_corexy.c', 'kin_corexz.c', 'kin_delta.c',
+    'kin_polar.c', 'kin_rotary_delta.c', 'kin_winch.c', 'kin_extruder.c',
+    'kin_shaper.c',
+]
+DEST_LIB = "c_helper.so"
+OTHER_FILES = [
+    'list.h', 'serialqueue.h', 'stepcompress.h', 'itersolve.h', 'pyhelper.h',
+    'trapq.h', 'pollreactor.h', 'msgblock.h'
+]
+
+defs_stepcompress = """
+    struct pull_history_steps {
+        uint64_t first_clock, last_clock;
+        int64_t start_position;
+        int step_count, interval, add;
+    };
+
+    struct stepcompress *stepcompress_alloc(uint32_t oid);
+    void stepcompress_fill(struct stepcompress *sc, uint32_t max_error
+        , int32_t queue_step_msgtag, int32_t set_next_step_dir_msgtag);
+    void stepcompress_set_invert_sdir(struct stepcompress *sc
+        , uint32_t invert_sdir);
+    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
+        , uint64_t clock, 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);
+    int stepcompress_extract_old(struct stepcompress *sc
+        , struct pull_history_steps *p, int max
+        , uint64_t start_clock, uint64_t end_clock);
+
+    struct steppersync *steppersync_alloc(struct serialqueue *sq
+        , struct stepcompress **sc_list, int sc_num, int move_num);
+    void steppersync_free(struct steppersync *ss);
+    void steppersync_set_time(struct steppersync *ss
+        , double time_offset, double mcu_freq);
+    int steppersync_flush(struct steppersync *ss, uint64_t move_clock);
+"""
+
+defs_itersolve = """
+    int32_t itersolve_generate_steps(struct stepper_kinematics *sk
+        , double flush_time);
+    double itersolve_check_active(struct stepper_kinematics *sk
+        , double flush_time);
+    int32_t itersolve_is_active_axis(struct stepper_kinematics *sk, char axis);
+    void itersolve_set_trapq(struct stepper_kinematics *sk, struct trapq *tq);
+    void itersolve_set_stepcompress(struct stepper_kinematics *sk
+        , struct stepcompress *sc, double step_dist);
+    double itersolve_calc_position_from_coord(struct stepper_kinematics *sk
+        , double x, double y, double z);
+    void itersolve_set_position(struct stepper_kinematics *sk
+        , double x, double y, double z);
+    double itersolve_get_commanded_pos(struct stepper_kinematics *sk);
+"""
+
+defs_trapq = """
+    struct pull_move {
+        double print_time, move_t;
+        double start_v, accel;
+        double start_x, start_y, start_z;
+        double x_r, y_r, z_r;
+    };
+
+    void trapq_append(struct trapq *tq, double print_time
+        , double accel_t, double cruise_t, double decel_t
+        , double start_pos_x, double start_pos_y, double start_pos_z
+        , double axes_r_x, double axes_r_y, double axes_r_z
+        , double start_v, double cruise_v, double accel);
+    struct trapq *trapq_alloc(void);
+    void trapq_free(struct trapq *tq);
+    void trapq_finalize_moves(struct trapq *tq, double print_time);
+    void trapq_set_position(struct trapq *tq, double print_time
+        , double pos_x, double pos_y, double pos_z);
+    int trapq_extract_old(struct trapq *tq, struct pull_move *p, int max
+        , double start_time, double end_time);
+"""
+
+defs_kin_cartesian = """
+    struct stepper_kinematics *cartesian_stepper_alloc(char axis);
+    struct stepper_kinematics *cartesian_reverse_stepper_alloc(char axis);
+"""
+
+defs_kin_corexy = """
+    struct stepper_kinematics *corexy_stepper_alloc(char type);
+"""
+
+defs_kin_corexz = """
+    struct stepper_kinematics *corexz_stepper_alloc(char type);
+"""
+
+defs_kin_delta = """
+    struct stepper_kinematics *delta_stepper_alloc(double arm2
+        , double tower_x, double tower_y);
+"""
+
+defs_kin_polar = """
+    struct stepper_kinematics *polar_stepper_alloc(char type);
+"""
+
+defs_kin_rotary_delta = """
+    struct stepper_kinematics *rotary_delta_stepper_alloc(
+        double shoulder_radius, double shoulder_height
+        , double angle, double upper_arm, double lower_arm);
+"""
+
+defs_kin_winch = """
+    struct stepper_kinematics *winch_stepper_alloc(double anchor_x
+        , double anchor_y, double anchor_z);
+"""
+
+defs_kin_extruder = """
+    struct stepper_kinematics *extruder_stepper_alloc(void);
+    void extruder_set_pressure_advance(struct stepper_kinematics *sk
+        , double pressure_advance, double smooth_time);
+"""
+
+defs_kin_shaper = """
+    double input_shaper_get_step_generation_window(int n, double a[]
+        , double t[]);
+    int input_shaper_set_shaper_params(struct stepper_kinematics *sk, char axis
+        , int n, double a[], double t[]);
+    int input_shaper_set_sk(struct stepper_kinematics *sk
+        , struct stepper_kinematics *orig_sk);
+    struct stepper_kinematics * input_shaper_alloc(void);
+"""
+
+defs_serialqueue = """
+    #define MESSAGE_MAX 64
+    struct pull_queue_message {
+        uint8_t msg[MESSAGE_MAX];
+        int len;
+        double sent_time, receive_time;
+        uint64_t notify_id;
+    };
+
+    struct serialqueue *serialqueue_alloc(int serial_fd, char serial_fd_type
+        , int client_id);
+    void serialqueue_exit(struct serialqueue *sq);
+    void serialqueue_free(struct serialqueue *sq);
+    struct command_queue *serialqueue_alloc_commandqueue(void);
+    void serialqueue_free_commandqueue(struct command_queue *cq);
+    void serialqueue_send(struct serialqueue *sq, struct command_queue *cq
+        , uint8_t *msg, int len, uint64_t min_clock, uint64_t req_clock
+        , uint64_t notify_id);
+    void serialqueue_pull(struct serialqueue *sq
+        , struct pull_queue_message *pqm);
+    void serialqueue_set_baud_adjust(struct serialqueue *sq
+        , double baud_adjust);
+    void serialqueue_set_receive_window(struct serialqueue *sq
+        , int receive_window);
+    void serialqueue_set_clock_est(struct serialqueue *sq, double est_freq
+        , double conv_time, uint64_t conv_clock, uint64_t last_clock);
+    void serialqueue_get_stats(struct serialqueue *sq, char *buf, int len);
+    int serialqueue_extract_old(struct serialqueue *sq, int sentq
+        , struct pull_queue_message *q, int max);
+"""
+
+defs_trdispatch = """
+    void trdispatch_start(struct trdispatch *td, uint32_t dispatch_reason);
+    void trdispatch_stop(struct trdispatch *td);
+    struct trdispatch *trdispatch_alloc(void);
+    struct trdispatch_mcu *trdispatch_mcu_alloc(struct trdispatch *td
+        , struct serialqueue *sq, struct command_queue *cq, uint32_t trsync_oid
+        , uint32_t set_timeout_msgtag, uint32_t trigger_msgtag
+        , uint32_t state_msgtag);
+    void trdispatch_mcu_setup(struct trdispatch_mcu *tdm
+        , uint64_t last_status_clock, uint64_t expire_clock
+        , uint64_t expire_ticks, uint64_t min_extend_ticks);
+"""
+
+defs_pyhelper = """
+    void set_python_logging_callback(void (*func)(const char *));
+    double get_monotonic(void);
+"""
+
+defs_std = """
+    void free(void*);
+"""
+
+defs_all = [
+    defs_pyhelper, defs_serialqueue, defs_std, defs_stepcompress,
+    defs_itersolve, defs_trapq, defs_trdispatch,
+    defs_kin_cartesian, defs_kin_corexy, defs_kin_corexz, defs_kin_delta,
+    defs_kin_polar, defs_kin_rotary_delta, defs_kin_winch, defs_kin_extruder,
+    defs_kin_shaper,
+]
+
+# Update filenames to an absolute path
+def get_abs_files(srcdir, filelist):
+    return [os.path.join(srcdir, fname) for fname in filelist]
+
+# Return the list of file modification times
+def get_mtimes(filelist):
+    out = []
+    for filename in filelist:
+        try:
+            t = os.path.getmtime(filename)
+        except os.error:
+            continue
+        out.append(t)
+    return out
+
+# Check if the code needs to be compiled
+def check_build_code(sources, target):
+    src_times = get_mtimes(sources)
+    obj_times = get_mtimes([target])
+    return not obj_times or max(src_times) > min(obj_times)
+
+# Check if the current gcc version supports a particular command-line option
+def check_gcc_option(option):
+    cmd = "%s %s -S -o /dev/null -xc /dev/null > /dev/null 2>&1" % (
+        GCC_CMD, option)
+    res = os.system(cmd)
+    return res == 0
+
+# Check if the current gcc version supports a particular command-line option
+def do_build_code(cmd):
+    res = os.system(cmd)
+    if res:
+        msg = "Unable to build C code module (error=%s)" % (res,)
+        logging.error(msg)
+        raise Exception(msg)
+
+FFI_main = None
+FFI_lib = None
+pyhelper_logging_callback = None
+
+# Hepler invoked from C errorf() code to log errors
+def logging_callback(msg):
+    logging.error(FFI_main.string(msg))
+
+# Return the Foreign Function Interface api to the caller
+def get_ffi():
+    global FFI_main, FFI_lib, pyhelper_logging_callback
+    if FFI_lib is None:
+        srcdir = os.path.dirname(os.path.realpath(__file__))
+        srcfiles = get_abs_files(srcdir, SOURCE_FILES)
+        ofiles = get_abs_files(srcdir, OTHER_FILES)
+        destlib = get_abs_files(srcdir, [DEST_LIB])[0]
+        if check_build_code(srcfiles+ofiles+[__file__], destlib):
+            if check_gcc_option(SSE_FLAGS):
+                cmd = "%s %s %s" % (GCC_CMD, SSE_FLAGS, COMPILE_ARGS)
+            else:
+                cmd = "%s %s" % (GCC_CMD, COMPILE_ARGS)
+            logging.info("Building C code module %s", DEST_LIB)
+            do_build_code(cmd % (destlib, ' '.join(srcfiles)))
+        FFI_main = cffi.FFI()
+        for d in defs_all:
+            FFI_main.cdef(d)
+        FFI_lib = FFI_main.dlopen(destlib)
+        # Setup error logging
+        pyhelper_logging_callback = FFI_main.callback("void func(const char *)",
+                                                      logging_callback)
+        FFI_lib.set_python_logging_callback(pyhelper_logging_callback)
+    return FFI_main, FFI_lib
+
+
+######################################################################
+# hub-ctrl hub power controller
+######################################################################
+
+HC_COMPILE_CMD = "gcc -Wall -g -O2 -o %s %s -lusb"
+HC_SOURCE_FILES = ['hub-ctrl.c']
+HC_SOURCE_DIR = '../../lib/hub-ctrl'
+HC_TARGET = "hub-ctrl"
+HC_CMD = "sudo %s/hub-ctrl -h 0 -P 2 -p %d"
+
+def run_hub_ctrl(enable_power):
+    srcdir = os.path.dirname(os.path.realpath(__file__))
+    hubdir = os.path.join(srcdir, HC_SOURCE_DIR)
+    srcfiles = get_abs_files(hubdir, HC_SOURCE_FILES)
+    destlib = get_abs_files(hubdir, [HC_TARGET])[0]
+    if check_build_code(srcfiles, destlib):
+        logging.info("Building C code module %s", HC_TARGET)
+        do_build_code(HC_COMPILE_CMD % (destlib, ' '.join(srcfiles)))
+    os.system(HC_CMD % (hubdir, enable_power))
+
+
+if __name__ == '__main__':
+    get_ffi()

klippy/chelper/compiler.h

@@ -0,0 +1,46 @@
+#ifndef __COMPILER_H
+#define __COMPILER_H
+// Low level definitions for C languange and gcc compiler.
+
+#define barrier() __asm__ __volatile__("": : :"memory")
+
+#define likely(x)       __builtin_expect(!!(x), 1)
+#define unlikely(x)     __builtin_expect(!!(x), 0)
+
+#define noinline __attribute__((noinline))
+#ifndef __always_inline
+#define __always_inline inline __attribute__((always_inline))
+#endif
+#define __visible __attribute__((externally_visible))
+#define __noreturn __attribute__((noreturn))
+
+#define PACKED __attribute__((packed))
+#ifndef __aligned
+#define __aligned(x) __attribute__((aligned(x)))
+#endif
+#ifndef __section
+#define __section(S) __attribute__((section(S)))
+#endif
+
+#define ARRAY_SIZE(a) (sizeof(a) / sizeof(a[0]))
+#define ALIGN(x,a)              __ALIGN_MASK(x,(typeof(x))(a)-1)
+#define __ALIGN_MASK(x,mask)    (((x)+(mask))&~(mask))
+#define ALIGN_DOWN(x,a)         ((x) & ~((typeof(x))(a)-1))
+
+#define container_of(ptr, type, member) ({                      \
+        const typeof( ((type *)0)->member ) *__mptr = (ptr);    \
+        (type *)( (char *)__mptr - offsetof(type,member) );})
+
+#define __stringify_1(x)        #x
+#define __stringify(x)          __stringify_1(x)
+
+#define ___PASTE(a,b) a##b
+#define __PASTE(a,b) ___PASTE(a,b)
+
+#define DIV_ROUND_UP(n,d) (((n) + (d) - 1) / (d))
+#define DIV_ROUND_CLOSEST(x, divisor)({                 \
+            typeof(divisor) __divisor = divisor;        \
+            (((x) + ((__divisor) / 2)) / (__divisor));  \
+        })
+
+#endif // compiler.h

klippy/chelper/itersolve.c

@@ -0,0 +1,280 @@
+// Iterative solver for kinematic moves
+//
+// Copyright (C) 2018-2020  Kevin O'Connor <kevin@koconnor.net>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include <math.h> // fabs
+#include <stddef.h> // offsetof
+#include <string.h> // memset
+#include "compiler.h" // __visible
+#include "itersolve.h" // itersolve_generate_steps
+#include "pyhelper.h" // errorf
+#include "stepcompress.h" // queue_append_start
+#include "trapq.h" // struct move
+
+
+/****************************************************************
+ * Main iterative solver
+ ****************************************************************/
+
+struct timepos {
+    double time, position;
+};
+
+#define SEEK_TIME_RESET 0.000100
+
+// Generate step times for a portion of a move
+static int32_t
+itersolve_gen_steps_range(struct stepper_kinematics *sk, struct move *m
+                          , double abs_start, double abs_end)
+{
+    sk_calc_callback calc_position_cb = sk->calc_position_cb;
+    double half_step = .5 * sk->step_dist;
+    double start = abs_start - m->print_time, end = abs_end - m->print_time;
+    if (start < 0.)
+        start = 0.;
+    if (end > m->move_t)
+        end = m->move_t;
+    struct timepos old_guess = {start, sk->commanded_pos}, guess = old_guess;
+    int sdir = stepcompress_get_step_dir(sk->sc);
+    int is_dir_change = 0, have_bracket = 0, check_oscillate = 0;
+    double target = sk->commanded_pos + (sdir ? half_step : -half_step);
+    double last_time=start, low_time=start, high_time=start + SEEK_TIME_RESET;
+    if (high_time > end)
+        high_time = end;
+    for (;;) {
+        // Use the "secant method" to guess a new time from previous guesses
+        double guess_dist = guess.position - target;
+        double og_dist = old_guess.position - target;
+        double next_time = ((old_guess.time*guess_dist - guess.time*og_dist)
+                            / (guess_dist - og_dist));
+        if (!(next_time > low_time && next_time < high_time)) { // or NaN
+            // Next guess is outside bounds checks - validate it
+            if (have_bracket) {
+                // A poor guess - fall back to bisection
+                next_time = (low_time + high_time) * .5;
+                check_oscillate = 0;
+            } else if (guess.time >= end) {
+                // No more steps present in requested time range
+                break;
+            } else {
+                // Might be a poor guess - limit to exponential search
+                next_time = high_time;
+                high_time = 2. * high_time - last_time;
+                if (high_time > end)
+                    high_time = end;
+            }
+        }
+        // Calculate position at next_time guess
+        old_guess = guess;
+        guess.time = next_time;
+        guess.position = calc_position_cb(sk, m, next_time);
+        guess_dist = guess.position - target;
+        if (fabs(guess_dist) > .000000001) {
+            // Guess does not look close enough - update bounds
+            double rel_dist = sdir ? guess_dist : -guess_dist;
+            if (rel_dist > 0.) {
+                // Found position past target, so step is definitely present
+                if (have_bracket && old_guess.time <= low_time) {
+                    if (check_oscillate)
+                        // Force bisect next to avoid persistent oscillations
+                        old_guess = guess;
+                    check_oscillate = 1;
+                }
+                high_time = guess.time;
+                have_bracket = 1;
+            } else if (rel_dist < -(half_step + half_step + .000000010)) {
+                // Found direction change
+                sdir = !sdir;
+                target = (sdir ? target + half_step + half_step
+                          : target - half_step - half_step);
+                low_time = last_time;
+                high_time = guess.time;
+                is_dir_change = have_bracket = 1;
+                check_oscillate = 0;
+            } else {
+                low_time = guess.time;
+            }
+            if (!have_bracket || high_time - low_time > .000000001) {
+                if (!is_dir_change && rel_dist >= -half_step)
+                    // Avoid rollback if stepper fully reaches step position
+                    stepcompress_commit(sk->sc);
+                // Guess is not close enough - guess again with new time
+                continue;
+            }
+        }
+        // Found next step - submit it
+        int ret = stepcompress_append(sk->sc, sdir, m->print_time, guess.time);
+        if (ret)
+            return ret;
+        target = sdir ? target+half_step+half_step : target-half_step-half_step;
+        // Reset bounds checking
+        double seek_time_delta = 1.5 * (guess.time - last_time);
+        if (seek_time_delta < .000000001)
+            seek_time_delta = .000000001;
+        if (is_dir_change && seek_time_delta > SEEK_TIME_RESET)
+            seek_time_delta = SEEK_TIME_RESET;
+        last_time = low_time = guess.time;
+        high_time = guess.time + seek_time_delta;
+        if (high_time > end)
+            high_time = end;
+        is_dir_change = have_bracket = check_oscillate = 0;
+    }
+    sk->commanded_pos = target - (sdir ? half_step : -half_step);
+    if (sk->post_cb)
+        sk->post_cb(sk);
+    return 0;
+}
+
+
+/****************************************************************
+ * Interface functions
+ ****************************************************************/
+
+// Check if a move is likely to cause movement on a stepper
+static inline int
+check_active(struct stepper_kinematics *sk, struct move *m)
+{
+    int af = sk->active_flags;
+    return ((af & AF_X && m->axes_r.x != 0.)
+            || (af & AF_Y && m->axes_r.y != 0.)
+            || (af & AF_Z && m->axes_r.z != 0.));
+}
+
+// Generate step times for a range of moves on the trapq
+int32_t __visible
+itersolve_generate_steps(struct stepper_kinematics *sk, double flush_time)
+{
+    double last_flush_time = sk->last_flush_time;
+    sk->last_flush_time = flush_time;
+    if (!sk->tq)
+        return 0;
+    trapq_check_sentinels(sk->tq);
+    struct move *m = list_first_entry(&sk->tq->moves, struct move, node);
+    while (last_flush_time >= m->print_time + m->move_t)
+        m = list_next_entry(m, node);
+    double force_steps_time = sk->last_move_time + sk->gen_steps_post_active;
+    int skip_count = 0;
+    for (;;) {
+        double move_start = m->print_time, move_end = move_start + m->move_t;
+        if (check_active(sk, m)) {
+            if (skip_count && sk->gen_steps_pre_active) {
+                // Must generate steps leading up to stepper activity
+                double abs_start = move_start - sk->gen_steps_pre_active;
+                if (abs_start < last_flush_time)
+                    abs_start = last_flush_time;
+                if (abs_start < force_steps_time)
+                    abs_start = force_steps_time;
+                struct move *pm = list_prev_entry(m, node);
+                while (--skip_count && pm->print_time > abs_start)
+                    pm = list_prev_entry(pm, node);
+                do {
+                    int32_t ret = itersolve_gen_steps_range(sk, pm, abs_start
+                                                            , flush_time);
+                    if (ret)
+                        return ret;
+                    pm = list_next_entry(pm, node);
+                } while (pm != m);
+            }
+            // Generate steps for this move
+            int32_t ret = itersolve_gen_steps_range(sk, m, last_flush_time
+                                                    , flush_time);
+            if (ret)
+                return ret;
+            if (move_end >= flush_time) {
+                sk->last_move_time = flush_time;
+                return 0;
+            }
+            skip_count = 0;
+            sk->last_move_time = move_end;
+            force_steps_time = sk->last_move_time + sk->gen_steps_post_active;
+        } else {
+            if (move_start < force_steps_time) {
+                // Must generates steps just past stepper activity
+                double abs_end = force_steps_time;
+                if (abs_end > flush_time)
+                    abs_end = flush_time;
+                int32_t ret = itersolve_gen_steps_range(sk, m, last_flush_time
+                                                        , abs_end);
+                if (ret)
+                    return ret;
+                skip_count = 1;
+            } else {
+                // This move doesn't impact this stepper - skip it
+                skip_count++;
+            }
+            if (flush_time + sk->gen_steps_pre_active <= move_end)
+                return 0;
+        }
+        m = list_next_entry(m, node);
+    }
+}
+
+// Check if the given stepper is likely to be active in the given time range
+double __visible
+itersolve_check_active(struct stepper_kinematics *sk, double flush_time)
+{
+    if (!sk->tq)
+        return 0.;
+    trapq_check_sentinels(sk->tq);
+    struct move *m = list_first_entry(&sk->tq->moves, struct move, node);
+    while (sk->last_flush_time >= m->print_time + m->move_t)
+        m = list_next_entry(m, node);
+    for (;;) {
+        if (check_active(sk, m))
+            return m->print_time;
+        if (flush_time <= m->print_time + m->move_t)
+            return 0.;
+        m = list_next_entry(m, node);
+    }
+}
+
+// Report if the given stepper is registered for the given axis
+int32_t __visible
+itersolve_is_active_axis(struct stepper_kinematics *sk, char axis)
+{
+    if (axis < 'x' || axis > 'z')
+        return 0;
+    return (sk->active_flags & (AF_X << (axis - 'x'))) != 0;
+}
+
+void __visible
+itersolve_set_trapq(struct stepper_kinematics *sk, struct trapq *tq)
+{
+    sk->tq = tq;
+}
+
+void __visible
+itersolve_set_stepcompress(struct stepper_kinematics *sk
+                           , struct stepcompress *sc, double step_dist)
+{
+    sk->sc = sc;
+    sk->step_dist = step_dist;
+}
+
+double __visible
+itersolve_calc_position_from_coord(struct stepper_kinematics *sk
+                                   , double x, double y, double z)
+{
+    struct move m;
+    memset(&m, 0, sizeof(m));
+    m.start_pos.x = x;
+    m.start_pos.y = y;
+    m.start_pos.z = z;
+    m.move_t = 1000.;
+    return sk->calc_position_cb(sk, &m, 500.);
+}
+
+void __visible
+itersolve_set_position(struct stepper_kinematics *sk
+                       , double x, double y, double z)
+{
+    sk->commanded_pos = itersolve_calc_position_from_coord(sk, x, y, z);
+}
+
+double __visible
+itersolve_get_commanded_pos(struct stepper_kinematics *sk)
+{
+    return sk->commanded_pos;
+}

klippy/chelper/itersolve.h

@@ -0,0 +1,41 @@
+#ifndef ITERSOLVE_H
+#define ITERSOLVE_H
+
+#include <stdint.h> // int32_t
+
+enum {
+    AF_X = 1 << 0, AF_Y = 1 << 1, AF_Z = 1 << 2,
+};
+
+struct stepper_kinematics;
+struct move;
+typedef double (*sk_calc_callback)(struct stepper_kinematics *sk, struct move *m
+                                   , double move_time);
+typedef void (*sk_post_callback)(struct stepper_kinematics *sk);
+struct stepper_kinematics {
+    double step_dist, commanded_pos;
+    struct stepcompress *sc;
+
+    double last_flush_time, last_move_time;
+    struct trapq *tq;
+    int active_flags;
+    double gen_steps_pre_active, gen_steps_post_active;
+
+    sk_calc_callback calc_position_cb;
+    sk_post_callback post_cb;
+};
+
+int32_t itersolve_generate_steps(struct stepper_kinematics *sk
+                                 , double flush_time);
+double itersolve_check_active(struct stepper_kinematics *sk, double flush_time);
+int32_t itersolve_is_active_axis(struct stepper_kinematics *sk, char axis);
+void itersolve_set_trapq(struct stepper_kinematics *sk, struct trapq *tq);
+void itersolve_set_stepcompress(struct stepper_kinematics *sk
+                                , struct stepcompress *sc, double step_dist);
+double itersolve_calc_position_from_coord(struct stepper_kinematics *sk
+                                          , double x, double y, double z);
+void itersolve_set_position(struct stepper_kinematics *sk
+                            , double x, double y, double z);
+double itersolve_get_commanded_pos(struct stepper_kinematics *sk);
+
+#endif // itersolve.h

klippy/chelper/kin_cartesian.c

@@ -0,0 +1,90 @@
+// Cartesian kinematics stepper pulse time generation
+//
+// Copyright (C) 2018-2019  Kevin O'Connor <kevin@koconnor.net>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include <stdlib.h> // malloc
+#include <string.h> // memset
+#include "compiler.h" // __visible
+#include "itersolve.h" // struct stepper_kinematics
+#include "pyhelper.h" // errorf
+#include "trapq.h" // move_get_coord
+
+static double
+cart_stepper_x_calc_position(struct stepper_kinematics *sk, struct move *m
+                             , double move_time)
+{
+    return move_get_coord(m, move_time).x;
+}
+
+static double
+cart_stepper_y_calc_position(struct stepper_kinematics *sk, struct move *m
+                             , double move_time)
+{
+    return move_get_coord(m, move_time).y;
+}
+
+static double
+cart_stepper_z_calc_position(struct stepper_kinematics *sk, struct move *m
+                             , double move_time)
+{
+    return move_get_coord(m, move_time).z;
+}
+
+struct stepper_kinematics * __visible
+cartesian_stepper_alloc(char axis)
+{
+    struct stepper_kinematics *sk = malloc(sizeof(*sk));
+    memset(sk, 0, sizeof(*sk));
+    if (axis == 'x') {
+        sk->calc_position_cb = cart_stepper_x_calc_position;
+        sk->active_flags = AF_X;
+    } else if (axis == 'y') {
+        sk->calc_position_cb = cart_stepper_y_calc_position;
+        sk->active_flags = AF_Y;
+    } else if (axis == 'z') {
+        sk->calc_position_cb = cart_stepper_z_calc_position;
+        sk->active_flags = AF_Z;
+    }
+    return sk;
+}
+
+static double
+cart_reverse_stepper_x_calc_position(struct stepper_kinematics *sk
+                             , struct move *m, double move_time)
+{
+    return -move_get_coord(m, move_time).x;
+}
+
+static double
+cart_reverse_stepper_y_calc_position(struct stepper_kinematics *sk
+                             , struct move *m, double move_time)
+{
+    return -move_get_coord(m, move_time).y;
+}
+
+static double
+cart_reverse_stepper_z_calc_position(struct stepper_kinematics *sk
+                             , struct move *m, double move_time)
+{
+    return -move_get_coord(m, move_time).z;
+}
+
+struct stepper_kinematics * __visible
+cartesian_reverse_stepper_alloc(char axis)
+{
+    struct stepper_kinematics *sk = malloc(sizeof(*sk));
+    memset(sk, 0, sizeof(*sk));
+    if (axis == 'x') {
+        sk->calc_position_cb = cart_reverse_stepper_x_calc_position;
+        sk->active_flags = AF_X;
+    } else if (axis == 'y') {
+        sk->calc_position_cb = cart_reverse_stepper_y_calc_position;
+        sk->active_flags = AF_Y;
+    } else if (axis == 'z') {
+        sk->calc_position_cb = cart_reverse_stepper_z_calc_position;
+        sk->active_flags = AF_Z;
+    }
+    return sk;
+}

klippy/chelper/kin_corexy.c

@@ -0,0 +1,40 @@
+// CoreXY kinematics stepper pulse time generation
+//
+// Copyright (C) 2018-2019  Kevin O'Connor <kevin@koconnor.net>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include <stdlib.h> // malloc
+#include <string.h> // memset
+#include "compiler.h" // __visible
+#include "itersolve.h" // struct stepper_kinematics
+#include "trapq.h" // move_get_coord
+
+static double
+corexy_stepper_plus_calc_position(struct stepper_kinematics *sk, struct move *m
+                                  , double move_time)
+{
+    struct coord c = move_get_coord(m, move_time);
+    return c.x + c.y;
+}
+
+static double
+corexy_stepper_minus_calc_position(struct stepper_kinematics *sk, struct move *m
+                                   , double move_time)
+{
+    struct coord c = move_get_coord(m, move_time);
+    return c.x - c.y;
+}
+
+struct stepper_kinematics * __visible
+corexy_stepper_alloc(char type)
+{
+    struct stepper_kinematics *sk = malloc(sizeof(*sk));
+    memset(sk, 0, sizeof(*sk));
+    if (type == '+')
+        sk->calc_position_cb = corexy_stepper_plus_calc_position;
+    else if (type == '-')
+        sk->calc_position_cb = corexy_stepper_minus_calc_position;
+    sk->active_flags = AF_X | AF_Y;
+    return sk;
+}

klippy/chelper/kin_corexz.c

@@ -0,0 +1,40 @@
+// CoreXZ kinematics stepper pulse time generation
+//
+// Copyright (C) 2020  Maks Zolin <mzolin@vorondesign.com>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include <stdlib.h> // malloc
+#include <string.h> // memset
+#include "compiler.h" // __visible
+#include "itersolve.h" // struct stepper_kinematics
+#include "trapq.h" // move_get_coord
+
+static double
+corexz_stepper_plus_calc_position(struct stepper_kinematics *sk, struct move *m
+                                  , double move_time)
+{
+    struct coord c = move_get_coord(m, move_time);
+    return c.x + c.z;
+}
+
+static double
+corexz_stepper_minus_calc_position(struct stepper_kinematics *sk, struct move *m
+                                   , double move_time)
+{
+    struct coord c = move_get_coord(m, move_time);
+    return c.x - c.z;
+}
+
+struct stepper_kinematics * __visible
+corexz_stepper_alloc(char type)
+{
+    struct stepper_kinematics *sk = malloc(sizeof(*sk));
+    memset(sk, 0, sizeof(*sk));
+    if (type == '+')
+        sk->calc_position_cb = corexz_stepper_plus_calc_position;
+    else if (type == '-')
+        sk->calc_position_cb = corexz_stepper_minus_calc_position;
+    sk->active_flags = AF_X | AF_Z;
+    return sk;
+}

klippy/chelper/kin_delta.c

@@ -0,0 +1,41 @@
+// Delta kinematics stepper pulse time generation
+//
+// Copyright (C) 2018-2019  Kevin O'Connor <kevin@koconnor.net>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include <math.h> // sqrt
+#include <stddef.h> // offsetof
+#include <stdlib.h> // malloc
+#include <string.h> // memset
+#include "compiler.h" // __visible
+#include "itersolve.h" // struct stepper_kinematics
+#include "trapq.h" // move_get_coord
+
+struct delta_stepper {
+    struct stepper_kinematics sk;
+    double arm2, tower_x, tower_y;
+};
+
+static double
+delta_stepper_calc_position(struct stepper_kinematics *sk, struct move *m
+                            , double move_time)
+{
+    struct delta_stepper *ds = container_of(sk, struct delta_stepper, sk);
+    struct coord c = move_get_coord(m, move_time);
+    double dx = ds->tower_x - c.x, dy = ds->tower_y - c.y;
+    return sqrt(ds->arm2 - dx*dx - dy*dy) + c.z;
+}
+
+struct stepper_kinematics * __visible
+delta_stepper_alloc(double arm2, double tower_x, double tower_y)
+{
+    struct delta_stepper *ds = malloc(sizeof(*ds));
+    memset(ds, 0, sizeof(*ds));
+    ds->arm2 = arm2;
+    ds->tower_x = tower_x;
+    ds->tower_y = tower_y;
+    ds->sk.calc_position_cb = delta_stepper_calc_position;
+    ds->sk.active_flags = AF_X | AF_Y | AF_Z;
+    return &ds->sk;
+}

klippy/chelper/kin_extruder.c

@@ -0,0 +1,145 @@
+// Extruder stepper pulse time generation
+//
+// Copyright (C) 2018-2019  Kevin O'Connor <kevin@koconnor.net>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include <stddef.h> // offsetof
+#include <stdlib.h> // malloc
+#include <string.h> // memset
+#include "compiler.h" // __visible
+#include "itersolve.h" // struct stepper_kinematics
+#include "pyhelper.h" // errorf
+#include "trapq.h" // move_get_distance
+
+// Without pressure advance, the extruder stepper position is:
+//     extruder_position(t) = nominal_position(t)
+// When pressure advance is enabled, additional filament is pushed
+// into the extruder during acceleration (and retracted during
+// deceleration). The formula is:
+//     pa_position(t) = (nominal_position(t)
+//                       + pressure_advance * nominal_velocity(t))
+// Which is then "smoothed" using a weighted average:
+//     smooth_position(t) = (
+//         definitive_integral(pa_position(x) * (smooth_time/2 - abs(t-x)) * dx,
+//                             from=t-smooth_time/2, to=t+smooth_time/2)
+//         / ((smooth_time/2)**2))
+
+// Calculate the definitive integral of the motion formula:
+//   position(t) = base + t * (start_v + t * half_accel)
+static double
+extruder_integrate(double base, double start_v, double half_accel
+                   , double start, double end)
+{
+    double half_v = .5 * start_v, sixth_a = (1. / 3.) * half_accel;
+    double si = start * (base + start * (half_v + start * sixth_a));
+    double ei = end * (base + end * (half_v + end * sixth_a));
+    return ei - si;
+}
+
+// Calculate the definitive integral of time weighted position:
+//   weighted_position(t) = t * (base + t * (start_v + t * half_accel))
+static double
+extruder_integrate_time(double base, double start_v, double half_accel
+                        , double start, double end)
+{
+    double half_b = .5 * base, third_v = (1. / 3.) * start_v;
+    double eighth_a = .25 * half_accel;
+    double si = start * start * (half_b + start * (third_v + start * eighth_a));
+    double ei = end * end * (half_b + end * (third_v + end * eighth_a));
+    return ei - si;
+}
+
+// Calculate the definitive integral of extruder for a given move
+static double
+pa_move_integrate(struct move *m, double pressure_advance
+                  , double base, double start, double end, double time_offset)
+{
+    if (start < 0.)
+        start = 0.;
+    if (end > m->move_t)
+        end = m->move_t;
+    // Calculate base position and velocity with pressure advance
+    int can_pressure_advance = m->axes_r.y != 0.;
+    if (!can_pressure_advance)
+        pressure_advance = 0.;
+    base += pressure_advance * m->start_v;
+    double start_v = m->start_v + pressure_advance * 2. * m->half_accel;
+    // Calculate definitive integral
+    double ha = m->half_accel;
+    double iext = extruder_integrate(base, start_v, ha, start, end);
+    double wgt_ext = extruder_integrate_time(base, start_v, ha, start, end);
+    return wgt_ext - time_offset * iext;
+}
+
+// Calculate the definitive integral of the extruder over a range of moves
+static double
+pa_range_integrate(struct move *m, double move_time
+                   , double pressure_advance, double hst)
+{
+    // Calculate integral for the current move
+    double res = 0., start = move_time - hst, end = move_time + hst;
+    double start_base = m->start_pos.x;
+    res += pa_move_integrate(m, pressure_advance, 0., start, move_time, start);
+    res -= pa_move_integrate(m, pressure_advance, 0., move_time, end, end);
+    // Integrate over previous moves
+    struct move *prev = m;
+    while (unlikely(start < 0.)) {
+        prev = list_prev_entry(prev, node);
+        start += prev->move_t;
+        double base = prev->start_pos.x - start_base;
+        res += pa_move_integrate(prev, pressure_advance, base, start
+                                 , prev->move_t, start);
+    }
+    // Integrate over future moves
+    while (unlikely(end > m->move_t)) {
+        end -= m->move_t;
+        m = list_next_entry(m, node);
+        double base = m->start_pos.x - start_base;
+        res -= pa_move_integrate(m, pressure_advance, base, 0., end, end);
+    }
+    return res;
+}
+
+struct extruder_stepper {
+    struct stepper_kinematics sk;
+    double pressure_advance, half_smooth_time, inv_half_smooth_time2;
+};
+
+static double
+extruder_calc_position(struct stepper_kinematics *sk, struct move *m
+                       , double move_time)
+{
+    struct extruder_stepper *es = container_of(sk, struct extruder_stepper, sk);
+    double hst = es->half_smooth_time;
+    if (!hst)
+        // Pressure advance not enabled
+        return m->start_pos.x + move_get_distance(m, move_time);
+    // Apply pressure advance and average over smooth_time
+    double area = pa_range_integrate(m, move_time, es->pressure_advance, hst);
+    return m->start_pos.x + area * es->inv_half_smooth_time2;
+}
+
+void __visible
+extruder_set_pressure_advance(struct stepper_kinematics *sk
+                              , double pressure_advance, double smooth_time)
+{
+    struct extruder_stepper *es = container_of(sk, struct extruder_stepper, sk);
+    double hst = smooth_time * .5;
+    es->half_smooth_time = hst;
+    es->sk.gen_steps_pre_active = es->sk.gen_steps_post_active = hst;
+    if (! hst)
+        return;
+    es->inv_half_smooth_time2 = 1. / (hst * hst);
+    es->pressure_advance = pressure_advance;
+}
+
+struct stepper_kinematics * __visible
+extruder_stepper_alloc(void)
+{
+    struct extruder_stepper *es = malloc(sizeof(*es));
+    memset(es, 0, sizeof(*es));
+    es->sk.calc_position_cb = extruder_calc_position;
+    es->sk.active_flags = AF_X;
+    return &es->sk;
+}

klippy/chelper/kin_polar.c

@@ -0,0 +1,59 @@
+// Polar kinematics stepper pulse time generation
+//
+// Copyright (C) 2018-2019  Kevin O'Connor <kevin@koconnor.net>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include <math.h> // sqrt
+#include <stdlib.h> // malloc
+#include <string.h> // memset
+#include "compiler.h" // __visible
+#include "itersolve.h" // struct stepper_kinematics
+#include "trapq.h" // move_get_coord
+
+static double
+polar_stepper_radius_calc_position(struct stepper_kinematics *sk, struct move *m
+                                   , double move_time)
+{
+    struct coord c = move_get_coord(m, move_time);
+    return sqrt(c.x*c.x + c.y*c.y);
+}
+
+static double
+polar_stepper_angle_calc_position(struct stepper_kinematics *sk, struct move *m
+                                  , double move_time)
+{
+    struct coord c = move_get_coord(m, move_time);
+    // XXX - handle x==y==0
+    double angle = atan2(c.y, c.x);
+    if (angle - sk->commanded_pos > M_PI)
+        angle -= 2. * M_PI;
+    else if (angle - sk->commanded_pos < -M_PI)
+        angle += 2. * M_PI;
+    return angle;
+}
+
+static void
+polar_stepper_angle_post_fixup(struct stepper_kinematics *sk)
+{
+    // Normalize the stepper_bed angle
+    if (sk->commanded_pos < -M_PI)
+        sk->commanded_pos += 2 * M_PI;
+    else if (sk->commanded_pos > M_PI)
+        sk->commanded_pos -= 2 * M_PI;
+}
+
+struct stepper_kinematics * __visible
+polar_stepper_alloc(char type)
+{
+    struct stepper_kinematics *sk = malloc(sizeof(*sk));
+    memset(sk, 0, sizeof(*sk));
+    if (type == 'r') {
+        sk->calc_position_cb = polar_stepper_radius_calc_position;
+    } else if (type == 'a') {
+        sk->calc_position_cb = polar_stepper_angle_calc_position;
+        sk->post_cb = polar_stepper_angle_post_fixup;
+    }
+    sk->active_flags = AF_X | AF_Y;
+    return sk;
+}

klippy/chelper/kin_rotary_delta.c

@@ -0,0 +1,73 @@
+// Rotary delta kinematics stepper pulse time generation
+//
+// Copyright (C) 2019  Kevin O'Connor <kevin@koconnor.net>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include <math.h> // sqrt
+#include <stddef.h> // offsetof
+#include <stdlib.h> // malloc
+#include <string.h> // memset
+#include "compiler.h" // __visible
+#include "itersolve.h" // struct stepper_kinematics
+#include "trapq.h" // move_get_coord
+
+// The arm angle calculation is based on the following two formulas:
+//   elbow_x**2 + elbow_y**2 = upper_arm**2
+//   (effector_x - elbow_x)**2 + (effector_y - elbow_y)**2 = lower_arm**2
+
+// Calculate upper arm angle given xy position of effector joint
+// (relative to shoulder joint), upper arm length, and lower arm length.
+static inline double
+rotary_two_arm_calc(double dx, double dy, double upper_arm2, double lower_arm2)
+{
+    // Determine constants such that: elbow_y = c1 - c2*elbow_x
+    double inv_dy = 1. / dy;
+    double c1 = .5 * inv_dy * (dx*dx + dy*dy + upper_arm2 - lower_arm2);
+    double c2 = dx * inv_dy;
+    // Calculate scaled elbow coordinates via quadratic equation.
+    double scale = c2*c2 + 1.0;
+    double scaled_elbow_x = c1*c2 + sqrt(scale*upper_arm2 - c1*c1);
+    double scaled_elbow_y = c1*scale - c2*scaled_elbow_x;
+    // Calculate angle in radians
+    return atan2(scaled_elbow_y, scaled_elbow_x);
+}
+
+struct rotary_stepper {
+    struct stepper_kinematics sk;
+    double cos, sin, shoulder_radius, shoulder_height;
+    double upper_arm2, lower_arm2;
+};
+
+static double
+rotary_stepper_calc_position(struct stepper_kinematics *sk, struct move *m
+                             , double move_time)
+{
+    struct rotary_stepper *rs = container_of(sk, struct rotary_stepper, sk);
+    struct coord c = move_get_coord(m, move_time);
+    // Rotate and shift axes to an origin at shoulder joint with upper
+    // arm constrained to xy plane and x aligned to shoulder platform.
+    double sjz = c.y * rs->cos - c.x * rs->sin;
+    double sjx = c.x * rs->cos + c.y * rs->sin - rs->shoulder_radius;
+    double sjy = c.z - rs->shoulder_height;
+    // Calculate angle in radians
+    return rotary_two_arm_calc(sjx, sjy, rs->upper_arm2
+                               , rs->lower_arm2 - sjz*sjz);
+}
+
+struct stepper_kinematics * __visible
+rotary_delta_stepper_alloc(double shoulder_radius, double shoulder_height
+                           , double angle, double upper_arm, double lower_arm)
+{
+    struct rotary_stepper *rs = malloc(sizeof(*rs));
+    memset(rs, 0, sizeof(*rs));
+    rs->cos = cos(angle);
+    rs->sin = sin(angle);
+    rs->shoulder_radius = shoulder_radius;
+    rs->shoulder_height = shoulder_height;
+    rs->upper_arm2 = upper_arm * upper_arm;
+    rs->lower_arm2 = lower_arm * lower_arm;
+    rs->sk.calc_position_cb = rotary_stepper_calc_position;
+    rs->sk.active_flags = AF_X | AF_Y | AF_Z;
+    return &rs->sk;
+}

klippy/chelper/kin_shaper.c

@@ -0,0 +1,232 @@
+// Kinematic input shapers to minimize motion vibrations in XY plane
+//
+// Copyright (C) 2019-2020  Kevin O'Connor <kevin@koconnor.net>
+// Copyright (C) 2020  Dmitry Butyugin <dmbutyugin@google.com>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include <math.h> // sqrt, exp
+#include <stddef.h> // offsetof
+#include <stdlib.h> // malloc
+#include <string.h> // memset
+#include "compiler.h" // __visible
+#include "itersolve.h" // struct stepper_kinematics
+#include "trapq.h" // struct move
+
+
+/****************************************************************
+ * Shaper initialization
+ ****************************************************************/
+
+struct shaper_pulses {
+    int num_pulses;
+    struct {
+        double t, a;
+    } pulses[5];
+};
+
+// Shift pulses around 'mid-point' t=0 so that the input shaper is an identity
+// transformation for constant-speed motion (i.e. input_shaper(v * T) = v * T)
+static void
+shift_pulses(struct shaper_pulses *sp)
+{
+    int i;
+    double ts = 0.;
+    for (i = 0; i < sp->num_pulses; ++i)
+        ts += sp->pulses[i].a * sp->pulses[i].t;
+    for (i = 0; i < sp->num_pulses; ++i)
+        sp->pulses[i].t -= ts;
+}
+
+static int
+init_shaper(int n, double a[], double t[], struct shaper_pulses *sp)
+{
+    if (n < 0 || n > ARRAY_SIZE(sp->pulses)) {
+        sp->num_pulses = 0;
+        return -1;
+    }
+    int i;
+    double sum_a = 0.;
+    for (i = 0; i < n; ++i)
+        sum_a += a[i];
+    double inv_a = 1. / sum_a;
+    // Reverse pulses vs their traditional definition
+    for (i = 0; i < n; ++i) {
+        sp->pulses[n-i-1].a = a[i] * inv_a;
+        sp->pulses[n-i-1].t = -t[i];
+    }
+    sp->num_pulses = n;
+    shift_pulses(sp);
+    return 0;
+}
+
+
+/****************************************************************
+ * Generic position calculation via shaper convolution
+ ****************************************************************/
+
+static inline double
+get_axis_position(struct move *m, int axis, double move_time)
+{
+    double axis_r = m->axes_r.axis[axis - 'x'];
+    double start_pos = m->start_pos.axis[axis - 'x'];
+    double move_dist = move_get_distance(m, move_time);
+    return start_pos + axis_r * move_dist;
+}
+
+static inline double
+get_axis_position_across_moves(struct move *m, int axis, double time)
+{
+    while (likely(time < 0.)) {
+        m = list_prev_entry(m, node);
+        time += m->move_t;
+    }
+    while (likely(time > m->move_t)) {
+        time -= m->move_t;
+        m = list_next_entry(m, node);
+    }
+    return get_axis_position(m, axis, time);
+}
+
+// Calculate the position from the convolution of the shaper with input signal
+static inline double
+calc_position(struct move *m, int axis, double move_time
+              , struct shaper_pulses *sp)
+{
+    double res = 0.;
+    int num_pulses = sp->num_pulses, i;
+    for (i = 0; i < num_pulses; ++i) {
+        double t = sp->pulses[i].t, a = sp->pulses[i].a;
+        res += a * get_axis_position_across_moves(m, axis, move_time + t);
+    }
+    return res;
+}
+
+
+/****************************************************************
+ * Kinematics-related shaper code
+ ****************************************************************/
+
+#define DUMMY_T 500.0
+
+struct input_shaper {
+    struct stepper_kinematics sk;
+    struct stepper_kinematics *orig_sk;
+    struct move m;
+    struct shaper_pulses sx, sy;
+};
+
+// Optimized calc_position when only x axis is needed
+static double
+shaper_x_calc_position(struct stepper_kinematics *sk, struct move *m
+                       , double move_time)
+{
+    struct input_shaper *is = container_of(sk, struct input_shaper, sk);
+    if (!is->sx.num_pulses)
+        return is->orig_sk->calc_position_cb(is->orig_sk, m, move_time);
+    is->m.start_pos.x = calc_position(m, 'x', move_time, &is->sx);
+    return is->orig_sk->calc_position_cb(is->orig_sk, &is->m, DUMMY_T);
+}
+
+// Optimized calc_position when only y axis is needed
+static double
+shaper_y_calc_position(struct stepper_kinematics *sk, struct move *m
+                       , double move_time)
+{
+    struct input_shaper *is = container_of(sk, struct input_shaper, sk);
+    if (!is->sy.num_pulses)
+        return is->orig_sk->calc_position_cb(is->orig_sk, m, move_time);
+    is->m.start_pos.y = calc_position(m, 'y', move_time, &is->sy);
+    return is->orig_sk->calc_position_cb(is->orig_sk, &is->m, DUMMY_T);
+}
+
+// General calc_position for both x and y axes
+static double
+shaper_xy_calc_position(struct stepper_kinematics *sk, struct move *m
+                        , double move_time)
+{
+    struct input_shaper *is = container_of(sk, struct input_shaper, sk);
+    if (!is->sx.num_pulses && !is->sy.num_pulses)
+        return is->orig_sk->calc_position_cb(is->orig_sk, m, move_time);
+    is->m.start_pos = move_get_coord(m, move_time);
+    if (is->sx.num_pulses)
+        is->m.start_pos.x = calc_position(m, 'x', move_time, &is->sx);
+    if (is->sy.num_pulses)
+        is->m.start_pos.y = calc_position(m, 'y', move_time, &is->sy);
+    return is->orig_sk->calc_position_cb(is->orig_sk, &is->m, DUMMY_T);
+}
+
+int __visible
+input_shaper_set_sk(struct stepper_kinematics *sk
+                    , struct stepper_kinematics *orig_sk)
+{
+    struct input_shaper *is = container_of(sk, struct input_shaper, sk);
+    if (orig_sk->active_flags == AF_X)
+        is->sk.calc_position_cb = shaper_x_calc_position;
+    else if (orig_sk->active_flags == AF_Y)
+        is->sk.calc_position_cb = shaper_y_calc_position;
+    else if (orig_sk->active_flags & (AF_X | AF_Y))
+        is->sk.calc_position_cb = shaper_xy_calc_position;
+    else
+        return -1;
+    is->sk.active_flags = orig_sk->active_flags;
+    is->orig_sk = orig_sk;
+    return 0;
+}
+
+static void
+shaper_note_generation_time(struct input_shaper *is)
+{
+    double pre_active = 0., post_active = 0.;
+    if ((is->sk.active_flags & AF_X) && is->sx.num_pulses) {
+        pre_active = is->sx.pulses[is->sx.num_pulses-1].t;
+        post_active = -is->sx.pulses[0].t;
+    }
+    if ((is->sk.active_flags & AF_Y) && is->sy.num_pulses) {
+        pre_active = is->sy.pulses[is->sy.num_pulses-1].t > pre_active
+            ? is->sy.pulses[is->sy.num_pulses-1].t : pre_active;
+        post_active = -is->sy.pulses[0].t > post_active
+            ? -is->sy.pulses[0].t : post_active;
+    }
+    is->sk.gen_steps_pre_active = pre_active;
+    is->sk.gen_steps_post_active = post_active;
+}
+
+int __visible
+input_shaper_set_shaper_params(struct stepper_kinematics *sk, char axis
+                               , int n, double a[], double t[])
+{
+    if (axis != 'x' && axis != 'y')
+        return -1;
+    struct input_shaper *is = container_of(sk, struct input_shaper, sk);
+    struct shaper_pulses *sp = axis == 'x' ? &is->sx : &is->sy;
+    int status = 0;
+    if (is->orig_sk->active_flags & (axis == 'x' ? AF_X : AF_Y))
+        status = init_shaper(n, a, t, sp);
+    else
+        sp->num_pulses = 0;
+    shaper_note_generation_time(is);
+    return status;
+}
+
+double __visible
+input_shaper_get_step_generation_window(int n, double a[], double t[])
+{
+    struct shaper_pulses sp;
+    init_shaper(n, a, t, &sp);
+    if (!sp.num_pulses)
+        return 0.;
+    double window = -sp.pulses[0].t;
+    if (sp.pulses[sp.num_pulses-1].t > window)
+        window = sp.pulses[sp.num_pulses-1].t;
+    return window;
+}
+
+struct stepper_kinematics * __visible
+input_shaper_alloc(void)
+{
+    struct input_shaper *is = malloc(sizeof(*is));
+    memset(is, 0, sizeof(*is));
+    is->m.move_t = 2. * DUMMY_T;
+    return &is->sk;
+}

klippy/chelper/kin_winch.c

@@ -0,0 +1,42 @@
+// Cable winch stepper kinematics
+//
+// Copyright (C) 2018-2019  Kevin O'Connor <kevin@koconnor.net>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include <math.h> // sqrt
+#include <stddef.h> // offsetof
+#include <stdlib.h> // malloc
+#include <string.h> // memset
+#include "compiler.h" // __visible
+#include "itersolve.h" // struct stepper_kinematics
+#include "trapq.h" // move_get_coord
+
+struct winch_stepper {
+    struct stepper_kinematics sk;
+    struct coord anchor;
+};
+
+static double
+winch_stepper_calc_position(struct stepper_kinematics *sk, struct move *m
+                            , double move_time)
+{
+    struct winch_stepper *hs = container_of(sk, struct winch_stepper, sk);
+    struct coord c = move_get_coord(m, move_time);
+    double dx = hs->anchor.x - c.x, dy = hs->anchor.y - c.y;
+    double dz = hs->anchor.z - c.z;
+    return sqrt(dx*dx + dy*dy + dz*dz);
+}
+
+struct stepper_kinematics * __visible
+winch_stepper_alloc(double anchor_x, double anchor_y, double anchor_z)
+{
+    struct winch_stepper *hs = malloc(sizeof(*hs));
+    memset(hs, 0, sizeof(*hs));
+    hs->anchor.x = anchor_x;
+    hs->anchor.y = anchor_y;
+    hs->anchor.z = anchor_z;
+    hs->sk.calc_position_cb = winch_stepper_calc_position;
+    hs->sk.active_flags = AF_X | AF_Y | AF_Z;
+    return &hs->sk;
+}

klippy/chelper/list.h

@@ -0,0 +1,126 @@
+#ifndef __LIST_H
+#define __LIST_H
+
+#define container_of(ptr, type, member) ({                      \
+        const typeof( ((type *)0)->member ) *__mptr = (ptr);    \
+        (type *)( (char *)__mptr - offsetof(type,member) );})
+
+
+/****************************************************************
+ * list - Double linked lists
+ ****************************************************************/
+
+struct list_node {
+    struct list_node *next, *prev;
+};
+
+struct list_head {
+    struct list_node root;
+};
+
+static inline void
+list_init(struct list_head *h)
+{
+    h->root.prev = h->root.next = &h->root;
+}
+
+static inline int
+list_empty(const struct list_head *h)
+{
+    return h->root.next == &h->root;
+}
+
+static inline int
+list_is_first(const struct list_node *n, const struct list_head *h)
+{
+    return n->prev == &h->root;
+}
+
+static inline int
+list_is_last(const struct list_node *n, const struct list_head *h)
+{
+    return n->next == &h->root;
+}
+
+static inline void
+list_del(struct list_node *n)
+{
+    struct list_node *prev = n->prev;
+    struct list_node *next = n->next;
+    next->prev = prev;
+    prev->next = next;
+}
+
+static inline void
+__list_add(struct list_node *n, struct list_node *prev, struct list_node *next)
+{
+    next->prev = n;
+    n->next = next;
+    n->prev = prev;
+    prev->next = n;
+}
+
+static inline void
+list_add_after(struct list_node *n, struct list_node *prev)
+{
+    __list_add(n, prev, prev->next);
+}
+
+static inline void
+list_add_before(struct list_node *n, struct list_node *next)
+{
+    __list_add(n, next->prev, next);
+}
+
+static inline void
+list_add_head(struct list_node *n, struct list_head *h)
+{
+    list_add_after(n, &h->root);
+}
+
+static inline void
+list_add_tail(struct list_node *n, struct list_head *h)
+{
+    list_add_before(n, &h->root);
+}
+
+static inline void
+list_join_tail(struct list_head *add, struct list_head *h)
+{
+    if (!list_empty(add)) {
+        struct list_node *prev = h->root.prev;
+        struct list_node *next = &h->root;
+        struct list_node *first = add->root.next;
+        struct list_node *last = add->root.prev;
+        first->prev = prev;
+        prev->next = first;
+        last->next = next;
+        next->prev = last;
+    }
+}
+
+#define list_next_entry(pos, member)                            \
+    container_of((pos)->member.next, typeof(*pos), member)
+
+#define list_prev_entry(pos, member)                            \
+    container_of((pos)->member.prev, typeof(*pos), member)
+
+#define list_first_entry(head, type, member)                    \
+    container_of((head)->root.next, type, member)
+
+#define list_last_entry(head, type, member)                     \
+    container_of((head)->root.prev, type, member)
+
+#define list_for_each_entry(pos, head, member)                  \
+    for (pos = list_first_entry((head), typeof(*pos), member)   \
+         ; &pos->member != &(head)->root                        \
+         ; pos = list_next_entry(pos, member))
+
+#define list_for_each_entry_safe(pos, n, head, member)          \
+    for (pos = list_first_entry((head), typeof(*pos), member)   \
+          , n = list_next_entry(pos, member)                    \
+         ; &pos->member != &(head)->root                        \
+         ; pos = n, n = list_next_entry(n, member))
+
+
+#endif // list.h

klippy/chelper/msgblock.c

@@ -0,0 +1,209 @@
+// Helper code for the Klipper mcu protocol "message blocks"
+//
+// Copyright (C) 2016-2021  Kevin O'Connor <kevin@koconnor.net>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include <stddef.h> // offsetof
+#include <stdlib.h> // malloc
+#include <string.h> // memset
+#include "msgblock.h" // message_alloc
+#include "pyhelper.h" // errorf
+
+
+/****************************************************************
+ * Serial protocol helpers
+ ****************************************************************/
+
+// Implement the standard crc "ccitt" algorithm on the given buffer
+uint16_t
+msgblock_crc16_ccitt(uint8_t *buf, uint8_t len)
+{
+    uint16_t crc = 0xffff;
+    while (len--) {
+        uint8_t data = *buf++;
+        data ^= crc & 0xff;
+        data ^= data << 4;
+        crc = ((((uint16_t)data << 8) | (crc >> 8)) ^ (uint8_t)(data >> 4)
+               ^ ((uint16_t)data << 3));
+    }
+    return crc;
+}
+
+// Verify a buffer starts with a valid mcu message
+int
+msgblock_check(uint8_t *need_sync, uint8_t *buf, int buf_len)
+{
+    if (buf_len < MESSAGE_MIN)
+        // Need more data
+        return 0;
+    if (*need_sync)
+        goto error;
+    uint8_t msglen = buf[MESSAGE_POS_LEN];
+    if (msglen < MESSAGE_MIN || msglen > MESSAGE_MAX)
+        goto error;
+    uint8_t msgseq = buf[MESSAGE_POS_SEQ];
+    if ((msgseq & ~MESSAGE_SEQ_MASK) != MESSAGE_DEST)
+        goto error;
+    if (buf_len < msglen)
+        // Need more data
+        return 0;
+    if (buf[msglen-MESSAGE_TRAILER_SYNC] != MESSAGE_SYNC)
+        goto error;
+    uint16_t msgcrc = ((buf[msglen-MESSAGE_TRAILER_CRC] << 8)
+                       | (uint8_t)buf[msglen-MESSAGE_TRAILER_CRC+1]);
+    uint16_t crc = msgblock_crc16_ccitt(buf, msglen-MESSAGE_TRAILER_SIZE);
+    if (crc != msgcrc)
+        goto error;
+    return msglen;
+
+error: ;
+    // Discard bytes until next SYNC found
+    uint8_t *next_sync = memchr(buf, MESSAGE_SYNC, buf_len);
+    if (next_sync) {
+        *need_sync = 0;
+        return -(next_sync - buf + 1);
+    }
+    *need_sync = 1;
+    return -buf_len;
+}
+
+// Encode an integer as a variable length quantity (vlq)
+static uint8_t *
+encode_int(uint8_t *p, uint32_t v)
+{
+    int32_t sv = v;
+    if (sv < (3L<<5)  && sv >= -(1L<<5))  goto f4;
+    if (sv < (3L<<12) && sv >= -(1L<<12)) goto f3;
+    if (sv < (3L<<19) && sv >= -(1L<<19)) goto f2;
+    if (sv < (3L<<26) && sv >= -(1L<<26)) goto f1;
+    *p++ = (v>>28) | 0x80;
+f1: *p++ = ((v>>21) & 0x7f) | 0x80;
+f2: *p++ = ((v>>14) & 0x7f) | 0x80;
+f3: *p++ = ((v>>7) & 0x7f) | 0x80;
+f4: *p++ = v & 0x7f;
+    return p;
+}
+
+// Parse an integer that was encoded as a "variable length quantity"
+static uint32_t
+parse_int(uint8_t **pp)
+{
+    uint8_t *p = *pp, c = *p++;
+    uint32_t v = c & 0x7f;
+    if ((c & 0x60) == 0x60)
+        v |= -0x20;
+    while (c & 0x80) {
+        c = *p++;
+        v = (v<<7) | (c & 0x7f);
+    }
+    *pp = p;
+    return v;
+}
+
+// Parse the VLQ contents of a message
+int
+msgblock_decode(uint32_t *data, int data_len, uint8_t *msg, int msg_len)
+{
+    uint8_t *p = &msg[MESSAGE_HEADER_SIZE];
+    uint8_t *end = &msg[msg_len - MESSAGE_TRAILER_SIZE];
+    while (data_len--) {
+        if (p >= end)
+            return -1;
+        *data++ = parse_int(&p);
+    }
+    if (p != end)
+        // Invalid message
+        return -1;
+    return 0;
+}
+
+
+/****************************************************************
+ * Command queues
+ ****************************************************************/
+
+// Allocate a 'struct queue_message' object
+struct queue_message *
+message_alloc(void)
+{
+    struct queue_message *qm = malloc(sizeof(*qm));
+    memset(qm, 0, sizeof(*qm));
+    return qm;
+}
+
+// Allocate a queue_message and fill it with the specified data
+struct queue_message *
+message_fill(uint8_t *data, int len)
+{
+    struct queue_message *qm = message_alloc();
+    memcpy(qm->msg, data, len);
+    qm->len = len;
+    return qm;
+}
+
+// Allocate a queue_message and fill it with a series of encoded vlq integers
+struct queue_message *
+message_alloc_and_encode(uint32_t *data, int len)
+{
+    struct queue_message *qm = message_alloc();
+    int i;
+    uint8_t *p = qm->msg;
+    for (i=0; i<len; i++) {
+        p = encode_int(p, data[i]);
+        if (p > &qm->msg[MESSAGE_PAYLOAD_MAX])
+            goto fail;
+    }
+    qm->len = p - qm->msg;
+    return qm;
+
+fail:
+    errorf("Encode error");
+    qm->len = 0;
+    return qm;
+}
+
+// Free the storage from a previous message_alloc() call
+void
+message_free(struct queue_message *qm)
+{
+    free(qm);
+}
+
+// Free all the messages on a queue
+void
+message_queue_free(struct list_head *root)
+{
+    while (!list_empty(root)) {
+        struct queue_message *qm = list_first_entry(
+            root, struct queue_message, node);
+        list_del(&qm->node);
+        message_free(qm);
+    }
+}
+
+
+/****************************************************************
+ * Clock estimation
+ ****************************************************************/
+
+// Extend a 32bit clock value to its full 64bit value
+uint64_t
+clock_from_clock32(struct clock_estimate *ce, uint32_t clock32)
+{
+    return ce->last_clock + (int32_t)(clock32 - ce->last_clock);
+}
+
+// Convert a clock to its estimated time
+double
+clock_to_time(struct clock_estimate *ce, uint64_t clock)
+{
+    return ce->conv_time + (int64_t)(clock - ce->conv_clock) / ce->est_freq;
+}
+
+// Convert a time to the nearest clock value
+uint64_t
+clock_from_time(struct clock_estimate *ce, double time)
+{
+    return (int64_t)((time - ce->conv_time)*ce->est_freq + .5) + ce->conv_clock;
+}

klippy/chelper/msgblock.h

@@ -0,0 +1,54 @@
+#ifndef MSGBLOCK_H
+#define MSGBLOCK_H
+
+#include <stdint.h> // uint8_t
+#include "list.h" // struct list_node
+
+#define MESSAGE_MIN 5
+#define MESSAGE_MAX 64
+#define MESSAGE_HEADER_SIZE  2
+#define MESSAGE_TRAILER_SIZE 3
+#define MESSAGE_POS_LEN 0
+#define MESSAGE_POS_SEQ 1
+#define MESSAGE_TRAILER_CRC  3
+#define MESSAGE_TRAILER_SYNC 1
+#define MESSAGE_PAYLOAD_MAX (MESSAGE_MAX - MESSAGE_MIN)
+#define MESSAGE_SEQ_MASK 0x0f
+#define MESSAGE_DEST 0x10
+#define MESSAGE_SYNC 0x7E
+
+struct queue_message {
+    int len;
+    uint8_t msg[MESSAGE_MAX];
+    union {
+        // Filled when on a command queue
+        struct {
+            uint64_t min_clock, req_clock;
+        };
+        // Filled when in sent/receive queues
+        struct {
+            double sent_time, receive_time;
+        };
+    };
+    uint64_t notify_id;
+    struct list_node node;
+};
+
+struct clock_estimate {
+    uint64_t last_clock, conv_clock;
+    double conv_time, est_freq;
+};
+
+uint16_t msgblock_crc16_ccitt(uint8_t *buf, uint8_t len);
+int msgblock_check(uint8_t *need_sync, uint8_t *buf, int buf_len);
+int msgblock_decode(uint32_t *data, int data_len, uint8_t *msg, int msg_len);
+struct queue_message *message_alloc(void);
+struct queue_message *message_fill(uint8_t *data, int len);
+struct queue_message *message_alloc_and_encode(uint32_t *data, int len);
+void message_free(struct queue_message *qm);
+void message_queue_free(struct list_head *root);
+uint64_t clock_from_clock32(struct clock_estimate *ce, uint32_t clock32);
+double clock_to_time(struct clock_estimate *ce, uint64_t clock);
+uint64_t clock_from_time(struct clock_estimate *ce, double time);
+
+#endif // msgblock.h

klippy/chelper/pollreactor.c

@@ -0,0 +1,179 @@
+// Code for dispatching timer and file descriptor events
+//
+// Copyright (C) 2016-2021  Kevin O'Connor <kevin@koconnor.net>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include <fcntl.h> // fcntl
+#include <math.h> // ceil
+#include <poll.h> // poll
+#include <stdlib.h> // malloc
+#include <string.h> // memset
+#include "pollreactor.h" // pollreactor_alloc
+#include "pyhelper.h" // report_errno
+
+struct pollreactor_timer {
+    double waketime;
+    double (*callback)(void *data, double eventtime);
+};
+
+struct pollreactor {
+    int num_fds, num_timers, must_exit;
+    void *callback_data;
+    double next_timer;
+    struct pollfd *fds;
+    void (**fd_callbacks)(void *data, double eventtime);
+    struct pollreactor_timer *timers;
+};
+
+// Allocate a new 'struct pollreactor' object
+struct pollreactor *
+pollreactor_alloc(int num_fds, int num_timers, void *callback_data)
+{
+    struct pollreactor *pr = malloc(sizeof(*pr));
+    memset(pr, 0, sizeof(*pr));
+    pr->num_fds = num_fds;
+    pr->num_timers = num_timers;
+    pr->must_exit = 0;
+    pr->callback_data = callback_data;
+    pr->next_timer = PR_NEVER;
+    pr->fds = malloc(num_fds * sizeof(*pr->fds));
+    memset(pr->fds, 0, num_fds * sizeof(*pr->fds));
+    pr->fd_callbacks = malloc(num_fds * sizeof(*pr->fd_callbacks));
+    memset(pr->fd_callbacks, 0, num_fds * sizeof(*pr->fd_callbacks));
+    pr->timers = malloc(num_timers * sizeof(*pr->timers));
+    memset(pr->timers, 0, num_timers * sizeof(*pr->timers));
+    int i;
+    for (i=0; i<num_timers; i++)
+        pr->timers[i].waketime = PR_NEVER;
+    return pr;
+}
+
+// Free resources associated with a 'struct pollreactor' object
+void
+pollreactor_free(struct pollreactor *pr)
+{
+    free(pr->fds);
+    pr->fds = NULL;
+    free(pr->fd_callbacks);
+    pr->fd_callbacks = NULL;
+    free(pr->timers);
+    pr->timers = NULL;
+    free(pr);
+}
+
+// Add a callback for when a file descriptor (fd) becomes readable
+void
+pollreactor_add_fd(struct pollreactor *pr, int pos, int fd, void *callback
+                   , int write_only)
+{
+    pr->fds[pos].fd = fd;
+    pr->fds[pos].events = POLLHUP | (write_only ? 0 : POLLIN);
+    pr->fds[pos].revents = 0;
+    pr->fd_callbacks[pos] = callback;
+}
+
+// Add a timer callback
+void
+pollreactor_add_timer(struct pollreactor *pr, int pos, void *callback)
+{
+    pr->timers[pos].callback = callback;
+    pr->timers[pos].waketime = PR_NEVER;
+}
+
+// Return the last schedule wake-up time for a timer
+double
+pollreactor_get_timer(struct pollreactor *pr, int pos)
+{
+    return pr->timers[pos].waketime;
+}
+
+// Set the wake-up time for a given timer
+void
+pollreactor_update_timer(struct pollreactor *pr, int pos, double waketime)
+{
+    pr->timers[pos].waketime = waketime;
+    if (waketime < pr->next_timer)
+        pr->next_timer = waketime;
+}
+
+// Internal code to invoke timer callbacks
+static int
+pollreactor_check_timers(struct pollreactor *pr, double eventtime, int busy)
+{
+    if (eventtime >= pr->next_timer) {
+        // Find and run pending timers
+        pr->next_timer = PR_NEVER;
+        int i;
+        for (i=0; i<pr->num_timers; i++) {
+            struct pollreactor_timer *timer = &pr->timers[i];
+            double t = timer->waketime;
+            if (eventtime >= t) {
+                busy = 1;
+                t = timer->callback(pr->callback_data, eventtime);
+                timer->waketime = t;
+            }
+            if (t < pr->next_timer)
+                pr->next_timer = t;
+        }
+    }
+    if (busy)
+        return 0;
+    // Calculate sleep duration
+    double timeout = ceil((pr->next_timer - eventtime) * 1000.);
+    return timeout < 1. ? 1 : (timeout > 1000. ? 1000 : (int)timeout);
+}
+
+// Repeatedly check for timer and fd events and invoke their callbacks
+void
+pollreactor_run(struct pollreactor *pr)
+{
+    double eventtime = get_monotonic();
+    int busy = 1;
+    while (! pr->must_exit) {
+        int timeout = pollreactor_check_timers(pr, eventtime, busy);
+        busy = 0;
+        int ret = poll(pr->fds, pr->num_fds, timeout);
+        eventtime = get_monotonic();
+        if (ret > 0) {
+            busy = 1;
+            int i;
+            for (i=0; i<pr->num_fds; i++)
+                if (pr->fds[i].revents)
+                    pr->fd_callbacks[i](pr->callback_data, eventtime);
+        } else if (ret < 0) {
+            report_errno("poll", ret);
+            pr->must_exit = 1;
+        }
+    }
+}
+
+// Request that a currently running pollreactor_run() loop exit
+void
+pollreactor_do_exit(struct pollreactor *pr)
+{
+    pr->must_exit = 1;
+}
+
+// Check if a pollreactor_run() loop has been requested to exit
+int
+pollreactor_is_exit(struct pollreactor *pr)
+{
+    return pr->must_exit;
+}
+
+int
+fd_set_non_blocking(int fd)
+{
+    int flags = fcntl(fd, F_GETFL);
+    if (flags < 0) {
+        report_errno("fcntl getfl", flags);
+        return -1;
+    }
+    int ret = fcntl(fd, F_SETFL, flags | O_NONBLOCK);
+    if (ret < 0) {
+        report_errno("fcntl setfl", flags);
+        return -1;
+    }
+    return 0;
+}

klippy/chelper/pollreactor.h

@@ -0,0 +1,20 @@
+#ifndef POLLREACTOR_H
+#define POLLREACTOR_H
+
+#define PR_NOW   0.
+#define PR_NEVER 9999999999999999.
+
+struct pollreactor *pollreactor_alloc(int num_fds, int num_timers
+                                      , void *callback_data);
+void pollreactor_free(struct pollreactor *pr);
+void pollreactor_add_fd(struct pollreactor *pr, int pos, int fd, void *callback
+                        , int write_only);
+void pollreactor_add_timer(struct pollreactor *pr, int pos, void *callback);
+double pollreactor_get_timer(struct pollreactor *pr, int pos);
+void pollreactor_update_timer(struct pollreactor *pr, int pos, double waketime);
+void pollreactor_run(struct pollreactor *pr);
+void pollreactor_do_exit(struct pollreactor *pr);
+int pollreactor_is_exit(struct pollreactor *pr);
+int fd_set_non_blocking(int fd);
+
+#endif // pollreactor.h

klippy/chelper/pyhelper.c

@@ -0,0 +1,94 @@
+// Helper functions for C / Python interface
+//
+// Copyright (C) 2016-2018  Kevin O'Connor <kevin@koconnor.net>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include <errno.h> // errno
+#include <stdarg.h> // va_start
+#include <stdint.h> // uint8_t
+#include <stdio.h> // fprintf
+#include <string.h> // strerror
+#include <time.h> // struct timespec
+#include "compiler.h" // __visible
+#include "pyhelper.h" // get_monotonic
+
+// Return the monotonic system time as a double
+double __visible
+get_monotonic(void)
+{
+    struct timespec ts;
+    int ret = clock_gettime(CLOCK_MONOTONIC_RAW, &ts);
+    if (ret) {
+        report_errno("clock_gettime", ret);
+        return 0.;
+    }
+    return (double)ts.tv_sec + (double)ts.tv_nsec * .000000001;
+}
+
+// Fill a 'struct timespec' with a system time stored in a double
+struct timespec
+fill_time(double time)
+{
+    time_t t = time;
+    return (struct timespec) {t, (time - t)*1000000000. };
+}
+
+static void
+default_logger(const char *msg)
+{
+    fprintf(stderr, "%s\n", msg);
+}
+
+static void (*python_logging_callback)(const char *msg) = default_logger;
+
+void __visible
+set_python_logging_callback(void (*func)(const char *))
+{
+    python_logging_callback = func;
+}
+
+// Log an error message
+void
+errorf(const char *fmt, ...)
+{
+    char buf[512];
+    va_list args;
+    va_start(args, fmt);
+    vsnprintf(buf, sizeof(buf), fmt, args);
+    va_end(args);
+    buf[sizeof(buf)-1] = '\0';
+    python_logging_callback(buf);
+}
+
+// Report 'errno' in a message written to stderr
+void
+report_errno(char *where, int rc)
+{
+    int e = errno;
+    errorf("Got error %d in %s: (%d)%s", rc, where, e, strerror(e));
+}
+
+// Return a hex character for a given number
+#define GETHEX(x) ((x) < 10 ? '0' + (x) : 'a' + (x) - 10)
+
+// Translate a binary string into an ASCII string with escape sequences
+char *
+dump_string(char *outbuf, int outbuf_size, char *inbuf, int inbuf_size)
+{
+    char *outend = &outbuf[outbuf_size-5], *o = outbuf;
+    uint8_t *inend = (void*)&inbuf[inbuf_size], *p = (void*)inbuf;
+    while (p < inend && o < outend) {
+        uint8_t c = *p++;
+        if (c > 31 && c < 127 && c != '\\') {
+            *o++ = c;
+            continue;
+        }
+        *o++ = '\\';
+        *o++ = 'x';
+        *o++ = GETHEX(c >> 4);
+        *o++ = GETHEX(c & 0x0f);
+    }
+    *o = '\0';
+    return outbuf;
+}

klippy/chelper/pyhelper.h

@@ -0,0 +1,11 @@
+#ifndef PYHELPER_H
+#define PYHELPER_H
+
+double get_monotonic(void);
+struct timespec fill_time(double time);
+void set_python_logging_callback(void (*func)(const char *));
+void errorf(const char *fmt, ...) __attribute__ ((format (printf, 1, 2)));
+void report_errno(char *where, int rc);
+char *dump_string(char *outbuf, int outbuf_size, char *inbuf, int inbuf_size);
+
+#endif // pyhelper.h

klippy/chelper/serialqueue.c

@@ -0,0 +1,947 @@
+// Serial port command queuing
+//
+// Copyright (C) 2016-2021  Kevin O'Connor <kevin@koconnor.net>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+// This goal of this code is to handle low-level serial port
+// communications with a microcontroller (mcu).  This code is written
+// in C (instead of python) to reduce communication latencies and to
+// reduce scheduling jitter.  The code queues messages to be
+// transmitted, schedules transmission of commands at specified mcu
+// clock times, prioritizes commands, and handles retransmissions.  A
+// background thread is launched to do this work and minimize latency.
+
+#include <linux/can.h> // // struct can_frame
+#include <math.h> // fabs
+#include <pthread.h> // pthread_mutex_lock
+#include <stddef.h> // offsetof
+#include <stdint.h> // uint64_t
+#include <stdio.h> // snprintf
+#include <stdlib.h> // malloc
+#include <string.h> // memset
+#include <termios.h> // tcflush
+#include <unistd.h> // pipe
+#include "compiler.h" // __visible
+#include "list.h" // list_add_tail
+#include "msgblock.h" // message_alloc
+#include "pollreactor.h" // pollreactor_alloc
+#include "pyhelper.h" // get_monotonic
+#include "serialqueue.h" // struct queue_message
+
+struct command_queue {
+    struct list_head stalled_queue, ready_queue;
+    struct list_node node;
+};
+
+struct serialqueue {
+    // Input reading
+    struct pollreactor *pr;
+    int serial_fd, serial_fd_type, client_id;
+    int pipe_fds[2];
+    uint8_t input_buf[4096];
+    uint8_t need_sync;
+    int input_pos;
+    // Threading
+    pthread_t tid;
+    pthread_mutex_t lock; // protects variables below
+    pthread_cond_t cond;
+    int receive_waiting;
+    // Baud / clock tracking
+    int receive_window;
+    double baud_adjust, idle_time;
+    struct clock_estimate ce;
+    double last_receive_sent_time;
+    // Retransmit support
+    uint64_t send_seq, receive_seq;
+    uint64_t ignore_nak_seq, last_ack_seq, retransmit_seq, rtt_sample_seq;
+    struct list_head sent_queue;
+    double srtt, rttvar, rto;
+    // Pending transmission message queues
+    struct list_head pending_queues;
+    int ready_bytes, stalled_bytes, need_ack_bytes, last_ack_bytes;
+    uint64_t need_kick_clock;
+    struct list_head notify_queue;
+    // Received messages
+    struct list_head receive_queue;
+    // Fastreader support
+    pthread_mutex_t fast_reader_dispatch_lock;
+    struct list_head fast_readers;
+    // Debugging
+    struct list_head old_sent, old_receive;
+    // Stats
+    uint32_t bytes_write, bytes_read, bytes_retransmit, bytes_invalid;
+};
+
+#define SQPF_SERIAL 0
+#define SQPF_PIPE   1
+#define SQPF_NUM    2
+
+#define SQPT_RETRANSMIT 0
+#define SQPT_COMMAND    1
+#define SQPT_NUM        2
+
+#define SQT_UART 'u'
+#define SQT_CAN 'c'
+#define SQT_DEBUGFILE 'f'
+
+#define MIN_RTO 0.025
+#define MAX_RTO 5.000
+#define MAX_PENDING_BLOCKS 12
+#define MIN_REQTIME_DELTA 0.250
+#define MIN_BACKGROUND_DELTA 0.005
+#define IDLE_QUERY_TIME 1.0
+
+#define DEBUG_QUEUE_SENT 100
+#define DEBUG_QUEUE_RECEIVE 100
+
+// Create a series of empty messages and add them to a list
+static void
+debug_queue_alloc(struct list_head *root, int count)
+{
+    int i;
+    for (i=0; i<count; i++) {
+        struct queue_message *qm = message_alloc();
+        list_add_head(&qm->node, root);
+    }
+}
+
+// Copy a message to a debug queue and free old debug messages
+static void
+debug_queue_add(struct list_head *root, struct queue_message *qm)
+{
+    list_add_tail(&qm->node, root);
+    struct queue_message *old = list_first_entry(
+        root, struct queue_message, node);
+    list_del(&old->node);
+    message_free(old);
+}
+
+// Wake up the receiver thread if it is waiting
+static void
+check_wake_receive(struct serialqueue *sq)
+{
+    if (sq->receive_waiting) {
+        sq->receive_waiting = 0;
+        pthread_cond_signal(&sq->cond);
+    }
+}
+
+// Write to the internal pipe to wake the background thread if in poll
+static void
+kick_bg_thread(struct serialqueue *sq)
+{
+    int ret = write(sq->pipe_fds[1], ".", 1);
+    if (ret < 0)
+        report_errno("pipe write", ret);
+}
+
+// Update internal state when the receive sequence increases
+static void
+update_receive_seq(struct serialqueue *sq, double eventtime, uint64_t rseq)
+{
+    // Remove from sent queue
+    uint64_t sent_seq = sq->receive_seq;
+    for (;;) {
+        struct queue_message *sent = list_first_entry(
+            &sq->sent_queue, struct queue_message, node);
+        if (list_empty(&sq->sent_queue)) {
+            // Got an ack for a message not sent; must be connection init
+            sq->send_seq = rseq;
+            sq->last_receive_sent_time = 0.;
+            break;
+        }
+        sq->need_ack_bytes -= sent->len;
+        list_del(&sent->node);
+        debug_queue_add(&sq->old_sent, sent);
+        sent_seq++;
+        if (rseq == sent_seq) {
+            // Found sent message corresponding with the received sequence
+            sq->last_receive_sent_time = sent->receive_time;
+            sq->last_ack_bytes = sent->len;
+            break;
+        }
+    }
+    sq->receive_seq = rseq;
+    pollreactor_update_timer(sq->pr, SQPT_COMMAND, PR_NOW);
+
+    // Update retransmit info
+    if (sq->rtt_sample_seq && rseq > sq->rtt_sample_seq
+        && sq->last_receive_sent_time) {
+        // RFC6298 rtt calculations
+        double delta = eventtime - sq->last_receive_sent_time;
+        if (!sq->srtt) {
+            sq->rttvar = delta / 2.0;
+            sq->srtt = delta * 10.0; // use a higher start default
+        } else {
+            sq->rttvar = (3.0 * sq->rttvar + fabs(sq->srtt - delta)) / 4.0;
+            sq->srtt = (7.0 * sq->srtt + delta) / 8.0;
+        }
+        double rttvar4 = sq->rttvar * 4.0;
+        if (rttvar4 < 0.001)
+            rttvar4 = 0.001;
+        sq->rto = sq->srtt + rttvar4;
+        if (sq->rto < MIN_RTO)
+            sq->rto = MIN_RTO;
+        else if (sq->rto > MAX_RTO)
+            sq->rto = MAX_RTO;
+        sq->rtt_sample_seq = 0;
+    }
+    if (list_empty(&sq->sent_queue)) {
+        pollreactor_update_timer(sq->pr, SQPT_RETRANSMIT, PR_NEVER);
+    } else {
+        struct queue_message *sent = list_first_entry(
+            &sq->sent_queue, struct queue_message, node);
+        double nr = eventtime + sq->rto + sent->len * sq->baud_adjust;
+        pollreactor_update_timer(sq->pr, SQPT_RETRANSMIT, nr);
+    }
+}
+
+// Process a well formed input message
+static void
+handle_message(struct serialqueue *sq, double eventtime, int len)
+{
+    pthread_mutex_lock(&sq->lock);
+
+    // Calculate receive sequence number
+    uint64_t rseq = ((sq->receive_seq & ~MESSAGE_SEQ_MASK)
+                     | (sq->input_buf[MESSAGE_POS_SEQ] & MESSAGE_SEQ_MASK));
+    if (rseq != sq->receive_seq) {
+        // New sequence number
+        if (rseq < sq->receive_seq)
+            rseq += MESSAGE_SEQ_MASK+1;
+        if (rseq > sq->send_seq && sq->receive_seq != 1) {
+            // An ack for a message not sent?  Out of order message?
+            sq->bytes_invalid += len;
+            pthread_mutex_unlock(&sq->lock);
+            return;
+        }
+        update_receive_seq(sq, eventtime, rseq);
+    }
+    sq->bytes_read += len;
+
+    // Check for pending messages on notify_queue
+    int must_wake = 0;
+    while (!list_empty(&sq->notify_queue)) {
+        struct queue_message *qm = list_first_entry(
+            &sq->notify_queue, struct queue_message, node);
+        uint64_t wake_seq = rseq - 1 - (len > MESSAGE_MIN ? 1 : 0);
+        uint64_t notify_msg_sent_seq = qm->req_clock;
+        if (notify_msg_sent_seq > wake_seq)
+            break;
+        list_del(&qm->node);
+        qm->len = 0;
+        qm->sent_time = sq->last_receive_sent_time;
+        qm->receive_time = eventtime;
+        list_add_tail(&qm->node, &sq->receive_queue);
+        must_wake = 1;
+    }
+
+    // Process message
+    if (len == MESSAGE_MIN) {
+        // Ack/nak message
+        if (sq->last_ack_seq < rseq)
+            sq->last_ack_seq = rseq;
+        else if (rseq > sq->ignore_nak_seq && !list_empty(&sq->sent_queue))
+            // Duplicate Ack is a Nak - do fast retransmit
+            pollreactor_update_timer(sq->pr, SQPT_RETRANSMIT, PR_NOW);
+    } else {
+        // Data message - add to receive queue
+        struct queue_message *qm = message_fill(sq->input_buf, len);
+        qm->sent_time = (rseq > sq->retransmit_seq
+                         ? sq->last_receive_sent_time : 0.);
+        qm->receive_time = get_monotonic(); // must be time post read()
+        qm->receive_time -= sq->baud_adjust * len;
+        list_add_tail(&qm->node, &sq->receive_queue);
+        must_wake = 1;
+    }
+
+    // Check fast readers
+    struct fastreader *fr;
+    list_for_each_entry(fr, &sq->fast_readers, node) {
+        if (len < fr->prefix_len + MESSAGE_MIN
+            || memcmp(&sq->input_buf[MESSAGE_HEADER_SIZE]
+                      , fr->prefix, fr->prefix_len) != 0)
+            continue;
+        // Release main lock and invoke callback
+        pthread_mutex_lock(&sq->fast_reader_dispatch_lock);
+        if (must_wake)
+            check_wake_receive(sq);
+        pthread_mutex_unlock(&sq->lock);
+        fr->func(fr, sq->input_buf, len);
+        pthread_mutex_unlock(&sq->fast_reader_dispatch_lock);
+        return;
+    }
+
+    if (must_wake)
+        check_wake_receive(sq);
+    pthread_mutex_unlock(&sq->lock);
+}
+
+// Callback for input activity on the serial fd
+static void
+input_event(struct serialqueue *sq, double eventtime)
+{
+    if (sq->serial_fd_type == SQT_CAN) {
+        struct can_frame cf;
+        int ret = read(sq->serial_fd, &cf, sizeof(cf));
+        if (ret <= 0) {
+            report_errno("can read", ret);
+            pollreactor_do_exit(sq->pr);
+            return;
+        }
+        if (cf.can_id != sq->client_id + 1)
+            return;
+        memcpy(&sq->input_buf[sq->input_pos], cf.data, cf.can_dlc);
+        sq->input_pos += cf.can_dlc;
+    } else {
+        int ret = read(sq->serial_fd, &sq->input_buf[sq->input_pos]
+                       , sizeof(sq->input_buf) - sq->input_pos);
+        if (ret <= 0) {
+            if(ret < 0)
+                report_errno("read", ret);
+            else
+                errorf("Got EOF when reading from device");
+            pollreactor_do_exit(sq->pr);
+            return;
+        }
+        sq->input_pos += ret;
+    }
+    for (;;) {
+        int len = msgblock_check(&sq->need_sync, sq->input_buf, sq->input_pos);
+        if (!len)
+            // Need more data
+            return;
+        if (len > 0) {
+            // Received a valid message
+            handle_message(sq, eventtime, len);
+        } else {
+            // Skip bad data at beginning of input
+            len = -len;
+            pthread_mutex_lock(&sq->lock);
+            sq->bytes_invalid += len;
+            pthread_mutex_unlock(&sq->lock);
+        }
+        sq->input_pos -= len;
+        if (sq->input_pos)
+            memmove(sq->input_buf, &sq->input_buf[len], sq->input_pos);
+    }
+}
+
+// Callback for input activity on the pipe fd (wakes command_event)
+static void
+kick_event(struct serialqueue *sq, double eventtime)
+{
+    char dummy[4096];
+    int ret = read(sq->pipe_fds[0], dummy, sizeof(dummy));
+    if (ret < 0)
+        report_errno("pipe read", ret);
+    pollreactor_update_timer(sq->pr, SQPT_COMMAND, PR_NOW);
+}
+
+static void
+do_write(struct serialqueue *sq, void *buf, int buflen)
+{
+    if (sq->serial_fd_type != SQT_CAN) {
+        int ret = write(sq->serial_fd, buf, buflen);
+        if (ret < 0)
+            report_errno("write", ret);
+        return;
+    }
+    // Write to CAN fd
+    struct can_frame cf;
+    while (buflen) {
+        int size = buflen > 8 ? 8 : buflen;
+        cf.can_id = sq->client_id;
+        cf.can_dlc = size;
+        memcpy(cf.data, buf, size);
+        int ret = write(sq->serial_fd, &cf, sizeof(cf));
+        if (ret < 0) {
+            report_errno("can write", ret);
+            return;
+        }
+        buf += size;
+        buflen -= size;
+    }
+}
+
+// Callback timer for when a retransmit should be done
+static double
+retransmit_event(struct serialqueue *sq, double eventtime)
+{
+    if (sq->serial_fd_type == SQT_UART) {
+        int ret = tcflush(sq->serial_fd, TCOFLUSH);
+        if (ret < 0)
+            report_errno("tcflush", ret);
+    }
+
+    pthread_mutex_lock(&sq->lock);
+
+    // Retransmit all pending messages
+    uint8_t buf[MESSAGE_MAX * MAX_PENDING_BLOCKS + 1];
+    int buflen = 0, first_buflen = 0;
+    buf[buflen++] = MESSAGE_SYNC;
+    struct queue_message *qm;
+    list_for_each_entry(qm, &sq->sent_queue, node) {
+        memcpy(&buf[buflen], qm->msg, qm->len);
+        buflen += qm->len;
+        if (!first_buflen)
+            first_buflen = qm->len + 1;
+    }
+    do_write(sq, buf, buflen);
+    sq->bytes_retransmit += buflen;
+
+    // Update rto
+    if (pollreactor_get_timer(sq->pr, SQPT_RETRANSMIT) == PR_NOW) {
+        // Retransmit due to nak
+        sq->ignore_nak_seq = sq->receive_seq;
+        if (sq->receive_seq < sq->retransmit_seq)
+            // Second nak for this retransmit - don't allow third
+            sq->ignore_nak_seq = sq->retransmit_seq;
+    } else {
+        // Retransmit due to timeout
+        sq->rto *= 2.0;
+        if (sq->rto > MAX_RTO)
+            sq->rto = MAX_RTO;
+        sq->ignore_nak_seq = sq->send_seq;
+    }
+    sq->retransmit_seq = sq->send_seq;
+    sq->rtt_sample_seq = 0;
+    sq->idle_time = eventtime + buflen * sq->baud_adjust;
+    double waketime = eventtime + first_buflen * sq->baud_adjust + sq->rto;
+
+    pthread_mutex_unlock(&sq->lock);
+    return waketime;
+}
+
+// Construct a block of data to be sent to the serial port
+static int
+build_and_send_command(struct serialqueue *sq, uint8_t *buf, double eventtime)
+{
+    int len = MESSAGE_HEADER_SIZE;
+    while (sq->ready_bytes) {
+        // Find highest priority message (message with lowest req_clock)
+        uint64_t min_clock = MAX_CLOCK;
+        struct command_queue *q, *cq = NULL;
+        struct queue_message *qm = NULL;
+        list_for_each_entry(q, &sq->pending_queues, node) {
+            if (!list_empty(&q->ready_queue)) {
+                struct queue_message *m = list_first_entry(
+                    &q->ready_queue, struct queue_message, node);
+                if (m->req_clock < min_clock) {
+                    min_clock = m->req_clock;
+                    cq = q;
+                    qm = m;
+                }
+            }
+        }
+        // Append message to outgoing command
+        if (len + qm->len > MESSAGE_MAX - MESSAGE_TRAILER_SIZE)
+            break;
+        list_del(&qm->node);
+        if (list_empty(&cq->ready_queue) && list_empty(&cq->stalled_queue))
+            list_del(&cq->node);
+        memcpy(&buf[len], qm->msg, qm->len);
+        len += qm->len;
+        sq->ready_bytes -= qm->len;
+        if (qm->notify_id) {
+            // Message requires notification - add to notify list
+            qm->req_clock = sq->send_seq;
+            list_add_tail(&qm->node, &sq->notify_queue);
+        } else {
+            message_free(qm);
+        }
+    }
+
+    // Fill header / trailer
+    len += MESSAGE_TRAILER_SIZE;
+    buf[MESSAGE_POS_LEN] = len;
+    buf[MESSAGE_POS_SEQ] = MESSAGE_DEST | (sq->send_seq & MESSAGE_SEQ_MASK);
+    uint16_t crc = msgblock_crc16_ccitt(buf, len - MESSAGE_TRAILER_SIZE);
+    buf[len - MESSAGE_TRAILER_CRC] = crc >> 8;
+    buf[len - MESSAGE_TRAILER_CRC+1] = crc & 0xff;
+    buf[len - MESSAGE_TRAILER_SYNC] = MESSAGE_SYNC;
+
+    // Store message block
+    if (eventtime > sq->idle_time)
+        sq->idle_time = eventtime;
+    sq->idle_time += len * sq->baud_adjust;
+    struct queue_message *out = message_alloc();
+    memcpy(out->msg, buf, len);
+    out->len = len;
+    out->sent_time = eventtime;
+    out->receive_time = sq->idle_time;
+    if (list_empty(&sq->sent_queue))
+        pollreactor_update_timer(sq->pr, SQPT_RETRANSMIT
+                                 , sq->idle_time + sq->rto);
+    if (!sq->rtt_sample_seq)
+        sq->rtt_sample_seq = sq->send_seq;
+    sq->send_seq++;
+    sq->need_ack_bytes += len;
+    list_add_tail(&out->node, &sq->sent_queue);
+    return len;
+}
+
+// Determine the time the next serial data should be sent
+static double
+check_send_command(struct serialqueue *sq, double eventtime)
+{
+    if (sq->send_seq - sq->receive_seq >= MAX_PENDING_BLOCKS
+        && sq->receive_seq != (uint64_t)-1)
+        // Need an ack before more messages can be sent
+        return PR_NEVER;
+    if (sq->send_seq > sq->receive_seq && sq->receive_window) {
+        int need_ack_bytes = sq->need_ack_bytes + MESSAGE_MAX;
+        if (sq->last_ack_seq < sq->receive_seq)
+            need_ack_bytes += sq->last_ack_bytes;
+        if (need_ack_bytes > sq->receive_window)
+            // Wait for ack from past messages before sending next message
+            return PR_NEVER;
+    }
+
+    // Check for stalled messages now ready
+    double idletime = eventtime > sq->idle_time ? eventtime : sq->idle_time;
+    idletime += MESSAGE_MIN * sq->baud_adjust;
+    uint64_t ack_clock = clock_from_time(&sq->ce, idletime);
+    uint64_t min_stalled_clock = MAX_CLOCK, min_ready_clock = MAX_CLOCK;
+    struct command_queue *cq;
+    list_for_each_entry(cq, &sq->pending_queues, node) {
+        // Move messages from the stalled_queue to the ready_queue
+        while (!list_empty(&cq->stalled_queue)) {
+            struct queue_message *qm = list_first_entry(
+                &cq->stalled_queue, struct queue_message, node);
+            if (ack_clock < qm->min_clock) {
+                if (qm->min_clock < min_stalled_clock)
+                    min_stalled_clock = qm->min_clock;
+                break;
+            }
+            list_del(&qm->node);
+            list_add_tail(&qm->node, &cq->ready_queue);
+            sq->stalled_bytes -= qm->len;
+            sq->ready_bytes += qm->len;
+        }
+        // Update min_ready_clock
+        if (!list_empty(&cq->ready_queue)) {
+            struct queue_message *qm = list_first_entry(
+                &cq->ready_queue, struct queue_message, node);
+            uint64_t req_clock = qm->req_clock;
+            double bgoffset = MIN_REQTIME_DELTA + MIN_BACKGROUND_DELTA;
+            if (req_clock == BACKGROUND_PRIORITY_CLOCK)
+                req_clock = clock_from_time(&sq->ce, sq->idle_time + bgoffset);
+            if (req_clock < min_ready_clock)
+                min_ready_clock = req_clock;
+        }
+    }
+
+    // Check for messages to send
+    if (sq->ready_bytes >= MESSAGE_PAYLOAD_MAX)
+        return PR_NOW;
+    if (! sq->ce.est_freq) {
+        if (sq->ready_bytes)
+            return PR_NOW;
+        sq->need_kick_clock = MAX_CLOCK;
+        return PR_NEVER;
+    }
+    uint64_t reqclock_delta = MIN_REQTIME_DELTA * sq->ce.est_freq;
+    if (min_ready_clock <= ack_clock + reqclock_delta)
+        return PR_NOW;
+    uint64_t wantclock = min_ready_clock - reqclock_delta;
+    if (min_stalled_clock < wantclock)
+        wantclock = min_stalled_clock;
+    sq->need_kick_clock = wantclock;
+    return idletime + (wantclock - ack_clock) / sq->ce.est_freq;
+}
+
+// Callback timer to send data to the serial port
+static double
+command_event(struct serialqueue *sq, double eventtime)
+{
+    pthread_mutex_lock(&sq->lock);
+    uint8_t buf[MESSAGE_MAX * MAX_PENDING_BLOCKS];
+    int buflen = 0;
+    double waketime;
+    for (;;) {
+        waketime = check_send_command(sq, eventtime);
+        if (waketime != PR_NOW || buflen + MESSAGE_MAX > sizeof(buf)) {
+            if (buflen) {
+                // Write message blocks
+                do_write(sq, buf, buflen);
+                sq->bytes_write += buflen;
+                buflen = 0;
+            }
+            if (waketime != PR_NOW)
+                break;
+        }
+        buflen += build_and_send_command(sq, &buf[buflen], eventtime);
+    }
+    pthread_mutex_unlock(&sq->lock);
+    return waketime;
+}
+
+// Main background thread for reading/writing to serial port
+static void *
+background_thread(void *data)
+{
+    struct serialqueue *sq = data;
+    pollreactor_run(sq->pr);
+
+    pthread_mutex_lock(&sq->lock);
+    check_wake_receive(sq);
+    pthread_mutex_unlock(&sq->lock);
+
+    return NULL;
+}
+
+// Create a new 'struct serialqueue' object
+struct serialqueue * __visible
+serialqueue_alloc(int serial_fd, char serial_fd_type, int client_id)
+{
+    struct serialqueue *sq = malloc(sizeof(*sq));
+    memset(sq, 0, sizeof(*sq));
+    sq->serial_fd = serial_fd;
+    sq->serial_fd_type = serial_fd_type;
+    sq->client_id = client_id;
+
+    int ret = pipe(sq->pipe_fds);
+    if (ret)
+        goto fail;
+
+    // Reactor setup
+    sq->pr = pollreactor_alloc(SQPF_NUM, SQPT_NUM, sq);
+    pollreactor_add_fd(sq->pr, SQPF_SERIAL, serial_fd, input_event
+                       , serial_fd_type==SQT_DEBUGFILE);
+    pollreactor_add_fd(sq->pr, SQPF_PIPE, sq->pipe_fds[0], kick_event, 0);
+    pollreactor_add_timer(sq->pr, SQPT_RETRANSMIT, retransmit_event);
+    pollreactor_add_timer(sq->pr, SQPT_COMMAND, command_event);
+    fd_set_non_blocking(serial_fd);
+    fd_set_non_blocking(sq->pipe_fds[0]);
+    fd_set_non_blocking(sq->pipe_fds[1]);
+
+    // Retransmit setup
+    sq->send_seq = 1;
+    if (serial_fd_type == SQT_DEBUGFILE) {
+        // Debug file output
+        sq->receive_seq = -1;
+        sq->rto = PR_NEVER;
+    } else {
+        sq->receive_seq = 1;
+        sq->rto = MIN_RTO;
+    }
+
+    // Queues
+    sq->need_kick_clock = MAX_CLOCK;
+    list_init(&sq->pending_queues);
+    list_init(&sq->sent_queue);
+    list_init(&sq->receive_queue);
+    list_init(&sq->notify_queue);
+    list_init(&sq->fast_readers);
+
+    // Debugging
+    list_init(&sq->old_sent);
+    list_init(&sq->old_receive);
+    debug_queue_alloc(&sq->old_sent, DEBUG_QUEUE_SENT);
+    debug_queue_alloc(&sq->old_receive, DEBUG_QUEUE_RECEIVE);
+
+    // Thread setup
+    ret = pthread_mutex_init(&sq->lock, NULL);
+    if (ret)
+        goto fail;
+    ret = pthread_cond_init(&sq->cond, NULL);
+    if (ret)
+        goto fail;
+    ret = pthread_mutex_init(&sq->fast_reader_dispatch_lock, NULL);
+    if (ret)
+        goto fail;
+    ret = pthread_create(&sq->tid, NULL, background_thread, sq);
+    if (ret)
+        goto fail;
+
+    return sq;
+
+fail:
+    report_errno("init", ret);
+    return NULL;
+}
+
+// Request that the background thread exit
+void __visible
+serialqueue_exit(struct serialqueue *sq)
+{
+    pollreactor_do_exit(sq->pr);
+    kick_bg_thread(sq);
+    int ret = pthread_join(sq->tid, NULL);
+    if (ret)
+        report_errno("pthread_join", ret);
+}
+
+// Free all resources associated with a serialqueue
+void __visible
+serialqueue_free(struct serialqueue *sq)
+{
+    if (!sq)
+        return;
+    if (!pollreactor_is_exit(sq->pr))
+        serialqueue_exit(sq);
+    pthread_mutex_lock(&sq->lock);
+    message_queue_free(&sq->sent_queue);
+    message_queue_free(&sq->receive_queue);
+    message_queue_free(&sq->notify_queue);
+    message_queue_free(&sq->old_sent);
+    message_queue_free(&sq->old_receive);
+    while (!list_empty(&sq->pending_queues)) {
+        struct command_queue *cq = list_first_entry(
+            &sq->pending_queues, struct command_queue, node);
+        list_del(&cq->node);
+        message_queue_free(&cq->ready_queue);
+        message_queue_free(&cq->stalled_queue);
+    }
+    pthread_mutex_unlock(&sq->lock);
+    pollreactor_free(sq->pr);
+    free(sq);
+}
+
+// Allocate a 'struct command_queue'
+struct command_queue * __visible
+serialqueue_alloc_commandqueue(void)
+{
+    struct command_queue *cq = malloc(sizeof(*cq));
+    memset(cq, 0, sizeof(*cq));
+    list_init(&cq->ready_queue);
+    list_init(&cq->stalled_queue);
+    return cq;
+}
+
+// Free a 'struct command_queue'
+void __visible
+serialqueue_free_commandqueue(struct command_queue *cq)
+{
+    if (!cq)
+        return;
+    if (!list_empty(&cq->ready_queue) || !list_empty(&cq->stalled_queue)) {
+        errorf("Memory leak! Can't free non-empty commandqueue");
+        return;
+    }
+    free(cq);
+}
+
+// Add a low-latency message handler
+void
+serialqueue_add_fastreader(struct serialqueue *sq, struct fastreader *fr)
+{
+    pthread_mutex_lock(&sq->lock);
+    list_add_tail(&fr->node, &sq->fast_readers);
+    pthread_mutex_unlock(&sq->lock);
+}
+
+// Remove a previously registered low-latency message handler
+void
+serialqueue_rm_fastreader(struct serialqueue *sq, struct fastreader *fr)
+{
+    pthread_mutex_lock(&sq->lock);
+    list_del(&fr->node);
+    pthread_mutex_unlock(&sq->lock);
+
+    pthread_mutex_lock(&sq->fast_reader_dispatch_lock); // XXX - goofy locking
+    pthread_mutex_unlock(&sq->fast_reader_dispatch_lock);
+}
+
+// Add a batch of messages to the given command_queue
+void
+serialqueue_send_batch(struct serialqueue *sq, struct command_queue *cq
+                       , struct list_head *msgs)
+{
+    // Make sure min_clock is set in list and calculate total bytes
+    int len = 0;
+    struct queue_message *qm;
+    list_for_each_entry(qm, msgs, node) {
+        if (qm->min_clock + (1LL<<31) < qm->req_clock
+            && qm->req_clock != BACKGROUND_PRIORITY_CLOCK)
+            qm->min_clock = qm->req_clock - (1LL<<31);
+        len += qm->len;
+    }
+    if (! len)
+        return;
+    qm = list_first_entry(msgs, struct queue_message, node);
+
+    // Add list to cq->stalled_queue
+    pthread_mutex_lock(&sq->lock);
+    if (list_empty(&cq->ready_queue) && list_empty(&cq->stalled_queue))
+        list_add_tail(&cq->node, &sq->pending_queues);
+    list_join_tail(msgs, &cq->stalled_queue);
+    sq->stalled_bytes += len;
+    int mustwake = 0;
+    if (qm->min_clock < sq->need_kick_clock) {
+        sq->need_kick_clock = 0;
+        mustwake = 1;
+    }
+    pthread_mutex_unlock(&sq->lock);
+
+    // Wake the background thread if necessary
+    if (mustwake)
+        kick_bg_thread(sq);
+}
+
+// Helper to send a single message
+void
+serialqueue_send_one(struct serialqueue *sq, struct command_queue *cq
+                     , struct queue_message *qm)
+{
+    struct list_head msgs;
+    list_init(&msgs);
+    list_add_tail(&qm->node, &msgs);
+    serialqueue_send_batch(sq, cq, &msgs);
+}
+
+// Schedule the transmission of a message on the serial port at a
+// given time and priority.
+void __visible
+serialqueue_send(struct serialqueue *sq, struct command_queue *cq, uint8_t *msg
+                 , int len, uint64_t min_clock, uint64_t req_clock
+                 , uint64_t notify_id)
+{
+    struct queue_message *qm = message_fill(msg, len);
+    qm->min_clock = min_clock;
+    qm->req_clock = req_clock;
+    qm->notify_id = notify_id;
+    serialqueue_send_one(sq, cq, qm);
+}
+
+// Return a message read from the serial port (or wait for one if none
+// available)
+void __visible
+serialqueue_pull(struct serialqueue *sq, struct pull_queue_message *pqm)
+{
+    pthread_mutex_lock(&sq->lock);
+    // Wait for message to be available
+    while (list_empty(&sq->receive_queue)) {
+        if (pollreactor_is_exit(sq->pr))
+            goto exit;
+        sq->receive_waiting = 1;
+        int ret = pthread_cond_wait(&sq->cond, &sq->lock);
+        if (ret)
+            report_errno("pthread_cond_wait", ret);
+    }
+
+    // Remove message from queue
+    struct queue_message *qm = list_first_entry(
+        &sq->receive_queue, struct queue_message, node);
+    list_del(&qm->node);
+
+    // Copy message
+    memcpy(pqm->msg, qm->msg, qm->len);
+    pqm->len = qm->len;
+    pqm->sent_time = qm->sent_time;
+    pqm->receive_time = qm->receive_time;
+    pqm->notify_id = qm->notify_id;
+    if (qm->len)
+        debug_queue_add(&sq->old_receive, qm);
+    else
+        message_free(qm);
+
+    pthread_mutex_unlock(&sq->lock);
+    return;
+
+exit:
+    pqm->len = -1;
+    pthread_mutex_unlock(&sq->lock);
+}
+
+void __visible
+serialqueue_set_baud_adjust(struct serialqueue *sq, double baud_adjust)
+{
+    pthread_mutex_lock(&sq->lock);
+    sq->baud_adjust = baud_adjust;
+    pthread_mutex_unlock(&sq->lock);
+}
+
+void __visible
+serialqueue_set_receive_window(struct serialqueue *sq, int receive_window)
+{
+    pthread_mutex_lock(&sq->lock);
+    sq->receive_window = receive_window;
+    pthread_mutex_unlock(&sq->lock);
+}
+
+// Set the estimated clock rate of the mcu on the other end of the
+// serial port
+void __visible
+serialqueue_set_clock_est(struct serialqueue *sq, double est_freq
+                          , double conv_time, uint64_t conv_clock
+                          , uint64_t last_clock)
+{
+    pthread_mutex_lock(&sq->lock);
+    sq->ce.est_freq = est_freq;
+    sq->ce.conv_time = conv_time;
+    sq->ce.conv_clock = conv_clock;
+    sq->ce.last_clock = last_clock;
+    pthread_mutex_unlock(&sq->lock);
+}
+
+// Return the latest clock estimate
+void
+serialqueue_get_clock_est(struct serialqueue *sq, struct clock_estimate *ce)
+{
+    pthread_mutex_lock(&sq->lock);
+    memcpy(ce, &sq->ce, sizeof(sq->ce));
+    pthread_mutex_unlock(&sq->lock);
+}
+
+// Return a string buffer containing statistics for the serial port
+void __visible
+serialqueue_get_stats(struct serialqueue *sq, char *buf, int len)
+{
+    struct serialqueue stats;
+    pthread_mutex_lock(&sq->lock);
+    memcpy(&stats, sq, sizeof(stats));
+    pthread_mutex_unlock(&sq->lock);
+
+    snprintf(buf, len, "bytes_write=%u bytes_read=%u"
+             " bytes_retransmit=%u bytes_invalid=%u"
+             " send_seq=%u receive_seq=%u retransmit_seq=%u"
+             " srtt=%.3f rttvar=%.3f rto=%.3f"
+             " ready_bytes=%u stalled_bytes=%u"
+             , stats.bytes_write, stats.bytes_read
+             , stats.bytes_retransmit, stats.bytes_invalid
+             , (int)stats.send_seq, (int)stats.receive_seq
+             , (int)stats.retransmit_seq
+             , stats.srtt, stats.rttvar, stats.rto
+             , stats.ready_bytes, stats.stalled_bytes);
+}
+
+// Extract old messages stored in the debug queues
+int __visible
+serialqueue_extract_old(struct serialqueue *sq, int sentq
+                        , struct pull_queue_message *q, int max)
+{
+    int count = sentq ? DEBUG_QUEUE_SENT : DEBUG_QUEUE_RECEIVE;
+    struct list_head *rootp = sentq ? &sq->old_sent : &sq->old_receive;
+    struct list_head replacement, current;
+    list_init(&replacement);
+    debug_queue_alloc(&replacement, count);
+    list_init(&current);
+
+    // Atomically replace existing debug list with new zero'd list
+    pthread_mutex_lock(&sq->lock);
+    list_join_tail(rootp, &current);
+    list_init(rootp);
+    list_join_tail(&replacement, rootp);
+    pthread_mutex_unlock(&sq->lock);
+
+    // Walk the debug list
+    int pos = 0;
+    while (!list_empty(&current)) {
+        struct queue_message *qm = list_first_entry(
+            &current, struct queue_message, node);
+        if (qm->len && pos < max) {
+            struct pull_queue_message *pqm = q++;
+            pos++;
+            memcpy(pqm->msg, qm->msg, qm->len);
+            pqm->len = qm->len;
+            pqm->sent_time = qm->sent_time;
+            pqm->receive_time = qm->receive_time;
+        }
+        list_del(&qm->node);
+        message_free(qm);
+    }
+    return pos;
+}

klippy/chelper/serialqueue.h

@@ -0,0 +1,56 @@
+#ifndef SERIALQUEUE_H
+#define SERIALQUEUE_H
+
+#include <stdint.h> // uint8_t
+#include "list.h" // struct list_head
+#include "msgblock.h" // MESSAGE_MAX
+
+#define MAX_CLOCK 0x7fffffffffffffffLL
+#define BACKGROUND_PRIORITY_CLOCK 0x7fffffff00000000LL
+
+struct fastreader;
+typedef void (*fastreader_cb)(struct fastreader *fr, uint8_t *data, int len);
+
+struct fastreader {
+    struct list_node node;
+    fastreader_cb func;
+    int prefix_len;
+    uint8_t prefix[MESSAGE_MAX];
+};
+
+struct pull_queue_message {
+    uint8_t msg[MESSAGE_MAX];
+    int len;
+    double sent_time, receive_time;
+    uint64_t notify_id;
+};
+
+struct serialqueue;
+struct serialqueue *serialqueue_alloc(int serial_fd, char serial_fd_type
+                                      , int client_id);
+void serialqueue_exit(struct serialqueue *sq);
+void serialqueue_free(struct serialqueue *sq);
+struct command_queue *serialqueue_alloc_commandqueue(void);
+void serialqueue_free_commandqueue(struct command_queue *cq);
+void serialqueue_add_fastreader(struct serialqueue *sq, struct fastreader *fr);
+void serialqueue_rm_fastreader(struct serialqueue *sq, struct fastreader *fr);
+void serialqueue_send_batch(struct serialqueue *sq, struct command_queue *cq
+                            , struct list_head *msgs);
+void serialqueue_send_one(struct serialqueue *sq, struct command_queue *cq
+                          , struct queue_message *qm);
+void serialqueue_send(struct serialqueue *sq, struct command_queue *cq
+                      , uint8_t *msg, int len, uint64_t min_clock
+                      , uint64_t req_clock, uint64_t notify_id);
+void serialqueue_pull(struct serialqueue *sq, struct pull_queue_message *pqm);
+void serialqueue_set_baud_adjust(struct serialqueue *sq, double baud_adjust);
+void serialqueue_set_receive_window(struct serialqueue *sq, int receive_window);
+void serialqueue_set_clock_est(struct serialqueue *sq, double est_freq
+                               , double conv_time, uint64_t conv_clock
+                               , uint64_t last_clock);
+void serialqueue_get_clock_est(struct serialqueue *sq
+                               , struct clock_estimate *ce);
+void serialqueue_get_stats(struct serialqueue *sq, char *buf, int len);
+int serialqueue_extract_old(struct serialqueue *sq, int sentq
+                            , struct pull_queue_message *q, int max);
+
+#endif // serialqueue.h

klippy/chelper/stepcompress.c

@@ -0,0 +1,795 @@
+// Stepper pulse schedule compression
+//
+// Copyright (C) 2016-2021  Kevin O'Connor <kevin@koconnor.net>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+// The goal of this code is to take a series of scheduled stepper
+// pulse times and compress them into a handful of commands that can
+// be efficiently transmitted and executed on a microcontroller (mcu).
+// The mcu accepts step pulse commands that take interval, count, and
+// add parameters such that 'count' pulses occur, with each step event
+// calculating the next step event time using:
+//  next_wake_time = last_wake_time + interval; interval += add
+// 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
+#include <stdlib.h> // malloc
+#include <string.h> // memset
+#include "compiler.h" // DIV_ROUND_UP
+#include "pyhelper.h" // errorf
+#include "serialqueue.h" // struct queue_message
+#include "stepcompress.h" // stepcompress_alloc
+
+#define CHECK_LINES 1
+#define QUEUE_START_SIZE 1024
+
+struct stepcompress {
+    // Buffer management
+    uint32_t *queue, *queue_end, *queue_pos, *queue_next;
+    // Internal tracking
+    uint32_t max_error;
+    double mcu_time_offset, mcu_freq, last_step_print_time;
+    // Message generation
+    uint64_t last_step_clock;
+    struct list_head msg_queue;
+    uint32_t oid;
+    int32_t queue_step_msgtag, set_next_step_dir_msgtag;
+    int sdir, invert_sdir;
+    // 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_steps {
+    struct list_node node;
+    uint64_t first_clock, last_clock;
+    int64_t start_position;
+    int step_count, interval, add;
+};
+
+
+/****************************************************************
+ * Step compression
+ ****************************************************************/
+
+static inline int32_t
+idiv_up(int32_t n, int32_t d)
+{
+    return (n>=0) ? DIV_ROUND_UP(n,d) : (n/d);
+}
+
+static inline int32_t
+idiv_down(int32_t n, int32_t d)
+{
+    return (n>=0) ? (n/d) : (n - d + 1) / d;
+}
+
+struct points {
+    int32_t minp, maxp;
+};
+
+// Given a requested step time, return the minimum and maximum
+// acceptable times
+static inline struct points
+minmax_point(struct stepcompress *sc, uint32_t *pos)
+{
+    uint32_t lsc = sc->last_step_clock, point = *pos - lsc;
+    uint32_t prevpoint = pos > sc->queue_pos ? *(pos-1) - lsc : 0;
+    uint32_t max_error = (point - prevpoint) / 2;
+    if (max_error > sc->max_error)
+        max_error = sc->max_error;
+    return (struct points){ point - max_error, point };
+}
+
+// The maximum add delta between two valid quadratic sequences of the
+// form "add*count*(count-1)/2 + interval*count" is "(6 + 4*sqrt(2)) *
+// maxerror / (count*count)".  The "6 + 4*sqrt(2)" is 11.65685, but
+// using 11 works well in practice.
+#define QUADRATIC_DEV 11
+
+// Find a 'step_move' that covers a series of step times
+static struct step_move
+compress_bisect_add(struct stepcompress *sc)
+{
+    uint32_t *qlast = sc->queue_next;
+    if (qlast > sc->queue_pos + 65535)
+        qlast = sc->queue_pos + 65535;
+    struct points point = minmax_point(sc, sc->queue_pos);
+    int32_t outer_mininterval = point.minp, outer_maxinterval = point.maxp;
+    int32_t add = 0, minadd = -0x8000, maxadd = 0x7fff;
+    int32_t bestinterval = 0, bestcount = 1, bestadd = 1, bestreach = INT32_MIN;
+    int32_t zerointerval = 0, zerocount = 0;
+
+    for (;;) {
+        // Find longest valid sequence with the given 'add'
+        struct points nextpoint;
+        int32_t nextmininterval = outer_mininterval;
+        int32_t nextmaxinterval = outer_maxinterval, interval = nextmaxinterval;
+        int32_t nextcount = 1;
+        for (;;) {
+            nextcount++;
+            if (&sc->queue_pos[nextcount-1] >= qlast) {
+                int32_t count = nextcount - 1;
+                return (struct step_move){ interval, count, add };
+            }
+            nextpoint = minmax_point(sc, sc->queue_pos + nextcount - 1);
+            int32_t nextaddfactor = nextcount*(nextcount-1)/2;
+            int32_t c = add*nextaddfactor;
+            if (nextmininterval*nextcount < nextpoint.minp - c)
+                nextmininterval = idiv_up(nextpoint.minp - c, nextcount);
+            if (nextmaxinterval*nextcount > nextpoint.maxp - c)
+                nextmaxinterval = idiv_down(nextpoint.maxp - c, nextcount);
+            if (nextmininterval > nextmaxinterval)
+                break;
+            interval = nextmaxinterval;
+        }
+
+        // Check if this is the best sequence found so far
+        int32_t count = nextcount - 1, addfactor = count*(count-1)/2;
+        int32_t reach = add*addfactor + interval*count;
+        if (reach > bestreach
+            || (reach == bestreach && interval > bestinterval)) {
+            bestinterval = interval;
+            bestcount = count;
+            bestadd = add;
+            bestreach = reach;
+            if (!add) {
+                zerointerval = interval;
+                zerocount = count;
+            }
+            if (count > 0x200)
+                // No 'add' will improve sequence; avoid integer overflow
+                break;
+        }
+
+        // Check if a greater or lesser add could extend the sequence
+        int32_t nextaddfactor = nextcount*(nextcount-1)/2;
+        int32_t nextreach = add*nextaddfactor + interval*nextcount;
+        if (nextreach < nextpoint.minp) {
+            minadd = add + 1;
+            outer_maxinterval = nextmaxinterval;
+        } else {
+            maxadd = add - 1;
+            outer_mininterval = nextmininterval;
+        }
+
+        // The maximum valid deviation between two quadratic sequences
+        // can be calculated and used to further limit the add range.
+        if (count > 1) {
+            int32_t errdelta = sc->max_error*QUADRATIC_DEV / (count*count);
+            if (minadd < add - errdelta)
+                minadd = add - errdelta;
+            if (maxadd > add + errdelta)
+                maxadd = add + errdelta;
+        }
+
+        // See if next point would further limit the add range
+        int32_t c = outer_maxinterval * nextcount;
+        if (minadd*nextaddfactor < nextpoint.minp - c)
+            minadd = idiv_up(nextpoint.minp - c, nextaddfactor);
+        c = outer_mininterval * nextcount;
+        if (maxadd*nextaddfactor > nextpoint.maxp - c)
+            maxadd = idiv_down(nextpoint.maxp - c, nextaddfactor);
+
+        // Bisect valid add range and try again with new 'add'
+        if (minadd > maxadd)
+            break;
+        add = maxadd - (maxadd - minadd) / 4;
+    }
+    if (zerocount + zerocount/16 >= bestcount)
+        // Prefer add=0 if it's similar to the best found sequence
+        return (struct step_move){ zerointerval, zerocount, 0 };
+    return (struct step_move){ bestinterval, bestcount, bestadd };
+}
+
+
+/****************************************************************
+ * Step compress checking
+ ****************************************************************/
+
+// Verify that a given 'step_move' matches the actual step times
+static int
+check_line(struct stepcompress *sc, struct step_move move)
+{
+    if (!CHECK_LINES)
+        return 0;
+    if (!move.count || (!move.interval && !move.add && move.count > 1)
+        || move.interval >= 0x80000000) {
+        errorf("stepcompress o=%d i=%d c=%d a=%d: Invalid sequence"
+               , sc->oid, move.interval, move.count, move.add);
+        return ERROR_RET;
+    }
+    uint32_t interval = move.interval, p = 0;
+    uint16_t i;
+    for (i=0; i<move.count; i++) {
+        struct points point = minmax_point(sc, sc->queue_pos + i);
+        p += interval;
+        if (p < point.minp || p > point.maxp) {
+            errorf("stepcompress o=%d i=%d c=%d a=%d: Point %d: %d not in %d:%d"
+                   , sc->oid, move.interval, move.count, move.add
+                   , i+1, p, point.minp, point.maxp);
+            return ERROR_RET;
+        }
+        if (interval >= 0x80000000) {
+            errorf("stepcompress o=%d i=%d c=%d a=%d:"
+                   " Point %d: interval overflow %d"
+                   , sc->oid, move.interval, move.count, move.add
+                   , i+1, interval);
+            return ERROR_RET;
+        }
+        interval += move.add;
+    }
+    return 0;
+}
+
+
+/****************************************************************
+ * Step compress interface
+ ****************************************************************/
+
+// Allocate a new 'stepcompress' object
+struct stepcompress * __visible
+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;
+}
+
+// Fill message id information
+void __visible
+stepcompress_fill(struct stepcompress *sc, uint32_t max_error
+                  , int32_t queue_step_msgtag, int32_t set_next_step_dir_msgtag)
+{
+    sc->max_error = max_error;
+    sc->queue_step_msgtag = queue_step_msgtag;
+    sc->set_next_step_dir_msgtag = set_next_step_dir_msgtag;
+}
+
+// Set the inverted stepper direction flag
+void __visible
+stepcompress_set_invert_sdir(struct stepcompress *sc, uint32_t invert_sdir)
+{
+    invert_sdir = !!invert_sdir;
+    if (invert_sdir != sc->invert_sdir) {
+        sc->invert_sdir = invert_sdir;
+        if (sc->sdir >= 0)
+            sc->sdir ^= 1;
+    }
+}
+
+// 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_steps *hs = list_last_entry(
+            &sc->history_list, struct history_steps, node);
+        if (hs->last_clock > end_clock)
+            break;
+        list_del(&hs->node);
+        free(hs);
+    }
+}
+
+// Free memory associated with a 'stepcompress' object
+void __visible
+stepcompress_free(struct stepcompress *sc)
+{
+    if (!sc)
+        return;
+    free(sc->queue);
+    message_queue_free(&sc->msg_queue);
+    free_history(sc, UINT64_MAX);
+    free(sc);
+}
+
+uint32_t
+stepcompress_get_oid(struct stepcompress *sc)
+{
+    return sc->oid;
+}
+
+int
+stepcompress_get_step_dir(struct stepcompress *sc)
+{
+    return sc->next_step_dir;
+}
+
+// Determine the "print time" of the last_step_clock
+static void
+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
+static void
+stepcompress_set_time(struct stepcompress *sc
+                      , double time_offset, double mcu_freq)
+{
+    sc->mcu_time_offset = time_offset;
+    sc->mcu_freq = mcu_freq;
+    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_steps *hs = malloc(sizeof(*hs));
+    hs->first_clock = first_clock;
+    hs->last_clock = last_clock;
+    hs->start_position = sc->last_position;
+    hs->interval = move->interval;
+    hs->add = move->add;
+    hs->step_count = sc->sdir ? move->count : -move->count;
+    sc->last_position += hs->step_count;
+    list_add_head(&hs->node, &sc->history_list);
+}
+
+// Convert previously scheduled steps into commands for the mcu
+static int
+queue_flush(struct stepcompress *sc, uint64_t move_clock)
+{
+    if (sc->queue_pos >= sc->queue_next)
+        return 0;
+    while (sc->last_step_clock < move_clock) {
+        struct step_move move = compress_bisect_add(sc);
+        int ret = check_line(sc, move);
+        if (ret)
+            return ret;
+
+        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;
+            break;
+        }
+        sc->queue_pos += move.count;
+    }
+    calc_last_step_print_time(sc);
+    return 0;
+}
+
+// Generate a queue_step for a step far in the future from the last step
+static int
+stepcompress_flush_far(struct stepcompress *sc, uint64_t abs_step_clock)
+{
+    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;
+}
+
+// Send the set_next_step_dir command
+static int
+set_next_step_dir(struct stepcompress *sc, int sdir)
+{
+    if (sc->sdir == sdir)
+        return 0;
+    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
+    };
+    struct queue_message *qm = message_alloc_and_encode(msg, 3);
+    qm->req_clock = sc->last_step_clock;
+    list_add_tail(&qm->node, &sc->msg_queue);
+    return 0;
+}
+
+// Slow path for queue_append() - handle next step far in future
+static int
+queue_append_far(struct stepcompress *sc)
+{
+    uint64_t step_clock = sc->next_step_clock;
+    sc->next_step_clock = 0;
+    int ret = queue_flush(sc, step_clock - CLOCK_DIFF_MAX + 1);
+    if (ret)
+        return ret;
+    if (step_clock >= sc->last_step_clock + CLOCK_DIFF_MAX)
+        return stepcompress_flush_far(sc, step_clock);
+    *sc->queue_next++ = step_clock;
+    return 0;
+}
+
+// Slow path for queue_append() - expand the internal queue storage
+static int
+queue_append_extend(struct stepcompress *sc)
+{
+    if (sc->queue_next - sc->queue_pos > 65535 + 2000) {
+        // No point in keeping more than 64K steps in memory
+        uint32_t flush = (*(sc->queue_next-65535)
+                          - (uint32_t)sc->last_step_clock);
+        int ret = queue_flush(sc, sc->last_step_clock + flush);
+        if (ret)
+            return ret;
+    }
+
+    if (sc->queue_next >= sc->queue_end) {
+        // Make room in the queue
+        int in_use = sc->queue_next - sc->queue_pos;
+        if (sc->queue_pos > sc->queue) {
+            // Shuffle the internal queue to avoid having to allocate more ram
+            memmove(sc->queue, sc->queue_pos, in_use * sizeof(*sc->queue));
+        } else {
+            // Expand the internal queue of step times
+            int alloc = sc->queue_end - sc->queue;
+            if (!alloc)
+                alloc = QUEUE_START_SIZE;
+            while (in_use >= alloc)
+                alloc *= 2;
+            sc->queue = realloc(sc->queue, alloc * sizeof(*sc->queue));
+            sc->queue_end = sc->queue + alloc;
+        }
+        sc->queue_pos = sc->queue;
+        sc->queue_next = sc->queue + in_use;
+    }
+
+    *sc->queue_next++ = sc->next_step_clock;
+    sc->next_step_clock = 0;
+    return 0;
+}
+
+// Add a step time to the queue (flushing the queue if needed)
+static int
+queue_append(struct stepcompress *sc)
+{
+    if (unlikely(sc->next_step_dir != sc->sdir)) {
+        int ret = set_next_step_dir(sc, sc->next_step_dir);
+        if (ret)
+            return ret;
+    }
+    if (unlikely(sc->next_step_clock >= sc->last_step_clock + CLOCK_DIFF_MAX))
+        return queue_append_far(sc);
+    if (unlikely(sc->queue_next >= sc->queue_end))
+        return queue_append_extend(sc);
+    *sc->queue_next++ = sc->next_step_clock;
+    sc->next_step_clock = 0;
+    return 0;
+}
+
+#define SDS_FILTER_TIME .000750
+
+// Add next step time
+int
+stepcompress_append(struct stepcompress *sc, int sdir
+                    , double print_time, double step_time)
+{
+    // Calculate step clock
+    double offset = print_time - sc->last_step_print_time;
+    double rel_sc = (step_time + offset) * sc->mcu_freq;
+    uint64_t step_clock = sc->last_step_clock + (uint64_t)rel_sc;
+    // Flush previous pending step (if any)
+    if (sc->next_step_clock) {
+        if (unlikely(sdir != sc->next_step_dir)) {
+            double diff = (int64_t)(step_clock - sc->next_step_clock);
+            if (diff < SDS_FILTER_TIME * sc->mcu_freq) {
+                // Rollback last step to avoid rapid step+dir+step
+                sc->next_step_clock = 0;
+                sc->next_step_dir = sdir;
+                return 0;
+            }
+        }
+        int ret = queue_append(sc);
+        if (ret)
+            return ret;
+    }
+    // Store this step as the next pending step
+    sc->next_step_clock = step_clock;
+    sc->next_step_dir = sdir;
+    return 0;
+}
+
+// Commit next pending step (ie, do not allow a rollback)
+int
+stepcompress_commit(struct stepcompress *sc)
+{
+    if (sc->next_step_clock)
+        return queue_append(sc);
+    return 0;
+}
+
+// Flush pending steps
+static int
+stepcompress_flush(struct stepcompress *sc, uint64_t move_clock)
+{
+    if (sc->next_step_clock && move_clock >= sc->next_step_clock) {
+        int ret = queue_append(sc);
+        if (ret)
+            return ret;
+    }
+    return queue_flush(sc, move_clock);
+}
+
+// Reset the internal state of the stepcompress object
+int __visible
+stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock)
+{
+    int ret = stepcompress_flush(sc, UINT64_MAX);
+    if (ret)
+        return ret;
+    sc->last_step_clock = last_step_clock;
+    sc->sdir = -1;
+    calc_last_step_print_time(sc);
+    return 0;
+}
+
+// Set last_position in the stepcompress object
+int __visible
+stepcompress_set_last_position(struct stepcompress *sc, uint64_t clock
+                               , int64_t last_position)
+{
+    int ret = stepcompress_flush(sc, UINT64_MAX);
+    if (ret)
+        return ret;
+    sc->last_position = last_position;
+
+    // Add a marker to the history list
+    struct history_steps *hs = malloc(sizeof(*hs));
+    memset(hs, 0, sizeof(*hs));
+    hs->first_clock = hs->last_clock = clock;
+    hs->start_position = last_position;
+    list_add_head(&hs->node, &sc->history_list);
+    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_steps *hs;
+    list_for_each_entry(hs, &sc->history_list, node) {
+        if (clock < hs->first_clock) {
+            last_position = hs->start_position;
+            continue;
+        }
+        if (clock >= hs->last_clock)
+            return hs->start_position + hs->step_count;
+        int32_t interval = hs->interval, add = hs->add;
+        int32_t ticks = (int32_t)(clock - hs->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 (hs->step_count < 0)
+            return hs->start_position - offset;
+        return hs->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)
+{
+    int ret = stepcompress_flush(sc, UINT64_MAX);
+    if (ret)
+        return ret;
+
+    struct queue_message *qm = message_alloc_and_encode(data, len);
+    qm->req_clock = sc->last_step_clock;
+    list_add_tail(&qm->node, &sc->msg_queue);
+    return 0;
+}
+
+// Return history of queue_step commands
+int __visible
+stepcompress_extract_old(struct stepcompress *sc, struct pull_history_steps *p
+                         , int max, uint64_t start_clock, uint64_t end_clock)
+{
+    int res = 0;
+    struct history_steps *hs;
+    list_for_each_entry(hs, &sc->history_list, node) {
+        if (start_clock >= hs->last_clock || res >= max)
+            break;
+        if (end_clock <= hs->first_clock)
+            continue;
+        p->first_clock = hs->first_clock;
+        p->last_clock = hs->last_clock;
+        p->start_position = hs->start_position;
+        p->step_count = hs->step_count;
+        p->interval = hs->interval;
+        p->add = hs->add;
+        p++;
+        res++;
+    }
+    return res;
+}
+
+
+/****************************************************************
+ * Step compress synchronization
+ ****************************************************************/
+
+// The steppersync object is used to synchronize the output of mcu
+// step commands.  The mcu can only queue a limited number of step
+// commands - this code tracks when items on the mcu step queue become
+// free so that new commands can be transmitted.  It also ensures the
+// mcu step queue is ordered between steppers so that no stepper
+// starves the other steppers of space in the mcu step queue.
+
+struct steppersync {
+    // Serial port
+    struct serialqueue *sq;
+    struct command_queue *cq;
+    // Storage for associated stepcompress objects
+    struct stepcompress **sc_list;
+    int sc_num;
+    // Storage for list of pending move clocks
+    uint64_t *move_clocks;
+    int num_move_clocks;
+};
+
+// Allocate a new 'steppersync' object
+struct steppersync * __visible
+steppersync_alloc(struct serialqueue *sq, struct stepcompress **sc_list
+                  , int sc_num, int move_num)
+{
+    struct steppersync *ss = malloc(sizeof(*ss));
+    memset(ss, 0, sizeof(*ss));
+    ss->sq = sq;
+    ss->cq = serialqueue_alloc_commandqueue();
+
+    ss->sc_list = malloc(sizeof(*sc_list)*sc_num);
+    memcpy(ss->sc_list, sc_list, sizeof(*sc_list)*sc_num);
+    ss->sc_num = sc_num;
+
+    ss->move_clocks = malloc(sizeof(*ss->move_clocks)*move_num);
+    memset(ss->move_clocks, 0, sizeof(*ss->move_clocks)*move_num);
+    ss->num_move_clocks = move_num;
+
+    return ss;
+}
+
+// Free memory associated with a 'steppersync' object
+void __visible
+steppersync_free(struct steppersync *ss)
+{
+    if (!ss)
+        return;
+    free(ss->sc_list);
+    free(ss->move_clocks);
+    serialqueue_free_commandqueue(ss->cq);
+    free(ss);
+}
+
+// Set the conversion rate of 'print_time' to mcu clock
+void __visible
+steppersync_set_time(struct steppersync *ss, double time_offset
+                     , double mcu_freq)
+{
+    int i;
+    for (i=0; i<ss->sc_num; i++) {
+        struct stepcompress *sc = ss->sc_list[i];
+        stepcompress_set_time(sc, time_offset, mcu_freq);
+    }
+}
+
+// Implement a binary heap algorithm to track when the next available
+// 'struct move' in the mcu will be available
+static void
+heap_replace(struct steppersync *ss, uint64_t req_clock)
+{
+    uint64_t *mc = ss->move_clocks;
+    int nmc = ss->num_move_clocks, pos = 0;
+    for (;;) {
+        int child1_pos = 2*pos+1, child2_pos = 2*pos+2;
+        uint64_t child2_clock = child2_pos < nmc ? mc[child2_pos] : UINT64_MAX;
+        uint64_t child1_clock = child1_pos < nmc ? mc[child1_pos] : UINT64_MAX;
+        if (req_clock <= child1_clock && req_clock <= child2_clock) {
+            mc[pos] = req_clock;
+            break;
+        }
+        if (child1_clock < child2_clock) {
+            mc[pos] = child1_clock;
+            pos = child1_pos;
+        } else {
+            mc[pos] = child2_clock;
+            pos = child2_pos;
+        }
+    }
+}
+
+// Find and transmit any scheduled steps prior to the given 'move_clock'
+int __visible
+steppersync_flush(struct steppersync *ss, uint64_t move_clock)
+{
+    // Flush each stepcompress to the specified move_clock
+    int i;
+    for (i=0; i<ss->sc_num; i++) {
+        int ret = stepcompress_flush(ss->sc_list[i], move_clock);
+        if (ret)
+            return ret;
+    }
+
+    // Order commands by the reqclock of each pending command
+    struct list_head msgs;
+    list_init(&msgs);
+    for (;;) {
+        // Find message with lowest reqclock
+        uint64_t req_clock = MAX_CLOCK;
+        struct queue_message *qm = NULL;
+        for (i=0; i<ss->sc_num; i++) {
+            struct stepcompress *sc = ss->sc_list[i];
+            if (!list_empty(&sc->msg_queue)) {
+                struct queue_message *m = list_first_entry(
+                    &sc->msg_queue, struct queue_message, node);
+                if (m->req_clock < req_clock) {
+                    qm = m;
+                    req_clock = m->req_clock;
+                }
+            }
+        }
+        if (!qm || (qm->min_clock && req_clock > move_clock))
+            break;
+
+        uint64_t next_avail = ss->move_clocks[0];
+        if (qm->min_clock)
+            // The qm->min_clock field is overloaded to indicate that
+            // the command uses the 'move queue' and to store the time
+            // that move queue item becomes available.
+            heap_replace(ss, qm->min_clock);
+        // Reset the min_clock to its normal meaning (minimum transmit time)
+        qm->min_clock = next_avail;
+
+        // Batch this command
+        list_del(&qm->node);
+        list_add_tail(&qm->node, &msgs);
+    }
+
+    // Transmit commands
+    if (!list_empty(&msgs))
+        serialqueue_send_batch(ss->sq, ss->cq, &msgs);
+    return 0;
+}

klippy/chelper/stepcompress.h

@@ -0,0 +1,45 @@
+#ifndef STEPCOMPRESS_H
+#define STEPCOMPRESS_H
+
+#include <stdint.h> // uint32_t
+
+#define ERROR_RET -989898989
+
+struct pull_history_steps {
+    uint64_t first_clock, last_clock;
+    int64_t start_position;
+    int step_count, interval, add;
+};
+
+struct stepcompress *stepcompress_alloc(uint32_t oid);
+void stepcompress_fill(struct stepcompress *sc, uint32_t max_error
+                       , int32_t queue_step_msgtag
+                       , int32_t set_next_step_dir_msgtag);
+void stepcompress_set_invert_sdir(struct stepcompress *sc
+                                  , uint32_t invert_sdir);
+void stepcompress_free(struct stepcompress *sc);
+uint32_t stepcompress_get_oid(struct stepcompress *sc);
+int stepcompress_get_step_dir(struct stepcompress *sc);
+int stepcompress_append(struct stepcompress *sc, int sdir
+                        , double print_time, double step_time);
+int stepcompress_commit(struct stepcompress *sc);
+int stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock);
+int stepcompress_set_last_position(struct stepcompress *sc, uint64_t clock
+                                   , 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);
+int stepcompress_extract_old(struct stepcompress *sc
+                             , struct pull_history_steps *p, int max
+                             , uint64_t start_clock, uint64_t end_clock);
+
+struct serialqueue;
+struct steppersync *steppersync_alloc(
+    struct serialqueue *sq, struct stepcompress **sc_list, int sc_num
+    , int move_num);
+void steppersync_free(struct steppersync *ss);
+void steppersync_set_time(struct steppersync *ss, double time_offset
+                          , double mcu_freq);
+int steppersync_flush(struct steppersync *ss, uint64_t move_clock);
+
+#endif // stepcompress.h

klippy/chelper/trapq.c

@@ -0,0 +1,258 @@
+// Trapezoidal velocity movement queue
+//
+// Copyright (C) 2018-2021  Kevin O'Connor <kevin@koconnor.net>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include <math.h> // sqrt
+#include <stddef.h> // offsetof
+#include <stdlib.h> // malloc
+#include <string.h> // memset
+#include "compiler.h" // unlikely
+#include "trapq.h" // move_get_coord
+
+// Allocate a new 'move' object
+struct move *
+move_alloc(void)
+{
+    struct move *m = malloc(sizeof(*m));
+    memset(m, 0, sizeof(*m));
+    return m;
+}
+
+// Fill and add a move to the trapezoid velocity queue
+void __visible
+trapq_append(struct trapq *tq, double print_time
+             , double accel_t, double cruise_t, double decel_t
+             , double start_pos_x, double start_pos_y, double start_pos_z
+             , double axes_r_x, double axes_r_y, double axes_r_z
+             , double start_v, double cruise_v, double accel)
+{
+    struct coord start_pos = { .x=start_pos_x, .y=start_pos_y, .z=start_pos_z };
+    struct coord axes_r = { .x=axes_r_x, .y=axes_r_y, .z=axes_r_z };
+    if (accel_t) {
+        struct move *m = move_alloc();
+        m->print_time = print_time;
+        m->move_t = accel_t;
+        m->start_v = start_v;
+        m->half_accel = .5 * accel;
+        m->start_pos = start_pos;
+        m->axes_r = axes_r;
+        trapq_add_move(tq, m);
+
+        print_time += accel_t;
+        start_pos = move_get_coord(m, accel_t);
+    }
+    if (cruise_t) {
+        struct move *m = move_alloc();
+        m->print_time = print_time;
+        m->move_t = cruise_t;
+        m->start_v = cruise_v;
+        m->half_accel = 0.;
+        m->start_pos = start_pos;
+        m->axes_r = axes_r;
+        trapq_add_move(tq, m);
+
+        print_time += cruise_t;
+        start_pos = move_get_coord(m, cruise_t);
+    }
+    if (decel_t) {
+        struct move *m = move_alloc();
+        m->print_time = print_time;
+        m->move_t = decel_t;
+        m->start_v = cruise_v;
+        m->half_accel = -.5 * accel;
+        m->start_pos = start_pos;
+        m->axes_r = axes_r;
+        trapq_add_move(tq, m);
+    }
+}
+
+// Return the distance moved given a time in a move
+inline double
+move_get_distance(struct move *m, double move_time)
+{
+    return (m->start_v + m->half_accel * move_time) * move_time;
+}
+
+// Return the XYZ coordinates given a time in a move
+inline struct coord
+move_get_coord(struct move *m, double move_time)
+{
+    double move_dist = move_get_distance(m, move_time);
+    return (struct coord) {
+        .x = m->start_pos.x + m->axes_r.x * move_dist,
+        .y = m->start_pos.y + m->axes_r.y * move_dist,
+        .z = m->start_pos.z + m->axes_r.z * move_dist };
+}
+
+#define NEVER_TIME 9999999999999999.9
+
+// Allocate a new 'trapq' object
+struct trapq * __visible
+trapq_alloc(void)
+{
+    struct trapq *tq = malloc(sizeof(*tq));
+    memset(tq, 0, sizeof(*tq));
+    list_init(&tq->moves);
+    list_init(&tq->history);
+    struct move *head_sentinel = move_alloc(), *tail_sentinel = move_alloc();
+    tail_sentinel->print_time = tail_sentinel->move_t = NEVER_TIME;
+    list_add_head(&head_sentinel->node, &tq->moves);
+    list_add_tail(&tail_sentinel->node, &tq->moves);
+    return tq;
+}
+
+// Free memory associated with a 'trapq' object
+void __visible
+trapq_free(struct trapq *tq)
+{
+    while (!list_empty(&tq->moves)) {
+        struct move *m = list_first_entry(&tq->moves, struct move, node);
+        list_del(&m->node);
+        free(m);
+    }
+    while (!list_empty(&tq->history)) {
+        struct move *m = list_first_entry(&tq->history, struct move, node);
+        list_del(&m->node);
+        free(m);
+    }
+    free(tq);
+}
+
+// Update the list sentinels
+void
+trapq_check_sentinels(struct trapq *tq)
+{
+    struct move *tail_sentinel = list_last_entry(&tq->moves, struct move, node);
+    if (tail_sentinel->print_time)
+        // Already up to date
+        return;
+    struct move *m = list_prev_entry(tail_sentinel, node);
+    struct move *head_sentinel = list_first_entry(&tq->moves, struct move,node);
+    if (m == head_sentinel) {
+        // No moves at all on this list
+        tail_sentinel->print_time = NEVER_TIME;
+        return;
+    }
+    tail_sentinel->print_time = m->print_time + m->move_t;
+    tail_sentinel->start_pos = move_get_coord(m, m->move_t);
+}
+
+#define MAX_NULL_MOVE 1.0
+
+// Add a move to the trapezoid velocity queue
+void
+trapq_add_move(struct trapq *tq, struct move *m)
+{
+    struct move *tail_sentinel = list_last_entry(&tq->moves, struct move, node);
+    struct move *prev = list_prev_entry(tail_sentinel, node);
+    if (prev->print_time + prev->move_t < m->print_time) {
+        // Add a null move to fill time gap
+        struct move *null_move = move_alloc();
+        null_move->start_pos = m->start_pos;
+        if (!prev->print_time && m->print_time > MAX_NULL_MOVE)
+            // Limit the first null move to improve numerical stability
+            null_move->print_time = m->print_time - MAX_NULL_MOVE;
+        else
+            null_move->print_time = prev->print_time + prev->move_t;
+        null_move->move_t = m->print_time - null_move->print_time;
+        list_add_before(&null_move->node, &tail_sentinel->node);
+    }
+    list_add_before(&m->node, &tail_sentinel->node);
+    tail_sentinel->print_time = 0.;
+}
+
+#define HISTORY_EXPIRE (30.0)
+
+// Expire any moves older than `print_time` from the trapezoid velocity queue
+void __visible
+trapq_finalize_moves(struct trapq *tq, double print_time)
+{
+    struct move *head_sentinel = list_first_entry(&tq->moves, struct move,node);
+    struct move *tail_sentinel = list_last_entry(&tq->moves, struct move, node);
+    // Move expired moves from main "moves" list to "history" list
+    for (;;) {
+        struct move *m = list_next_entry(head_sentinel, node);
+        if (m == tail_sentinel) {
+            tail_sentinel->print_time = NEVER_TIME;
+            break;
+        }
+        if (m->print_time + m->move_t > print_time)
+            break;
+        list_del(&m->node);
+        if (m->start_v || m->half_accel)
+            list_add_head(&m->node, &tq->history);
+        else
+            free(m);
+    }
+    // Free old moves from history list
+    if (list_empty(&tq->history))
+        return;
+    struct move *latest = list_first_entry(&tq->history, struct move, node);
+    double expire_time = latest->print_time + latest->move_t - HISTORY_EXPIRE;
+    for (;;) {
+        struct move *m = list_last_entry(&tq->history, struct move, node);
+        if (m == latest || m->print_time + m->move_t > expire_time)
+            break;
+        list_del(&m->node);
+        free(m);
+    }
+}
+
+// Note a position change in the trapq history
+void __visible
+trapq_set_position(struct trapq *tq, double print_time
+                   , double pos_x, double pos_y, double pos_z)
+{
+    // Flush all moves from trapq
+    trapq_finalize_moves(tq, NEVER_TIME);
+
+    // Prune any moves in the trapq history that were interrupted
+    while (!list_empty(&tq->history)) {
+        struct move *m = list_first_entry(&tq->history, struct move, node);
+        if (m->print_time < print_time) {
+            if (m->print_time + m->move_t > print_time)
+                m->move_t = print_time - m->print_time;
+            break;
+        }
+        list_del(&m->node);
+        free(m);
+    }
+
+    // Add a marker to the trapq history
+    struct move *m = move_alloc();
+    m->print_time = print_time;
+    m->start_pos.x = pos_x;
+    m->start_pos.y = pos_y;
+    m->start_pos.z = pos_z;
+    list_add_head(&m->node, &tq->history);
+}
+
+// Return history of movement queue
+int __visible
+trapq_extract_old(struct trapq *tq, struct pull_move *p, int max
+                  , double start_time, double end_time)
+{
+    int res = 0;
+    struct move *m;
+    list_for_each_entry(m, &tq->history, node) {
+        if (start_time >= m->print_time + m->move_t || res >= max)
+            break;
+        if (end_time <= m->print_time)
+            continue;
+        p->print_time = m->print_time;
+        p->move_t = m->move_t;
+        p->start_v = m->start_v;
+        p->accel = 2. * m->half_accel;
+        p->start_x = m->start_pos.x;
+        p->start_y = m->start_pos.y;
+        p->start_z = m->start_pos.z;
+        p->x_r = m->axes_r.x;
+        p->y_r = m->axes_r.y;
+        p->z_r = m->axes_r.z;
+        p++;
+        res++;
+    }
+    return res;
+}

klippy/chelper/trapq.h

@@ -0,0 +1,52 @@
+#ifndef TRAPQ_H
+#define TRAPQ_H
+
+#include "list.h" // list_node
+
+struct coord {
+    union {
+        struct {
+            double x, y, z;
+        };
+        double axis[3];
+    };
+};
+
+struct move {
+    double print_time, move_t;
+    double start_v, half_accel;
+    struct coord start_pos, axes_r;
+
+    struct list_node node;
+};
+
+struct trapq {
+    struct list_head moves, history;
+};
+
+struct pull_move {
+    double print_time, move_t;
+    double start_v, accel;
+    double start_x, start_y, start_z;
+    double x_r, y_r, z_r;
+};
+
+struct move *move_alloc(void);
+void trapq_append(struct trapq *tq, double print_time
+                  , double accel_t, double cruise_t, double decel_t
+                  , double start_pos_x, double start_pos_y, double start_pos_z
+                  , double axes_r_x, double axes_r_y, double axes_r_z
+                  , double start_v, double cruise_v, double accel);
+double move_get_distance(struct move *m, double move_time);
+struct coord move_get_coord(struct move *m, double move_time);
+struct trapq *trapq_alloc(void);
+void trapq_free(struct trapq *tq);
+void trapq_check_sentinels(struct trapq *tq);
+void trapq_add_move(struct trapq *tq, struct move *m);
+void trapq_finalize_moves(struct trapq *tq, double print_time);
+void trapq_set_position(struct trapq *tq, double print_time
+                        , double pos_x, double pos_y, double pos_z);
+int trapq_extract_old(struct trapq *tq, struct pull_move *p, int max
+                      , double start_time, double end_time);
+
+#endif // trapq.h

klippy/chelper/trdispatch.c

@@ -0,0 +1,226 @@
+// Trigger sync "trsync" message dispatch
+//
+// Copyright (C) 2021  Kevin O'Connor <kevin@koconnor.net>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include <pthread.h> // pthread_mutex_lock
+#include <stddef.h> // offsetof
+#include <stdlib.h> // malloc
+#include <string.h> // memset
+#include "compiler.h" // ARRAY_SIZE
+#include "list.h" // list_add_tail
+#include "pollreactor.h" // PR_NEVER
+#include "pyhelper.h" // report_errno
+#include "serialqueue.h" // serialqueue_add_fastreader
+
+struct trdispatch {
+    struct list_head tdm_list;
+
+    pthread_mutex_t lock; // protects variables below
+    uint32_t is_active, can_trigger, dispatch_reason;
+};
+
+struct trdispatch_mcu {
+    struct fastreader fr;
+    struct trdispatch *td;
+    struct list_node node;
+    struct serialqueue *sq;
+    struct command_queue *cq;
+    uint32_t trsync_oid, set_timeout_msgtag, trigger_msgtag;
+
+    // Remaining fields protected by trdispatch lock
+    uint64_t last_status_clock, expire_clock;
+    uint64_t expire_ticks, min_extend_ticks;
+    struct clock_estimate ce;
+};
+
+// Send: trsync_trigger oid=%c reason=%c
+static void
+send_trsync_trigger(struct trdispatch_mcu *tdm)
+{
+    uint32_t msg[3] = {
+        tdm->trigger_msgtag, tdm->trsync_oid, tdm->td->dispatch_reason
+    };
+    struct queue_message *qm = message_alloc_and_encode(msg, ARRAY_SIZE(msg));
+    serialqueue_send_one(tdm->sq, tdm->cq, qm);
+}
+
+// Send: trsync_set_timeout oid=%c clock=%u
+static void
+send_trsync_set_timeout(struct trdispatch_mcu *tdm)
+{
+    uint32_t msg[3] = {
+        tdm->set_timeout_msgtag, tdm->trsync_oid, tdm->expire_clock
+    };
+    struct queue_message *qm = message_alloc_and_encode(msg, ARRAY_SIZE(msg));
+    qm->req_clock = tdm->expire_clock;
+    serialqueue_send_one(tdm->sq, tdm->cq, qm);
+}
+
+// Handle a trsync_state message (callback from serialqueue fastreader)
+static void
+handle_trsync_state(struct fastreader *fr, uint8_t *data, int len)
+{
+    struct trdispatch_mcu *tdm = container_of(fr, struct trdispatch_mcu, fr);
+
+    // Parse: trsync_state oid=%c can_trigger=%c trigger_reason=%c clock=%u
+    uint32_t fields[5];
+    int ret = msgblock_decode(fields, ARRAY_SIZE(fields), data, len);
+    if (ret || fields[1] != tdm->trsync_oid)
+        return;
+    uint32_t can_trigger=fields[2], clock=fields[4];
+
+    // Process message
+    struct trdispatch *td = tdm->td;
+    pthread_mutex_lock(&td->lock);
+    if (!td->can_trigger)
+        goto done;
+
+    if (!can_trigger) {
+        // mcu reports trigger or timeout - propagate to all mcus
+        td->can_trigger = 0;
+        struct trdispatch_mcu *m;
+        list_for_each_entry(m, &td->tdm_list, node) {
+            send_trsync_trigger(m);
+        }
+        goto done;
+    }
+
+    // mcu is still working okay - update last_status_clock
+    serialqueue_get_clock_est(tdm->sq, &tdm->ce);
+    tdm->last_status_clock = clock_from_clock32(&tdm->ce, clock);
+
+    // Determine minimum acknowledged time among all mcus
+    double min_time = PR_NEVER, next_min_time = PR_NEVER;
+    struct trdispatch_mcu *m, *min_tdm = NULL;
+    list_for_each_entry(m, &td->tdm_list, node) {
+        double status_time = clock_to_time(&m->ce, m->last_status_clock);
+        if (status_time < next_min_time) {
+            next_min_time = status_time;
+            if (status_time < min_time) {
+                next_min_time = min_time;
+                min_time = status_time;
+                min_tdm = m;
+            }
+        }
+    }
+    if (next_min_time == PR_NEVER)
+        next_min_time = min_time;
+
+    // Send trsync_set_timeout messages to other mcus (if needed)
+    list_for_each_entry(m, &td->tdm_list, node) {
+        double status_time = m == min_tdm ? next_min_time : min_time;
+        uint64_t expire=clock_from_time(&m->ce, status_time) + m->expire_ticks;
+        if ((int64_t)(expire - m->expire_clock) >= m->min_extend_ticks) {
+            m->expire_clock = expire;
+            send_trsync_set_timeout(m);
+        }
+    }
+
+done:
+    pthread_mutex_unlock(&td->lock);
+}
+
+// Begin synchronization
+void __visible
+trdispatch_start(struct trdispatch *td, uint32_t dispatch_reason)
+{
+    pthread_mutex_lock(&td->lock);
+    if (td->is_active || list_empty(&td->tdm_list)) {
+        pthread_mutex_unlock(&td->lock);
+        return;
+    }
+    td->dispatch_reason = dispatch_reason;
+    td->is_active = td->can_trigger = 1;
+    pthread_mutex_unlock(&td->lock);
+
+    // Register handle_trsync_state message parser for each mcu
+    struct trdispatch_mcu *tdm;
+    list_for_each_entry(tdm, &td->tdm_list, node) {
+        serialqueue_add_fastreader(tdm->sq, &tdm->fr);
+    }
+}
+
+// Cleanup after a test completes
+void __visible
+trdispatch_stop(struct trdispatch *td)
+{
+    pthread_mutex_lock(&td->lock);
+    if (!td->is_active) {
+        pthread_mutex_unlock(&td->lock);
+        return;
+    }
+    td->is_active = 0;
+    pthread_mutex_unlock(&td->lock);
+
+    // Unregister handle_trsync_state message parsers
+    struct trdispatch_mcu *tdm;
+    list_for_each_entry(tdm, &td->tdm_list, node) {
+        serialqueue_rm_fastreader(tdm->sq, &tdm->fr);
+    }
+}
+
+// Create a new 'struct trdispatch' object
+struct trdispatch * __visible
+trdispatch_alloc(void)
+{
+    struct trdispatch *td = malloc(sizeof(*td));
+    memset(td, 0, sizeof(*td));
+
+    list_init(&td->tdm_list);
+
+    int ret = pthread_mutex_init(&td->lock, NULL);
+    if (ret) {
+        report_errno("trdispatch_alloc pthread_mutex_init", ret);
+        return NULL;
+    }
+    return td;
+}
+
+// Create a new 'struct trdispatch_mcu' object
+struct trdispatch_mcu * __visible
+trdispatch_mcu_alloc(struct trdispatch *td, struct serialqueue *sq
+                     , struct command_queue *cq, uint32_t trsync_oid
+                     , uint32_t set_timeout_msgtag, uint32_t trigger_msgtag
+                     , uint32_t state_msgtag)
+{
+    struct trdispatch_mcu *tdm = malloc(sizeof(*tdm));
+    memset(tdm, 0, sizeof(*tdm));
+
+    tdm->sq = sq;
+    tdm->cq = cq;
+    tdm->trsync_oid = trsync_oid;
+    tdm->set_timeout_msgtag = set_timeout_msgtag;
+    tdm->trigger_msgtag = trigger_msgtag;
+
+    // Setup fastreader to match trsync_state messages
+    uint32_t state_prefix[] = {state_msgtag, trsync_oid};
+    struct queue_message *dummy = message_alloc_and_encode(
+        state_prefix, ARRAY_SIZE(state_prefix));
+    memcpy(tdm->fr.prefix, dummy->msg, dummy->len);
+    tdm->fr.prefix_len = dummy->len;
+    free(dummy);
+    tdm->fr.func = handle_trsync_state;
+
+    tdm->td = td;
+    list_add_tail(&tdm->node, &td->tdm_list);
+
+    return tdm;
+}
+
+// Setup for a trigger test
+void __visible
+trdispatch_mcu_setup(struct trdispatch_mcu *tdm
+                     , uint64_t last_status_clock, uint64_t expire_clock
+                     , uint64_t expire_ticks, uint64_t min_extend_ticks)
+{
+    struct trdispatch *td = tdm->td;
+    pthread_mutex_lock(&td->lock);
+    tdm->last_status_clock = last_status_clock;
+    tdm->expire_clock = expire_clock;
+    tdm->expire_ticks = expire_ticks;
+    tdm->min_extend_ticks = min_extend_ticks;
+    serialqueue_get_clock_est(tdm->sq, &tdm->ce);
+    pthread_mutex_unlock(&td->lock);
+}

klippy/clocksync.py

@@ -0,0 +1,220 @@
+# Micro-controller clock synchronization
+#
+# Copyright (C) 2016-2018  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import logging, math
+
+RTT_AGE = .000010 / (60. * 60.)
+DECAY = 1. / 30.
+TRANSMIT_EXTRA = .001
+
+class ClockSync:
+    def __init__(self, reactor):
+        self.reactor = reactor
+        self.serial = None
+        self.get_clock_timer = reactor.register_timer(self._get_clock_event)
+        self.get_clock_cmd = self.cmd_queue = None
+        self.queries_pending = 0
+        self.mcu_freq = 1.
+        self.last_clock = 0
+        self.clock_est = (0., 0., 0.)
+        # Minimum round-trip-time tracking
+        self.min_half_rtt = 999999999.9
+        self.min_rtt_time = 0.
+        # Linear regression of mcu clock and system sent_time
+        self.time_avg = self.time_variance = 0.
+        self.clock_avg = self.clock_covariance = 0.
+        self.prediction_variance = 0.
+        self.last_prediction_time = 0.
+    def connect(self, serial):
+        self.serial = serial
+        self.mcu_freq = serial.msgparser.get_constant_float('CLOCK_FREQ')
+        # Load initial clock and frequency
+        params = serial.send_with_response('get_uptime', 'uptime')
+        self.last_clock = (params['high'] << 32) | params['clock']
+        self.clock_avg = self.last_clock
+        self.time_avg = params['#sent_time']
+        self.clock_est = (self.time_avg, self.clock_avg, self.mcu_freq)
+        self.prediction_variance = (.001 * self.mcu_freq)**2
+        # Enable periodic get_clock timer
+        for i in range(8):
+            self.reactor.pause(self.reactor.monotonic() + 0.050)
+            self.last_prediction_time = -9999.
+            params = serial.send_with_response('get_clock', 'clock')
+            self._handle_clock(params)
+        self.get_clock_cmd = serial.get_msgparser().create_command('get_clock')
+        self.cmd_queue = serial.alloc_command_queue()
+        serial.register_response(self._handle_clock, 'clock')
+        self.reactor.update_timer(self.get_clock_timer, self.reactor.NOW)
+    def connect_file(self, serial, pace=False):
+        self.serial = serial
+        self.mcu_freq = serial.msgparser.get_constant_float('CLOCK_FREQ')
+        self.clock_est = (0., 0., self.mcu_freq)
+        freq = 1000000000000.
+        if pace:
+            freq = self.mcu_freq
+        serial.set_clock_est(freq, self.reactor.monotonic(), 0, 0)
+    # MCU clock querying (_handle_clock is invoked from background thread)
+    def _get_clock_event(self, eventtime):
+        self.serial.raw_send(self.get_clock_cmd, 0, 0, self.cmd_queue)
+        self.queries_pending += 1
+        # Use an unusual time for the next event so clock messages
+        # don't resonate with other periodic events.
+        return eventtime + .9839
+    def _handle_clock(self, params):
+        self.queries_pending = 0
+        # Extend clock to 64bit
+        last_clock = self.last_clock
+        clock = (last_clock & ~0xffffffff) | params['clock']
+        if clock < last_clock:
+            clock += 0x100000000
+        self.last_clock = clock
+        # Check if this is the best round-trip-time seen so far
+        sent_time = params['#sent_time']
+        if not sent_time:
+            return
+        receive_time = params['#receive_time']
+        half_rtt = .5 * (receive_time - sent_time)
+        aged_rtt = (sent_time - self.min_rtt_time) * RTT_AGE
+        if half_rtt < self.min_half_rtt + aged_rtt:
+            self.min_half_rtt = half_rtt
+            self.min_rtt_time = sent_time
+            logging.debug("new minimum rtt %.3f: hrtt=%.6f freq=%d",
+                          sent_time, half_rtt, self.clock_est[2])
+        # Filter out samples that are extreme outliers
+        exp_clock = ((sent_time - self.time_avg) * self.clock_est[2]
+                     + self.clock_avg)
+        clock_diff2 = (clock - exp_clock)**2
+        if (clock_diff2 > 25. * self.prediction_variance
+            and clock_diff2 > (.000500 * self.mcu_freq)**2):
+            if clock > exp_clock and sent_time < self.last_prediction_time+10.:
+                logging.debug("Ignoring clock sample %.3f:"
+                              " freq=%d diff=%d stddev=%.3f",
+                              sent_time, self.clock_est[2], clock - exp_clock,
+                              math.sqrt(self.prediction_variance))
+                return
+            logging.info("Resetting prediction variance %.3f:"
+                         " freq=%d diff=%d stddev=%.3f",
+                         sent_time, self.clock_est[2], clock - exp_clock,
+                         math.sqrt(self.prediction_variance))
+            self.prediction_variance = (.001 * self.mcu_freq)**2
+        else:
+            self.last_prediction_time = sent_time
+            self.prediction_variance = (
+                (1. - DECAY) * (self.prediction_variance + clock_diff2 * DECAY))
+        # Add clock and sent_time to linear regression
+        diff_sent_time = sent_time - self.time_avg
+        self.time_avg += DECAY * diff_sent_time
+        self.time_variance = (1. - DECAY) * (
+            self.time_variance + diff_sent_time**2 * DECAY)
+        diff_clock = clock - self.clock_avg
+        self.clock_avg += DECAY * diff_clock
+        self.clock_covariance = (1. - DECAY) * (
+            self.clock_covariance + diff_sent_time * diff_clock * DECAY)
+        # Update prediction from linear regression
+        new_freq = self.clock_covariance / self.time_variance
+        pred_stddev = math.sqrt(self.prediction_variance)
+        self.serial.set_clock_est(new_freq, self.time_avg + TRANSMIT_EXTRA,
+                                  int(self.clock_avg - 3. * pred_stddev), clock)
+        self.clock_est = (self.time_avg + self.min_half_rtt,
+                          self.clock_avg, new_freq)
+        #logging.debug("regr %.3f: freq=%.3f d=%d(%.3f)",
+        #              sent_time, new_freq, clock - exp_clock, pred_stddev)
+    # clock frequency conversions
+    def print_time_to_clock(self, print_time):
+        return int(print_time * self.mcu_freq)
+    def clock_to_print_time(self, clock):
+        return clock / self.mcu_freq
+    # system time conversions
+    def get_clock(self, eventtime):
+        sample_time, clock, freq = self.clock_est
+        return int(clock + (eventtime - sample_time) * freq)
+    def estimate_clock_systime(self, reqclock):
+        sample_time, clock, freq = self.clock_est
+        return float(reqclock - clock)/freq + sample_time
+    def estimated_print_time(self, eventtime):
+        return self.clock_to_print_time(self.get_clock(eventtime))
+    # misc commands
+    def clock32_to_clock64(self, clock32):
+        last_clock = self.last_clock
+        clock_diff = (last_clock - clock32) & 0xffffffff
+        if clock_diff & 0x80000000:
+            return last_clock + 0x100000000 - clock_diff
+        return last_clock - clock_diff
+    def is_active(self):
+        return self.queries_pending <= 4
+    def dump_debug(self):
+        sample_time, clock, freq = self.clock_est
+        return ("clocksync state: mcu_freq=%d last_clock=%d"
+                " clock_est=(%.3f %d %.3f) min_half_rtt=%.6f min_rtt_time=%.3f"
+                " time_avg=%.3f(%.3f) clock_avg=%.3f(%.3f)"
+                " pred_variance=%.3f" % (
+                    self.mcu_freq, self.last_clock, sample_time, clock, freq,
+                    self.min_half_rtt, self.min_rtt_time,
+                    self.time_avg, self.time_variance,
+                    self.clock_avg, self.clock_covariance,
+                    self.prediction_variance))
+    def stats(self, eventtime):
+        sample_time, clock, freq = self.clock_est
+        return "freq=%d" % (freq,)
+    def calibrate_clock(self, print_time, eventtime):
+        return (0., self.mcu_freq)
+
+# Clock syncing code for secondary MCUs (whose clocks are sync'ed to a
+# primary MCU)
+class SecondarySync(ClockSync):
+    def __init__(self, reactor, main_sync):
+        ClockSync.__init__(self, reactor)
+        self.main_sync = main_sync
+        self.clock_adj = (0., 1.)
+        self.last_sync_time = 0.
+    def connect(self, serial):
+        ClockSync.connect(self, serial)
+        self.clock_adj = (0., self.mcu_freq)
+        curtime = self.reactor.monotonic()
+        main_print_time = self.main_sync.estimated_print_time(curtime)
+        local_print_time = self.estimated_print_time(curtime)
+        self.clock_adj = (main_print_time - local_print_time, self.mcu_freq)
+        self.calibrate_clock(0., curtime)
+    def connect_file(self, serial, pace=False):
+        ClockSync.connect_file(self, serial, pace)
+        self.clock_adj = (0., self.mcu_freq)
+    # clock frequency conversions
+    def print_time_to_clock(self, print_time):
+        adjusted_offset, adjusted_freq = self.clock_adj
+        return int((print_time - adjusted_offset) * adjusted_freq)
+    def clock_to_print_time(self, clock):
+        adjusted_offset, adjusted_freq = self.clock_adj
+        return clock / adjusted_freq + adjusted_offset
+    # misc commands
+    def dump_debug(self):
+        adjusted_offset, adjusted_freq = self.clock_adj
+        return "%s clock_adj=(%.3f %.3f)" % (
+            ClockSync.dump_debug(self), adjusted_offset, adjusted_freq)
+    def stats(self, eventtime):
+        adjusted_offset, adjusted_freq = self.clock_adj
+        return "%s adj=%d" % (ClockSync.stats(self, eventtime), adjusted_freq)
+    def calibrate_clock(self, print_time, eventtime):
+        # Calculate: est_print_time = main_sync.estimatated_print_time()
+        ser_time, ser_clock, ser_freq = self.main_sync.clock_est
+        main_mcu_freq = self.main_sync.mcu_freq
+        est_main_clock = (eventtime - ser_time) * ser_freq + ser_clock
+        est_print_time = est_main_clock / main_mcu_freq
+        # Determine sync1_print_time and sync2_print_time
+        sync1_print_time = max(print_time, est_print_time)
+        sync2_print_time = max(sync1_print_time + 4., self.last_sync_time,
+                               print_time + 2.5 * (print_time - est_print_time))
+        # Calc sync2_sys_time (inverse of main_sync.estimatated_print_time)
+        sync2_main_clock = sync2_print_time * main_mcu_freq
+        sync2_sys_time = ser_time + (sync2_main_clock - ser_clock) / ser_freq
+        # Adjust freq so estimated print_time will match at sync2_print_time
+        sync1_clock = self.print_time_to_clock(sync1_print_time)
+        sync2_clock = self.get_clock(sync2_sys_time)
+        adjusted_freq = ((sync2_clock - sync1_clock)
+                         / (sync2_print_time - sync1_print_time))
+        adjusted_offset = sync1_print_time - sync1_clock / adjusted_freq
+        # Apply new values
+        self.clock_adj = (adjusted_offset, adjusted_freq)
+        self.last_sync_time = sync2_print_time
+        return self.clock_adj

klippy/configfile.py

@@ -0,0 +1,419 @@
+# Code for reading and writing the Klipper config file
+#
+# Copyright (C) 2016-2021  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import sys, os, glob, re, time, logging, configparser, io
+
+error = configparser.Error
+
+class sentinel:
+    pass
+
+class ConfigWrapper:
+    error = configparser.Error
+    def __init__(self, printer, fileconfig, access_tracking, section):
+        self.printer = printer
+        self.fileconfig = fileconfig
+        self.access_tracking = access_tracking
+        self.section = section
+    def get_printer(self):
+        return self.printer
+    def get_name(self):
+        return self.section
+    def _get_wrapper(self, parser, option, default, minval=None, maxval=None,
+                     above=None, below=None, note_valid=True):
+        if not self.fileconfig.has_option(self.section, option):
+            if default is not sentinel:
+                if note_valid and default is not None:
+                    acc_id = (self.section.lower(), option.lower())
+                    self.access_tracking[acc_id] = default
+                return default
+            raise error("Option '%s' in section '%s' must be specified"
+                        % (option, self.section))
+        try:
+            v = parser(self.section, option)
+        except self.error as e:
+            raise
+        except:
+            raise error("Unable to parse option '%s' in section '%s'"
+                        % (option, self.section))
+        if note_valid:
+            self.access_tracking[(self.section.lower(), option.lower())] = v
+        if minval is not None and v < minval:
+            raise error("Option '%s' in section '%s' must have minimum of %s"
+                        % (option, self.section, minval))
+        if maxval is not None and v > maxval:
+            raise error("Option '%s' in section '%s' must have maximum of %s"
+                        % (option, self.section, maxval))
+        if above is not None and v <= above:
+            raise error("Option '%s' in section '%s' must be above %s"
+                        % (option, self.section, above))
+        if below is not None and v >= below:
+            raise self.error("Option '%s' in section '%s' must be below %s"
+                             % (option, self.section, below))
+        return v
+    def get(self, option, default=sentinel, note_valid=True):
+        return self._get_wrapper(self.fileconfig.get, option, default,
+                                 note_valid=note_valid)
+    def getint(self, option, default=sentinel, minval=None, maxval=None,
+               note_valid=True):
+        return self._get_wrapper(self.fileconfig.getint, option, default,
+                                 minval, maxval, note_valid=note_valid)
+    def getfloat(self, option, default=sentinel, minval=None, maxval=None,
+                 above=None, below=None, note_valid=True):
+        return self._get_wrapper(self.fileconfig.getfloat, option, default,
+                                 minval, maxval, above, below,
+                                 note_valid=note_valid)
+    def getboolean(self, option, default=sentinel, note_valid=True):
+        return self._get_wrapper(self.fileconfig.getboolean, option, default,
+                                 note_valid=note_valid)
+    def getchoice(self, option, choices, default=sentinel, note_valid=True):
+        if choices and type(list(choices.keys())[0]) == int:
+            c = self.getint(option, default, note_valid=note_valid)
+        else:
+            c = self.get(option, default, note_valid=note_valid)
+        if c not in choices:
+            raise error("Choice '%s' for option '%s' in section '%s'"
+                        " is not a valid choice" % (c, option, self.section))
+        return choices[c]
+    def getlists(self, option, default=sentinel, seps=(',',), count=None,
+                 parser=str, note_valid=True):
+        def lparser(value, pos):
+            if pos:
+                # Nested list
+                parts = [p.strip() for p in value.split(seps[pos])]
+                return tuple([lparser(p, pos - 1) for p in parts if p])
+            res = [parser(p.strip()) for p in value.split(seps[pos])]
+            if count is not None and len(res) != count:
+                raise error("Option '%s' in section '%s' must have %d elements"
+                            % (option, self.section, count))
+            return tuple(res)
+        def fcparser(section, option):
+            return lparser(self.fileconfig.get(section, option), len(seps) - 1)
+        return self._get_wrapper(fcparser, option, default,
+                                 note_valid=note_valid)
+    def getlist(self, option, default=sentinel, sep=',', count=None,
+                note_valid=True):
+        return self.getlists(option, default, seps=(sep,), count=count,
+                             parser=str, note_valid=note_valid)
+    def getintlist(self, option, default=sentinel, sep=',', count=None,
+                   note_valid=True):
+        return self.getlists(option, default, seps=(sep,), count=count,
+                             parser=int, note_valid=note_valid)
+    def getfloatlist(self, option, default=sentinel, sep=',', count=None,
+                     note_valid=True):
+        return self.getlists(option, default, seps=(sep,), count=count,
+                             parser=float, note_valid=note_valid)
+    def getsection(self, section):
+        return ConfigWrapper(self.printer, self.fileconfig,
+                             self.access_tracking, section)
+    def has_section(self, section):
+        return self.fileconfig.has_section(section)
+    def get_prefix_sections(self, prefix):
+        return [self.getsection(s) for s in self.fileconfig.sections()
+                if s.startswith(prefix)]
+    def get_prefix_options(self, prefix):
+        return [o for o in self.fileconfig.options(self.section)
+                if o.startswith(prefix)]
+    def deprecate(self, option, value=None):
+        if not self.fileconfig.has_option(self.section, option):
+            return
+        if value is None:
+            msg = ("Option '%s' in section '%s' is deprecated."
+                   % (option, self.section))
+        else:
+            msg = ("Value '%s' in option '%s' in section '%s' is deprecated."
+                   % (value, option, self.section))
+        pconfig = self.printer.lookup_object("configfile")
+        pconfig.deprecate(self.section, option, value, msg)
+
+AUTOSAVE_HEADER = """
+#*# <---------------------- SAVE_CONFIG ---------------------->
+#*# DO NOT EDIT THIS BLOCK OR BELOW. The contents are auto-generated.
+#*#
+"""
+
+class PrinterConfig:
+    def __init__(self, printer):
+        self.printer = printer
+        self.autosave = None
+        self.deprecated = {}
+        self.status_raw_config = {}
+        self.status_save_pending = {}
+        self.status_settings = {}
+        self.status_warnings = []
+        self.save_config_pending = False
+        gcode = self.printer.lookup_object('gcode')
+        gcode.register_command("SAVE_CONFIG", self.cmd_SAVE_CONFIG,
+                               desc=self.cmd_SAVE_CONFIG_help)
+    def get_printer(self):
+        return self.printer
+    def _read_config_file(self, filename):
+        try:
+            f = open(filename, 'r')
+            data = f.read()
+            f.close()
+        except:
+            msg = "Unable to open config file %s" % (filename,)
+            logging.exception(msg)
+            raise error(msg)
+        return data.replace('\r\n', '\n')
+    def _find_autosave_data(self, data):
+        regular_data = data
+        autosave_data = ""
+        pos = data.find(AUTOSAVE_HEADER)
+        if pos >= 0:
+            regular_data = data[:pos]
+            autosave_data = data[pos + len(AUTOSAVE_HEADER):].strip()
+        # Check for errors and strip line prefixes
+        if "\n#*# " in regular_data:
+            logging.warn("Can't read autosave from config file"
+                         " - autosave state corrupted")
+            return data, ""
+        out = [""]
+        for line in autosave_data.split('\n'):
+            if ((not line.startswith("#*#")
+                 or (len(line) >= 4 and not line.startswith("#*# ")))
+                and autosave_data):
+                logging.warn("Can't read autosave from config file"
+                             " - modifications after header")
+                return data, ""
+            out.append(line[4:])
+        out.append("")
+        return regular_data, "\n".join(out)
+    comment_r = re.compile('[#;].*$')
+    value_r = re.compile('[^A-Za-z0-9_].*$')
+    def _strip_duplicates(self, data, config):
+        fileconfig = config.fileconfig
+        # Comment out fields in 'data' that are defined in 'config'
+        lines = data.split('\n')
+        section = None
+        is_dup_field = False
+        for lineno, line in enumerate(lines):
+            pruned_line = self.comment_r.sub('', line).rstrip()
+            if not pruned_line:
+                continue
+            if pruned_line[0].isspace():
+                if is_dup_field:
+                    lines[lineno] = '#' + lines[lineno]
+                continue
+            is_dup_field = False
+            if pruned_line[0] == '[':
+                section = pruned_line[1:-1].strip()
+                continue
+            field = self.value_r.sub('', pruned_line)
+            if config.fileconfig.has_option(section, field):
+                is_dup_field = True
+                lines[lineno] = '#' + lines[lineno]
+        return "\n".join(lines)
+    def _parse_config_buffer(self, buffer, filename, fileconfig):
+        if not buffer:
+            return
+        data = '\n'.join(buffer)
+        del buffer[:]
+        sbuffer = io.StringIO(data)
+        fileconfig.readfp(sbuffer, filename)
+    def _resolve_include(self, source_filename, include_spec, fileconfig,
+                         visited):
+        dirname = os.path.dirname(source_filename)
+        include_spec = include_spec.strip()
+        include_glob = os.path.join(dirname, include_spec)
+        include_filenames = glob.glob(include_glob)
+        if not include_filenames and not glob.has_magic(include_glob):
+            # Empty set is OK if wildcard but not for direct file reference
+            raise error("Include file '%s' does not exist" % (include_glob,))
+        include_filenames.sort()
+        for include_filename in include_filenames:
+            include_data = self._read_config_file(include_filename)
+            self._parse_config(include_data, include_filename, fileconfig,
+                               visited)
+        return include_filenames
+    def _parse_config(self, data, filename, fileconfig, visited):
+        path = os.path.abspath(filename)
+        if path in visited:
+            raise error("Recursive include of config file '%s'" % (filename))
+        visited.add(path)
+        lines = data.split('\n')
+        # Buffer lines between includes and parse as a unit so that overrides
+        # in includes apply linearly as they do within a single file
+        buffer = []
+        for line in lines:
+            # Strip trailing comment
+            pos = line.find('#')
+            if pos >= 0:
+                line = line[:pos]
+            # Process include or buffer line
+            mo = configparser.RawConfigParser.SECTCRE.match(line)
+            header = mo and mo.group('header')
+            if header and header.startswith('include '):
+                self._parse_config_buffer(buffer, filename, fileconfig)
+                include_spec = header[8:].strip()
+                self._resolve_include(filename, include_spec, fileconfig,
+                                      visited)
+            else:
+                buffer.append(line)
+        self._parse_config_buffer(buffer, filename, fileconfig)
+        visited.remove(path)
+    def _build_config_wrapper(self, data, filename):
+        if sys.version_info.major >= 3:
+            fileconfig = configparser.RawConfigParser(
+                strict=False, inline_comment_prefixes=(';', '#'))
+        else:
+            fileconfig = configparser.RawConfigParser()
+        self._parse_config(data, filename, fileconfig, set())
+        return ConfigWrapper(self.printer, fileconfig, {}, 'printer')
+    def _build_config_string(self, config):
+        sfile = io.StringIO()
+        config.fileconfig.write(sfile)
+        return sfile.getvalue().strip()
+    def read_config(self, filename):
+        return self._build_config_wrapper(self._read_config_file(filename),
+                                          filename)
+    def read_main_config(self):
+        filename = self.printer.get_start_args()['config_file']
+        data = self._read_config_file(filename)
+        regular_data, autosave_data = self._find_autosave_data(data)
+        regular_config = self._build_config_wrapper(regular_data, filename)
+        autosave_data = self._strip_duplicates(autosave_data, regular_config)
+        self.autosave = self._build_config_wrapper(autosave_data, filename)
+        cfg = self._build_config_wrapper(regular_data + autosave_data, filename)
+        return cfg
+    def check_unused_options(self, config):
+        fileconfig = config.fileconfig
+        objects = dict(self.printer.lookup_objects())
+        # Determine all the fields that have been accessed
+        access_tracking = dict(config.access_tracking)
+        for section in self.autosave.fileconfig.sections():
+            for option in self.autosave.fileconfig.options(section):
+                access_tracking[(section.lower(), option.lower())] = 1
+        # Validate that there are no undefined parameters in the config file
+        valid_sections = { s: 1 for s, o in access_tracking }
+        for section_name in fileconfig.sections():
+            section = section_name.lower()
+            if section not in valid_sections and section not in objects:
+                raise error("Section '%s' is not a valid config section"
+                            % (section,))
+            for option in fileconfig.options(section_name):
+                option = option.lower()
+                if (section, option) not in access_tracking:
+                    raise error("Option '%s' is not valid in section '%s'"
+                                % (option, section))
+        # Setup get_status()
+        self._build_status(config)
+    def log_config(self, config):
+        lines = ["===== Config file =====",
+                 self._build_config_string(config),
+                 "======================="]
+        self.printer.set_rollover_info("config", "\n".join(lines))
+    # Status reporting
+    def deprecate(self, section, option, value=None, msg=None):
+        self.deprecated[(section, option, value)] = msg
+    def _build_status(self, config):
+        self.status_raw_config.clear()
+        for section in config.get_prefix_sections(''):
+            self.status_raw_config[section.get_name()] = section_status = {}
+            for option in section.get_prefix_options(''):
+                section_status[option] = section.get(option, note_valid=False)
+        self.status_settings = {}
+        for (section, option), value in config.access_tracking.items():
+            self.status_settings.setdefault(section, {})[option] = value
+        self.status_warnings = []
+        for (section, option, value), msg in self.deprecated.items():
+            if value is None:
+                res = {'type': 'deprecated_option'}
+            else:
+                res = {'type': 'deprecated_value', 'value': value}
+            res['message'] = msg
+            res['section'] = section
+            res['option'] = option
+            self.status_warnings.append(res)
+    def get_status(self, eventtime):
+        return {'config': self.status_raw_config,
+                'settings': self.status_settings,
+                'warnings': self.status_warnings,
+                'save_config_pending': self.save_config_pending,
+                'save_config_pending_items': self.status_save_pending}
+    # Autosave functions
+    def set(self, section, option, value):
+        if not self.autosave.fileconfig.has_section(section):
+            self.autosave.fileconfig.add_section(section)
+        svalue = str(value)
+        self.autosave.fileconfig.set(section, option, svalue)
+        pending = dict(self.status_save_pending)
+        if not section in pending or pending[section] is None:
+            pending[section] = {}
+        else:
+            pending[section] = dict(pending[section])
+        pending[section][option] = svalue
+        self.status_save_pending = pending
+        self.save_config_pending = True
+        logging.info("save_config: set [%s] %s = %s", section, option, svalue)
+    def remove_section(self, section):
+        if self.autosave.fileconfig.has_section(section):
+            self.autosave.fileconfig.remove_section(section)
+            pending = dict(self.status_save_pending)
+            pending[section] = None
+            self.status_save_pending = pending
+            self.save_config_pending = True
+        elif (section in self.status_save_pending and
+              self.status_save_pending[section] is not None):
+            pending = dict(self.status_save_pending)
+            del pending[section]
+            self.status_save_pending = pending
+            self.save_config_pending = True
+    def _disallow_include_conflicts(self, regular_data, cfgname, gcode):
+        config = self._build_config_wrapper(regular_data, cfgname)
+        for section in self.autosave.fileconfig.sections():
+            for option in self.autosave.fileconfig.options(section):
+                if config.fileconfig.has_option(section, option):
+                    msg = ("SAVE_CONFIG section '%s' option '%s' conflicts "
+                           "with included value" % (section, option))
+                    raise gcode.error(msg)
+    cmd_SAVE_CONFIG_help = "Overwrite config file and restart"
+    def cmd_SAVE_CONFIG(self, gcmd):
+        if not self.autosave.fileconfig.sections():
+            return
+        gcode = self.printer.lookup_object('gcode')
+        # Create string containing autosave data
+        autosave_data = self._build_config_string(self.autosave)
+        lines = [('#*# ' + l).strip()
+                 for l in autosave_data.split('\n')]
+        lines.insert(0, "\n" + AUTOSAVE_HEADER.rstrip())
+        lines.append("")
+        autosave_data = '\n'.join(lines)
+        # Read in and validate current config file
+        cfgname = self.printer.get_start_args()['config_file']
+        try:
+            data = self._read_config_file(cfgname)
+            regular_data, old_autosave_data = self._find_autosave_data(data)
+            config = self._build_config_wrapper(regular_data, cfgname)
+        except error as e:
+            msg = "Unable to parse existing config on SAVE_CONFIG"
+            logging.exception(msg)
+            raise gcode.error(msg)
+        regular_data = self._strip_duplicates(regular_data, self.autosave)
+        self._disallow_include_conflicts(regular_data, cfgname, gcode)
+        data = regular_data.rstrip() + autosave_data
+        # Determine filenames
+        datestr = time.strftime("-%Y%m%d_%H%M%S")
+        backup_name = cfgname + datestr
+        temp_name = cfgname + "_autosave"
+        if cfgname.endswith(".cfg"):
+            backup_name = cfgname[:-4] + datestr + ".cfg"
+            temp_name = cfgname[:-4] + "_autosave.cfg"
+        # Create new config file with temporary name and swap with main config
+        logging.info("SAVE_CONFIG to '%s' (backup in '%s')",
+                     cfgname, backup_name)
+        try:
+            f = open(temp_name, 'w')
+            f.write(data)
+            f.close()
+            os.rename(cfgname, backup_name)
+            os.rename(temp_name, cfgname)
+        except:
+            msg = "Unable to write config file during SAVE_CONFIG"
+            logging.exception(msg)
+            raise gcode.error(msg)
+        # Request a restart
+        gcode.request_restart('restart')

klippy/console.py

@@ -0,0 +1,240 @@
+#!/usr/bin/env python2
+# Script to implement a test console with firmware over serial port
+#
+# Copyright (C) 2016-2021  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import sys, optparse, os, re, logging
+import util, reactor, serialhdl, pins, msgproto, clocksync
+
+help_txt = """
+  This is a debugging console for the Klipper micro-controller.
+  In addition to mcu commands, the following artificial commands are
+  available:
+    PINS  : Load pin name aliases (eg, "PINS arduino")
+    DELAY : Send a command at a clock time (eg, "DELAY 9999 get_uptime")
+    FLOOD : Send a command many times (eg, "FLOOD 22 .01 get_uptime")
+    SUPPRESS : Suppress a response message (eg, "SUPPRESS analog_in_state 4")
+    SET   : Create a local variable (eg, "SET myvar 123.4")
+    STATS : Report serial statistics
+    LIST  : List available mcu commands, local commands, and local variables
+    HELP  : Show this text
+  All commands also support evaluation by enclosing an expression in { }.
+  For example, "reset_step_clock oid=4 clock={clock + freq}".  In addition
+  to user defined variables (via the SET command) the following builtin
+  variables may be used in expressions:
+    clock : The current mcu clock time (as estimated by the host)
+    freq  : The mcu clock frequency
+"""
+
+re_eval = re.compile(r'\{(?P<eval>[^}]*)\}')
+
+class KeyboardReader:
+    def __init__(self, reactor, serialport, baud, canbus_iface, canbus_nodeid):
+        self.serialport = serialport
+        self.baud = baud
+        self.canbus_iface = canbus_iface
+        self.canbus_nodeid = canbus_nodeid
+        self.ser = serialhdl.SerialReader(reactor)
+        self.reactor = reactor
+        self.start_time = reactor.monotonic()
+        self.clocksync = clocksync.ClockSync(self.reactor)
+        self.fd = sys.stdin.fileno()
+        util.set_nonblock(self.fd)
+        self.mcu_freq = 0
+        self.pins = pins.PinResolver(validate_aliases=False)
+        self.data = ""
+        reactor.register_fd(self.fd, self.process_kbd)
+        reactor.register_callback(self.connect)
+        self.local_commands = {
+            "SET": self.command_SET,
+            "DELAY": self.command_DELAY, "FLOOD": self.command_FLOOD,
+            "SUPPRESS": self.command_SUPPRESS, "STATS": self.command_STATS,
+            "LIST": self.command_LIST, "HELP": self.command_HELP,
+        }
+        self.eval_globals = {}
+    def connect(self, eventtime):
+        self.output(help_txt)
+        self.output("="*20 + " attempting to connect " + "="*20)
+        if self.canbus_iface is not None:
+            self.ser.connect_canbus(self.serialport, self.canbus_nodeid,
+                                    self.canbus_iface)
+        elif self.baud:
+            self.ser.connect_uart(self.serialport, self.baud)
+        else:
+            self.ser.connect_pipe(self.serialport)
+        msgparser = self.ser.get_msgparser()
+        message_count = len(msgparser.get_messages())
+        version, build_versions = msgparser.get_version_info()
+        self.output("Loaded %d commands (%s / %s)"
+                    % (message_count, version, build_versions))
+        self.output("MCU config: %s" % (" ".join(
+            ["%s=%s" % (k, v) for k, v in msgparser.get_constants().items()])))
+        self.clocksync.connect(self.ser)
+        self.ser.handle_default = self.handle_default
+        self.ser.register_response(self.handle_output, '#output')
+        self.mcu_freq = msgparser.get_constant_float('CLOCK_FREQ')
+        self.output("="*20 + "       connected       " + "="*20)
+        return self.reactor.NEVER
+    def output(self, msg):
+        sys.stdout.write("%s\n" % (msg,))
+        sys.stdout.flush()
+    def handle_default(self, params):
+        tdiff = params['#receive_time'] - self.start_time
+        msg = self.ser.get_msgparser().format_params(params)
+        self.output("%07.3f: %s" % (tdiff, msg))
+    def handle_output(self, params):
+        tdiff = params['#receive_time'] - self.start_time
+        self.output("%07.3f: %s: %s" % (tdiff, params['#name'], params['#msg']))
+    def handle_suppress(self, params):
+        pass
+    def update_evals(self, eventtime):
+        self.eval_globals['freq'] = self.mcu_freq
+        self.eval_globals['clock'] = self.clocksync.get_clock(eventtime)
+    def command_SET(self, parts):
+        val = parts[2]
+        try:
+            val = float(val)
+        except ValueError:
+            pass
+        self.eval_globals[parts[1]] = val
+    def command_DELAY(self, parts):
+        try:
+            val = int(parts[1])
+        except ValueError as e:
+            self.output("Error: %s" % (str(e),))
+            return
+        try:
+            self.ser.send(' '.join(parts[2:]), minclock=val)
+        except msgproto.error as e:
+            self.output("Error: %s" % (str(e),))
+            return
+    def command_FLOOD(self, parts):
+        try:
+            count = int(parts[1])
+            delay = float(parts[2])
+        except ValueError as e:
+            self.output("Error: %s" % (str(e),))
+            return
+        msg = ' '.join(parts[3:])
+        delay_clock = int(delay * self.mcu_freq)
+        msg_clock = int(self.clocksync.get_clock(self.reactor.monotonic())
+                        + self.mcu_freq * .200)
+        try:
+            for i in range(count):
+                next_clock = msg_clock + delay_clock
+                self.ser.send(msg, minclock=msg_clock, reqclock=next_clock)
+                msg_clock = next_clock
+        except msgproto.error as e:
+            self.output("Error: %s" % (str(e),))
+            return
+    def command_SUPPRESS(self, parts):
+        oid = None
+        try:
+            name = parts[1]
+            if len(parts) > 2:
+                oid = int(parts[2])
+        except ValueError as e:
+            self.output("Error: %s" % (str(e),))
+            return
+        self.ser.register_response(self.handle_suppress, name, oid)
+    def command_STATS(self, parts):
+        curtime = self.reactor.monotonic()
+        self.output(' '.join([self.ser.stats(curtime),
+                              self.clocksync.stats(curtime)]))
+    def command_LIST(self, parts):
+        self.update_evals(self.reactor.monotonic())
+        mp = self.ser.get_msgparser()
+        cmds = [msgformat for msgtag, msgtype, msgformat in mp.get_messages()
+                if msgtype == 'command']
+        out = "Available mcu commands:"
+        out += "\n  ".join([""] + sorted(cmds))
+        out += "\nAvailable artificial commands:"
+        out += "\n  ".join([""] + [n for n in sorted(self.local_commands)])
+        out += "\nAvailable local variables:"
+        lvars = sorted(self.eval_globals.items())
+        out += "\n  ".join([""] + ["%s: %s" % (k, v) for k, v in lvars])
+        self.output(out)
+    def command_HELP(self, parts):
+        self.output(help_txt)
+    def translate(self, line, eventtime):
+        evalparts = re_eval.split(line)
+        if len(evalparts) > 1:
+            self.update_evals(eventtime)
+            try:
+                for i in range(1, len(evalparts), 2):
+                    e = eval(evalparts[i], dict(self.eval_globals))
+                    if type(e) == type(0.):
+                        e = int(e)
+                    evalparts[i] = str(e)
+            except:
+                self.output("Unable to evaluate: %s" % (line,))
+                return None
+            line = ''.join(evalparts)
+            self.output("Eval: %s" % (line,))
+        try:
+            line = self.pins.update_command(line).strip()
+        except:
+            self.output("Unable to map pin: %s" % (line,))
+            return None
+        if line:
+            parts = line.split()
+            if parts[0] in self.local_commands:
+                self.local_commands[parts[0]](parts)
+                return None
+        return line
+    def process_kbd(self, eventtime):
+        self.data += str(os.read(self.fd, 4096).decode())
+
+        kbdlines = self.data.split('\n')
+        for line in kbdlines[:-1]:
+            line = line.strip()
+            cpos = line.find('#')
+            if cpos >= 0:
+                line = line[:cpos]
+                if not line:
+                    continue
+            msg = self.translate(line.strip(), eventtime)
+            if msg is None:
+                continue
+            try:
+                self.ser.send(msg)
+            except msgproto.error as e:
+                self.output("Error: %s" % (str(e),))
+        self.data = kbdlines[-1]
+
+def main():
+    usage = "%prog [options] <serialdevice>"
+    opts = optparse.OptionParser(usage)
+    opts.add_option("-v", action="store_true", dest="verbose",
+                    help="enable debug messages")
+    opts.add_option("-b", "--baud", type="int", dest="baud", help="baud rate")
+    opts.add_option("-c", "--canbus_iface", dest="canbus_iface",
+                    help="Use CAN bus interface; serialdevice is the chip UUID")
+    opts.add_option("-i", "--canbus_nodeid", type="int", dest="canbus_nodeid",
+                    default=64, help="The CAN nodeid to use (default 64)")
+    options, args = opts.parse_args()
+    if len(args) != 1:
+        opts.error("Incorrect number of arguments")
+    serialport = args[0]
+
+    baud = options.baud
+    if baud is None and not (serialport.startswith("/dev/rpmsg_")
+                             or serialport.startswith("/tmp/")):
+        baud = 250000
+
+    debuglevel = logging.INFO
+    if options.verbose:
+        debuglevel = logging.DEBUG
+    logging.basicConfig(level=debuglevel)
+
+    r = reactor.Reactor()
+    kbd = KeyboardReader(r, serialport, baud, options.canbus_iface,
+                         options.canbus_nodeid)
+    try:
+        r.run()
+    except KeyboardInterrupt:
+        sys.stdout.write("\n")
+
+if __name__ == '__main__':
+    main()

klippy/extras/__init__.py

@@ -0,0 +1,5 @@
+# Package definition for the extras directory
+#
+# Copyright (C) 2018  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.

klippy/extras/ad5206.py

@@ -0,0 +1,22 @@
+# AD5206 digipot code
+#
+# Copyright (C) 2017,2018  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+from . import bus
+
+class ad5206:
+    def __init__(self, config):
+        self.spi = bus.MCU_SPI_from_config(
+            config, 0, pin_option="enable_pin", default_speed=25000000)
+        scale = config.getfloat('scale', 1., above=0.)
+        for i in range(6):
+            val = config.getfloat('channel_%d' % (i+1,), None,
+                                  minval=0., maxval=scale)
+            if val is not None:
+                self.set_register(i, int(val * 256. / scale + .5))
+    def set_register(self, reg, value):
+        self.spi.spi_send([reg, value])
+
+def load_config_prefix(config):
+    return ad5206(config)

klippy/extras/adc_scaled.py

@@ -0,0 +1,79 @@
+# Support for scaling ADC values based on measured VREF and VSSA
+#
+# Copyright (C) 2020  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+
+SAMPLE_TIME = 0.001
+SAMPLE_COUNT = 8
+REPORT_TIME = 0.300
+RANGE_CHECK_COUNT = 4
+
+class MCU_scaled_adc:
+    def __init__(self, main, pin_params):
+        self._main = main
+        self._last_state = (0., 0.)
+        self._mcu_adc = main.mcu.setup_pin('adc', pin_params)
+        query_adc = main.printer.lookup_object('query_adc')
+        qname = main.name + ":" + pin_params['pin']
+        query_adc.register_adc(qname, self._mcu_adc)
+        self._callback = None
+        self.setup_minmax = self._mcu_adc.setup_minmax
+        self.get_mcu = self._mcu_adc.get_mcu
+    def _handle_callback(self, read_time, read_value):
+        max_adc = self._main.last_vref[1]
+        min_adc = self._main.last_vssa[1]
+        scaled_val = (read_value - min_adc) / (max_adc - min_adc)
+        self._last_state = (scaled_val, read_time)
+        self._callback(read_time, scaled_val)
+    def setup_adc_callback(self, report_time, callback):
+        self._callback = callback
+        self._mcu_adc.setup_adc_callback(report_time, self._handle_callback)
+    def get_last_value(self):
+        return self._last_state
+
+class PrinterADCScaled:
+    def __init__(self, config):
+        self.printer = config.get_printer()
+        self.name = config.get_name().split()[1]
+        self.last_vref = (0., 0.)
+        self.last_vssa = (0., 0.)
+        # Configure vref and vssa pins
+        self.mcu_vref = self._config_pin(config, 'vref', self.vref_callback)
+        self.mcu_vssa = self._config_pin(config, 'vssa', self.vssa_callback)
+        smooth_time = config.getfloat('smooth_time', 2., above=0.)
+        self.inv_smooth_time = 1. / smooth_time
+        self.mcu = self.mcu_vref.get_mcu()
+        if self.mcu is not self.mcu_vssa.get_mcu():
+            raise config.error("vref and vssa must be on same mcu")
+        # Register setup_pin
+        ppins = self.printer.lookup_object('pins')
+        ppins.register_chip(self.name, self)
+    def _config_pin(self, config, name, callback):
+        pin_name = config.get(name + '_pin')
+        ppins = self.printer.lookup_object('pins')
+        mcu_adc = ppins.setup_pin('adc', pin_name)
+        mcu_adc.setup_adc_callback(REPORT_TIME, callback)
+        mcu_adc.setup_minmax(SAMPLE_TIME, SAMPLE_COUNT, minval=0., maxval=1.,
+                             range_check_count=RANGE_CHECK_COUNT)
+        query_adc = config.get_printer().load_object(config, 'query_adc')
+        query_adc.register_adc(self.name + ":" + name, mcu_adc)
+        return mcu_adc
+    def setup_pin(self, pin_type, pin_params):
+        if pin_type != 'adc':
+            raise self.printer.config_error("adc_scaled only supports adc pins")
+        return MCU_scaled_adc(self, pin_params)
+    def calc_smooth(self, read_time, read_value, last):
+        last_time, last_value = last
+        time_diff = read_time - last_time
+        value_diff = read_value - last_value
+        adj_time = min(time_diff * self.inv_smooth_time, 1.)
+        smoothed_value = last_value + value_diff * adj_time
+        return (read_time, smoothed_value)
+    def vref_callback(self, read_time, read_value):
+        self.last_vref = self.calc_smooth(read_time, read_value, self.last_vref)
+    def vssa_callback(self, read_time, read_value):
+        self.last_vssa = self.calc_smooth(read_time, read_value, self.last_vssa)
+
+def load_config_prefix(config):
+    return PrinterADCScaled(config)

klippy/extras/adc_temperature.py

@@ -0,0 +1,302 @@
+# Obtain temperature using linear interpolation of ADC values
+#
+# Copyright (C) 2016-2018  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import logging, bisect
+
+
+######################################################################
+# Interface between MCU adc and heater temperature callbacks
+######################################################################
+
+SAMPLE_TIME = 0.001
+SAMPLE_COUNT = 8
+REPORT_TIME = 0.300
+RANGE_CHECK_COUNT = 4
+
+# Interface between ADC and heater temperature callbacks
+class PrinterADCtoTemperature:
+    def __init__(self, config, adc_convert):
+        self.adc_convert = adc_convert
+        ppins = config.get_printer().lookup_object('pins')
+        self.mcu_adc = ppins.setup_pin('adc', config.get('sensor_pin'))
+        self.mcu_adc.setup_adc_callback(REPORT_TIME, self.adc_callback)
+        query_adc = config.get_printer().load_object(config, 'query_adc')
+        query_adc.register_adc(config.get_name(), self.mcu_adc)
+    def setup_callback(self, temperature_callback):
+        self.temperature_callback = temperature_callback
+    def get_report_time_delta(self):
+        return REPORT_TIME
+    def adc_callback(self, read_time, read_value):
+        temp = self.adc_convert.calc_temp(read_value)
+        self.temperature_callback(read_time + SAMPLE_COUNT * SAMPLE_TIME, temp)
+    def setup_minmax(self, min_temp, max_temp):
+        adc_range = [self.adc_convert.calc_adc(t) for t in [min_temp, max_temp]]
+        self.mcu_adc.setup_minmax(SAMPLE_TIME, SAMPLE_COUNT,
+                                  minval=min(adc_range), maxval=max(adc_range),
+                                  range_check_count=RANGE_CHECK_COUNT)
+
+
+######################################################################
+# Linear interpolation
+######################################################################
+
+# Helper code to perform linear interpolation
+class LinearInterpolate:
+    def __init__(self, samples):
+        self.keys = []
+        self.slopes = []
+        last_key = last_value = None
+        for key, value in sorted(samples):
+            if last_key is None:
+                last_key = key
+                last_value = value
+                continue
+            if key <= last_key:
+                raise ValueError("duplicate value")
+            gain = (value - last_value) / (key - last_key)
+            offset = last_value - last_key * gain
+            if self.slopes and self.slopes[-1] == (gain, offset):
+                continue
+            last_value = value
+            last_key = key
+            self.keys.append(key)
+            self.slopes.append((gain, offset))
+        if not self.keys:
+            raise ValueError("need at least two samples")
+        self.keys.append(9999999999999.)
+        self.slopes.append(self.slopes[-1])
+    def interpolate(self, key):
+        pos = bisect.bisect(self.keys, key)
+        gain, offset = self.slopes[pos]
+        return key * gain + offset
+    def reverse_interpolate(self, value):
+        values = [key * gain + offset for key, (gain, offset) in zip(
+            self.keys, self.slopes)]
+        if values[0] < values[-2]:
+            valid = [i for i in range(len(values)) if values[i] >= value]
+        else:
+            valid = [i for i in range(len(values)) if values[i] <= value]
+        gain, offset = self.slopes[min(valid + [len(values) - 1])]
+        return (value - offset) / gain
+
+
+######################################################################
+# Linear voltage to temperature converter
+######################################################################
+
+# Linear style conversion chips calibrated from temperature measurements
+class LinearVoltage:
+    def __init__(self, config, params):
+        adc_voltage = config.getfloat('adc_voltage', 5., above=0.)
+        voltage_offset = config.getfloat('voltage_offset', 0.0)
+        samples = []
+        for temp, volt in params:
+            adc = (volt - voltage_offset) / adc_voltage
+            if adc < 0. or adc > 1.:
+                logging.warn("Ignoring adc sample %.3f/%.3f in heater %s",
+                             temp, volt, config.get_name())
+                continue
+            samples.append((adc, temp))
+        try:
+            li = LinearInterpolate(samples)
+        except ValueError as e:
+            raise config.error("adc_temperature %s in heater %s" % (
+                str(e), config.get_name()))
+        self.calc_temp = li.interpolate
+        self.calc_adc = li.reverse_interpolate
+
+# Custom defined sensors from the config file
+class CustomLinearVoltage:
+    def __init__(self, config):
+        self.name = " ".join(config.get_name().split()[1:])
+        self.params = []
+        for i in range(1, 1000):
+            t = config.getfloat("temperature%d" % (i,), None)
+            if t is None:
+                break
+            v = config.getfloat("voltage%d" % (i,))
+            self.params.append((t, v))
+    def create(self, config):
+        lv = LinearVoltage(config, self.params)
+        return PrinterADCtoTemperature(config, lv)
+
+
+######################################################################
+# Linear resistance to temperature converter
+######################################################################
+
+# Linear resistance calibrated from temperature measurements
+class LinearResistance:
+    def __init__(self, config, samples):
+        self.pullup = config.getfloat('pullup_resistor', 4700., above=0.)
+        try:
+            self.li = LinearInterpolate([(r, t) for t, r in samples])
+        except ValueError as e:
+            raise config.error("adc_temperature %s in heater %s" % (
+                str(e), config.get_name()))
+    def calc_temp(self, adc):
+        # Calculate temperature from adc
+        adc = max(.00001, min(.99999, adc))
+        r = self.pullup * adc / (1.0 - adc)
+        return self.li.interpolate(r)
+    def calc_adc(self, temp):
+        # Calculate adc reading from a temperature
+        r = self.li.reverse_interpolate(temp)
+        return r / (self.pullup + r)
+
+# Custom defined sensors from the config file
+class CustomLinearResistance:
+    def __init__(self, config):
+        self.name = " ".join(config.get_name().split()[1:])
+        self.samples = []
+        for i in range(1, 1000):
+            t = config.getfloat("temperature%d" % (i,), None)
+            if t is None:
+                break
+            r = config.getfloat("resistance%d" % (i,))
+            self.samples.append((t, r))
+    def create(self, config):
+        lr = LinearResistance(config, self.samples)
+        return PrinterADCtoTemperature(config, lr)
+
+
+######################################################################
+# Default sensors
+######################################################################
+
+AD595 = [
+    (0., .0027), (10., .101), (20., .200), (25., .250), (30., .300),
+    (40., .401), (50., .503), (60., .605), (80., .810), (100., 1.015),
+    (120., 1.219), (140., 1.420), (160., 1.620), (180., 1.817), (200., 2.015),
+    (220., 2.213), (240., 2.413), (260., 2.614), (280., 2.817), (300., 3.022),
+    (320., 3.227), (340., 3.434), (360., 3.641), (380., 3.849), (400., 4.057),
+    (420., 4.266), (440., 4.476), (460., 4.686), (480., 4.896)
+]
+
+AD597 = [
+    (0., 0.), (10., .097), (20., .196), (25., .245), (30., .295),
+    (40., 0.395), (50., 0.496), (60., 0.598), (80., 0.802), (100., 1.005),
+    (120., 1.207), (140., 1.407), (160., 1.605), (180., 1.801), (200., 1.997),
+    (220., 2.194), (240., 2.392), (260., 2.592), (280., 2.794), (300., 2.996),
+    (320., 3.201), (340., 3.406), (360., 3.611), (380., 3.817), (400., 4.024),
+    (420., 4.232), (440., 4.440), (460., 4.649), (480., 4.857), (500., 5.066)
+]
+
+AD8494 = [
+    (-180, -0.714), (-160, -0.658), (-140, -0.594), (-120, -0.523),
+    (-100, -0.446), (-80, -0.365), (-60, -0.278), (-40, -0.188),
+    (-20, -0.095), (0, 0.002), (20, 0.1), (25, 0.125), (40, 0.201),
+    (60, 0.303), (80, 0.406), (100, 0.511), (120, 0.617), (140, 0.723),
+    (160, 0.829), (180, 0.937), (200, 1.044), (220, 1.151), (240, 1.259),
+    (260, 1.366), (280, 1.473), (300, 1.58), (320, 1.687), (340, 1.794),
+    (360, 1.901), (380, 2.008), (400, 2.114), (420, 2.221), (440, 2.328),
+    (460, 2.435), (480, 2.542), (500, 2.65), (520, 2.759), (540, 2.868),
+    (560, 2.979), (580, 3.09), (600, 3.203), (620, 3.316), (640, 3.431),
+    (660, 3.548), (680, 3.666), (700, 3.786), (720, 3.906), (740, 4.029),
+    (760, 4.152), (780, 4.276), (800, 4.401), (820, 4.526), (840, 4.65),
+    (860, 4.774), (880, 4.897), (900, 5.018), (920, 5.138), (940, 5.257),
+    (960, 5.374), (980, 5.49), (1000, 5.606), (1020, 5.72), (1040, 5.833),
+    (1060, 5.946), (1080, 6.058), (1100, 6.17), (1120, 6.282), (1140, 6.394),
+    (1160, 6.505), (1180, 6.616), (1200, 6.727)
+]
+
+AD8495 = [
+    (-260, -0.786), (-240, -0.774), (-220, -0.751), (-200, -0.719),
+    (-180, -0.677), (-160, -0.627), (-140, -0.569), (-120, -0.504),
+    (-100, -0.432), (-80, -0.355), (-60, -0.272), (-40, -0.184), (-20, -0.093),
+    (0, 0.003), (20, 0.1), (25, 0.125), (40, 0.2), (60, 0.301), (80, 0.402),
+    (100, 0.504), (120, 0.605), (140, 0.705), (160, 0.803), (180, 0.901),
+    (200, 0.999), (220, 1.097), (240, 1.196), (260, 1.295), (280, 1.396),
+    (300, 1.497), (320, 1.599), (340, 1.701), (360, 1.803), (380, 1.906),
+    (400, 2.01), (420, 2.113), (440, 2.217), (460, 2.321), (480, 2.425),
+    (500, 2.529), (520, 2.634), (540, 2.738), (560, 2.843), (580, 2.947),
+    (600, 3.051), (620, 3.155), (640, 3.259), (660, 3.362), (680, 3.465),
+    (700, 3.568), (720, 3.67), (740, 3.772), (760, 3.874), (780, 3.975),
+    (800, 4.076), (820, 4.176), (840, 4.275), (860, 4.374), (880, 4.473),
+    (900, 4.571), (920, 4.669), (940, 4.766), (960, 4.863), (980, 4.959),
+    (1000, 5.055), (1020, 5.15), (1040, 5.245), (1060, 5.339), (1080, 5.432),
+    (1100, 5.525), (1120, 5.617), (1140, 5.709), (1160, 5.8), (1180, 5.891),
+    (1200, 5.98), (1220, 6.069), (1240, 6.158), (1260, 6.245), (1280, 6.332),
+    (1300, 6.418), (1320, 6.503), (1340, 6.587), (1360, 6.671), (1380, 6.754)
+]
+
+AD8496 = [
+    (-180, -0.642), (-160, -0.59), (-140, -0.53), (-120, -0.464),
+    (-100, -0.392), (-80, -0.315), (-60, -0.235), (-40, -0.15), (-20, -0.063),
+    (0, 0.027), (20, 0.119), (25, 0.142), (40, 0.213), (60, 0.308),
+    (80, 0.405), (100, 0.503), (120, 0.601), (140, 0.701), (160, 0.8),
+    (180, 0.9), (200, 1.001), (220, 1.101), (240, 1.201), (260, 1.302),
+    (280, 1.402), (300, 1.502), (320, 1.602), (340, 1.702), (360, 1.801),
+    (380, 1.901), (400, 2.001), (420, 2.1), (440, 2.2), (460, 2.3),
+    (480, 2.401), (500, 2.502), (520, 2.603), (540, 2.705), (560, 2.808),
+    (580, 2.912), (600, 3.017), (620, 3.124), (640, 3.231), (660, 3.34),
+    (680, 3.451), (700, 3.562), (720, 3.675), (740, 3.789), (760, 3.904),
+    (780, 4.02), (800, 4.137), (820, 4.254), (840, 4.37), (860, 4.486),
+    (880, 4.6), (900, 4.714), (920, 4.826), (940, 4.937), (960, 5.047),
+    (980, 5.155), (1000, 5.263), (1020, 5.369), (1040, 5.475), (1060, 5.581),
+    (1080, 5.686), (1100, 5.79), (1120, 5.895), (1140, 5.999), (1160, 6.103),
+    (1180, 6.207), (1200, 6.311)
+]
+
+AD8497 = [
+    (-260, -0.785), (-240, -0.773), (-220, -0.751), (-200, -0.718),
+    (-180, -0.676), (-160, -0.626), (-140, -0.568), (-120, -0.503),
+    (-100, -0.432), (-80, -0.354), (-60, -0.271), (-40, -0.184),
+    (-20, -0.092), (0, 0.003), (20, 0.101), (25, 0.126), (40, 0.2),
+    (60, 0.301), (80, 0.403), (100, 0.505), (120, 0.605), (140, 0.705),
+    (160, 0.804), (180, 0.902), (200, 0.999), (220, 1.097), (240, 1.196),
+    (260, 1.296), (280, 1.396), (300, 1.498), (320, 1.599), (340, 1.701),
+    (360, 1.804), (380, 1.907), (400, 2.01), (420, 2.114), (440, 2.218),
+    (460, 2.322), (480, 2.426), (500, 2.53), (520, 2.634), (540, 2.739),
+    (560, 2.843), (580, 2.948), (600, 3.052), (620, 3.156), (640, 3.259),
+    (660, 3.363), (680, 3.466), (700, 3.569), (720, 3.671), (740, 3.773),
+    (760, 3.874), (780, 3.976), (800, 4.076), (820, 4.176), (840, 4.276),
+    (860, 4.375), (880, 4.474), (900, 4.572), (920, 4.67), (940, 4.767),
+    (960, 4.863), (980, 4.96), (1000, 5.055), (1020, 5.151), (1040, 5.245),
+    (1060, 5.339), (1080, 5.433), (1100, 5.526), (1120, 5.618), (1140, 5.71),
+    (1160, 5.801), (1180, 5.891), (1200, 5.981), (1220, 6.07), (1240, 6.158),
+    (1260, 6.246), (1280, 6.332), (1300, 6.418), (1320, 6.503), (1340, 6.588),
+    (1360, 6.671), (1380, 6.754)
+]
+
+def calc_pt100(base=100.):
+    # Calc PT100/PT1000 resistances using Callendar-Van Dusen formula
+    A, B = (3.9083e-3, -5.775e-7)
+    return [(float(t), base * (1. + A*t + B*t*t)) for t in range(0, 500, 10)]
+
+def calc_ina826_pt100():
+    # Standard circuit is 4400ohm pullup with 10x gain to 5V
+    return [(t, 10. * 5. * r / (4400. + r)) for t, r in calc_pt100()]
+
+DefaultVoltageSensors = [
+    ("AD595", AD595), ("AD597", AD597), ("AD8494", AD8494), ("AD8495", AD8495),
+    ("AD8496", AD8496), ("AD8497", AD8497),
+    ("PT100 INA826", calc_ina826_pt100())
+]
+
+DefaultResistanceSensors = [
+    ("PT1000", calc_pt100(1000.))
+]
+
+def load_config(config):
+    # Register default sensors
+    pheaters = config.get_printer().load_object(config, "heaters")
+    for sensor_type, params in DefaultVoltageSensors:
+        func = (lambda config, params=params:
+                PrinterADCtoTemperature(config, LinearVoltage(config, params)))
+        pheaters.add_sensor_factory(sensor_type, func)
+    for sensor_type, params in DefaultResistanceSensors:
+        func = (lambda config, params=params:
+                PrinterADCtoTemperature(config,
+                                        LinearResistance(config, params)))
+        pheaters.add_sensor_factory(sensor_type, func)
+
+def load_config_prefix(config):
+    if config.get("resistance1", None) is None:
+        custom_sensor = CustomLinearVoltage(config)
+    else:
+        custom_sensor = CustomLinearResistance(config)
+    pheaters = config.get_printer().load_object(config, "heaters")
+    pheaters.add_sensor_factory(custom_sensor.name, custom_sensor.create)

klippy/extras/adxl345.py

@@ -0,0 +1,444 @@
+# Support for reading acceleration data from an adxl345 chip
+#
+# Copyright (C) 2020-2021  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import logging, time, collections, threading, multiprocessing, os
+from . import bus, motion_report
+
+# ADXL345 registers
+REG_DEVID = 0x00
+REG_BW_RATE = 0x2C
+REG_POWER_CTL = 0x2D
+REG_DATA_FORMAT = 0x31
+REG_FIFO_CTL = 0x38
+REG_MOD_READ = 0x80
+REG_MOD_MULTI = 0x40
+
+QUERY_RATES = {
+    25: 0x8, 50: 0x9, 100: 0xa, 200: 0xb, 400: 0xc,
+    800: 0xd, 1600: 0xe, 3200: 0xf,
+}
+
+ADXL345_DEV_ID = 0xe5
+SET_FIFO_CTL = 0x90
+
+FREEFALL_ACCEL = 9.80665 * 1000.
+SCALE = 0.0039 * FREEFALL_ACCEL # 3.9mg/LSB * Earth gravity in mm/s**2
+
+Accel_Measurement = collections.namedtuple(
+    'Accel_Measurement', ('time', 'accel_x', 'accel_y', 'accel_z'))
+
+# Helper class to obtain measurements
+class AccelQueryHelper:
+    def __init__(self, printer, cconn):
+        self.printer = printer
+        self.cconn = cconn
+        print_time = printer.lookup_object('toolhead').get_last_move_time()
+        self.request_start_time = self.request_end_time = print_time
+        self.samples = self.raw_samples = []
+    def finish_measurements(self):
+        toolhead = self.printer.lookup_object('toolhead')
+        self.request_end_time = toolhead.get_last_move_time()
+        toolhead.wait_moves()
+        self.cconn.finalize()
+    def _get_raw_samples(self):
+        raw_samples = self.cconn.get_messages()
+        if raw_samples:
+            self.raw_samples = raw_samples
+        return self.raw_samples
+    def has_valid_samples(self):
+        raw_samples = self._get_raw_samples()
+        for msg in raw_samples:
+            data = msg['params']['data']
+            first_sample_time = data[0][0]
+            last_sample_time = data[-1][0]
+            if (first_sample_time > self.request_end_time
+                    or last_sample_time < self.request_start_time):
+                continue
+            # The time intervals [first_sample_time, last_sample_time]
+            # and [request_start_time, request_end_time] have non-zero
+            # intersection. It is still theoretically possible that none
+            # of the samples from raw_samples fall into the time interval
+            # [request_start_time, request_end_time] if it is too narrow
+            # or on very heavy data losses. In practice, that interval
+            # is at least 1 second, so this possibility is negligible.
+            return True
+        return False
+    def get_samples(self):
+        raw_samples = self._get_raw_samples()
+        if not raw_samples:
+            return self.samples
+        total = sum([len(m['params']['data']) for m in raw_samples])
+        count = 0
+        self.samples = samples = [None] * total
+        for msg in raw_samples:
+            for samp_time, x, y, z in msg['params']['data']:
+                if samp_time < self.request_start_time:
+                    continue
+                if samp_time > self.request_end_time:
+                    break
+                samples[count] = Accel_Measurement(samp_time, x, y, z)
+                count += 1
+        del samples[count:]
+        return self.samples
+    def write_to_file(self, filename):
+        def write_impl():
+            try:
+                # Try to re-nice writing process
+                os.nice(20)
+            except:
+                pass
+            f = open(filename, "w")
+            f.write("#time,accel_x,accel_y,accel_z\n")
+            samples = self.samples or self.get_samples()
+            for t, accel_x, accel_y, accel_z in samples:
+                f.write("%.6f,%.6f,%.6f,%.6f\n" % (
+                    t, accel_x, accel_y, accel_z))
+            f.close()
+        write_proc = multiprocessing.Process(target=write_impl)
+        write_proc.daemon = True
+        write_proc.start()
+
+# Helper class for G-Code commands
+class AccelCommandHelper:
+    def __init__(self, config, chip):
+        self.printer = config.get_printer()
+        self.chip = chip
+        self.bg_client = None
+        name_parts = config.get_name().split()
+        self.base_name = name_parts[0]
+        self.name = name_parts[-1]
+        self.register_commands(self.name)
+        if len(name_parts) == 1:
+            if self.name == "adxl345" or not config.has_section("adxl345"):
+                self.register_commands(None)
+    def register_commands(self, name):
+        # Register commands
+        gcode = self.printer.lookup_object('gcode')
+        gcode.register_mux_command("ACCELEROMETER_MEASURE", "CHIP", name,
+                                   self.cmd_ACCELEROMETER_MEASURE,
+                                   desc=self.cmd_ACCELEROMETER_MEASURE_help)
+        gcode.register_mux_command("ACCELEROMETER_QUERY", "CHIP", name,
+                                   self.cmd_ACCELEROMETER_QUERY,
+                                   desc=self.cmd_ACCELEROMETER_QUERY_help)
+        gcode.register_mux_command("ACCELEROMETER_DEBUG_READ", "CHIP", name,
+                                   self.cmd_ACCELEROMETER_DEBUG_READ,
+                                   desc=self.cmd_ACCELEROMETER_DEBUG_READ_help)
+        gcode.register_mux_command("ACCELEROMETER_DEBUG_WRITE", "CHIP", name,
+                                   self.cmd_ACCELEROMETER_DEBUG_WRITE,
+                                   desc=self.cmd_ACCELEROMETER_DEBUG_WRITE_help)
+    cmd_ACCELEROMETER_MEASURE_help = "Start/stop accelerometer"
+    def cmd_ACCELEROMETER_MEASURE(self, gcmd):
+        if self.bg_client is None:
+            # Start measurements
+            self.bg_client = self.chip.start_internal_client()
+            gcmd.respond_info("accelerometer measurements started")
+            return
+        # End measurements
+        name = gcmd.get("NAME", time.strftime("%Y%m%d_%H%M%S"))
+        if not name.replace('-', '').replace('_', '').isalnum():
+            raise gcmd.error("Invalid NAME parameter")
+        bg_client = self.bg_client
+        self.bg_client = None
+        bg_client.finish_measurements()
+        # Write data to file
+        if self.base_name == self.name:
+            filename = "/tmp/%s-%s.csv" % (self.base_name, name)
+        else:
+            filename = "/tmp/%s-%s-%s.csv" % (self.base_name, self.name, name)
+        bg_client.write_to_file(filename)
+        gcmd.respond_info("Writing raw accelerometer data to %s file"
+                          % (filename,))
+    cmd_ACCELEROMETER_QUERY_help = "Query accelerometer for the current values"
+    def cmd_ACCELEROMETER_QUERY(self, gcmd):
+        aclient = self.chip.start_internal_client()
+        self.printer.lookup_object('toolhead').dwell(1.)
+        aclient.finish_measurements()
+        values = aclient.get_samples()
+        if not values:
+            raise gcmd.error("No accelerometer measurements found")
+        _, accel_x, accel_y, accel_z = values[-1]
+        gcmd.respond_info("accelerometer values (x, y, z): %.6f, %.6f, %.6f"
+                          % (accel_x, accel_y, accel_z))
+    cmd_ACCELEROMETER_DEBUG_READ_help = "Query register (for debugging)"
+    def cmd_ACCELEROMETER_DEBUG_READ(self, gcmd):
+        reg = gcmd.get("REG", minval=0, maxval=126, parser=lambda x: int(x, 0))
+        val = self.chip.read_reg(reg)
+        gcmd.respond_info("Accelerometer REG[0x%x] = 0x%x" % (reg, val))
+    cmd_ACCELEROMETER_DEBUG_WRITE_help = "Set register (for debugging)"
+    def cmd_ACCELEROMETER_DEBUG_WRITE(self, gcmd):
+        reg = gcmd.get("REG", minval=0, maxval=126, parser=lambda x: int(x, 0))
+        val = gcmd.get("VAL", minval=0, maxval=255, parser=lambda x: int(x, 0))
+        self.chip.set_reg(reg, val)
+
+# Helper class for chip clock synchronization via linear regression
+class ClockSyncRegression:
+    def __init__(self, mcu, chip_clock_smooth, decay = 1. / 20.):
+        self.mcu = mcu
+        self.chip_clock_smooth = chip_clock_smooth
+        self.decay = decay
+        self.last_chip_clock = self.last_exp_mcu_clock = 0.
+        self.mcu_clock_avg = self.mcu_clock_variance = 0.
+        self.chip_clock_avg = self.chip_clock_covariance = 0.
+    def reset(self, mcu_clock, chip_clock):
+        self.mcu_clock_avg = self.last_mcu_clock = mcu_clock
+        self.chip_clock_avg = chip_clock
+        self.mcu_clock_variance = self.chip_clock_covariance = 0.
+        self.last_chip_clock = self.last_exp_mcu_clock = 0.
+    def update(self, mcu_clock, chip_clock):
+        # Update linear regression
+        decay = self.decay
+        diff_mcu_clock = mcu_clock - self.mcu_clock_avg
+        self.mcu_clock_avg += decay * diff_mcu_clock
+        self.mcu_clock_variance = (1. - decay) * (
+            self.mcu_clock_variance + diff_mcu_clock**2 * decay)
+        diff_chip_clock = chip_clock - self.chip_clock_avg
+        self.chip_clock_avg += decay * diff_chip_clock
+        self.chip_clock_covariance = (1. - decay) * (
+            self.chip_clock_covariance + diff_mcu_clock*diff_chip_clock*decay)
+    def set_last_chip_clock(self, chip_clock):
+        base_mcu, base_chip, inv_cfreq = self.get_clock_translation()
+        self.last_chip_clock = chip_clock
+        self.last_exp_mcu_clock = base_mcu + (chip_clock-base_chip) * inv_cfreq
+    def get_clock_translation(self):
+        inv_chip_freq = self.mcu_clock_variance / self.chip_clock_covariance
+        if not self.last_chip_clock:
+            return self.mcu_clock_avg, self.chip_clock_avg, inv_chip_freq
+        # Find mcu clock associated with future chip_clock
+        s_chip_clock = self.last_chip_clock + self.chip_clock_smooth
+        scdiff = s_chip_clock - self.chip_clock_avg
+        s_mcu_clock = self.mcu_clock_avg + scdiff * inv_chip_freq
+        # Calculate frequency to converge at future point
+        mdiff = s_mcu_clock - self.last_exp_mcu_clock
+        s_inv_chip_freq = mdiff / self.chip_clock_smooth
+        return self.last_exp_mcu_clock, self.last_chip_clock, s_inv_chip_freq
+    def get_time_translation(self):
+        base_mcu, base_chip, inv_cfreq = self.get_clock_translation()
+        clock_to_print_time = self.mcu.clock_to_print_time
+        base_time = clock_to_print_time(base_mcu)
+        inv_freq = clock_to_print_time(base_mcu + inv_cfreq) - base_time
+        return base_time, base_chip, inv_freq
+
+MIN_MSG_TIME = 0.100
+
+BYTES_PER_SAMPLE = 5
+SAMPLES_PER_BLOCK = 10
+
+# Printer class that controls ADXL345 chip
+class ADXL345:
+    def __init__(self, config):
+        self.printer = config.get_printer()
+        AccelCommandHelper(config, self)
+        self.query_rate = 0
+        am = {'x': (0, SCALE), 'y': (1, SCALE), 'z': (2, SCALE),
+              '-x': (0, -SCALE), '-y': (1, -SCALE), '-z': (2, -SCALE)}
+        axes_map = config.getlist('axes_map', ('x','y','z'), count=3)
+        if any([a not in am for a in axes_map]):
+            raise config.error("Invalid adxl345 axes_map parameter")
+        self.axes_map = [am[a.strip()] for a in axes_map]
+        self.data_rate = config.getint('rate', 3200)
+        if self.data_rate not in QUERY_RATES:
+            raise config.error("Invalid rate parameter: %d" % (self.data_rate,))
+        # Measurement storage (accessed from background thread)
+        self.lock = threading.Lock()
+        self.raw_samples = []
+        # Setup mcu sensor_adxl345 bulk query code
+        self.spi = bus.MCU_SPI_from_config(config, 3, default_speed=5000000)
+        self.mcu = mcu = self.spi.get_mcu()
+        self.oid = oid = mcu.create_oid()
+        self.query_adxl345_cmd = self.query_adxl345_end_cmd = None
+        self.query_adxl345_status_cmd = None
+        mcu.add_config_cmd("config_adxl345 oid=%d spi_oid=%d"
+                           % (oid, self.spi.get_oid()))
+        mcu.add_config_cmd("query_adxl345 oid=%d clock=0 rest_ticks=0"
+                           % (oid,), on_restart=True)
+        mcu.register_config_callback(self._build_config)
+        mcu.register_response(self._handle_adxl345_data, "adxl345_data", oid)
+        # Clock tracking
+        self.last_sequence = self.max_query_duration = 0
+        self.last_limit_count = self.last_error_count = 0
+        self.clock_sync = ClockSyncRegression(self.mcu, 640)
+        # API server endpoints
+        self.api_dump = motion_report.APIDumpHelper(
+            self.printer, self._api_update, self._api_startstop, 0.100)
+        self.name = config.get_name().split()[-1]
+        wh = self.printer.lookup_object('webhooks')
+        wh.register_mux_endpoint("adxl345/dump_adxl345", "sensor", self.name,
+                                 self._handle_dump_adxl345)
+    def _build_config(self):
+        cmdqueue = self.spi.get_command_queue()
+        self.query_adxl345_cmd = self.mcu.lookup_command(
+            "query_adxl345 oid=%c clock=%u rest_ticks=%u", cq=cmdqueue)
+        self.query_adxl345_end_cmd = self.mcu.lookup_query_command(
+            "query_adxl345 oid=%c clock=%u rest_ticks=%u",
+            "adxl345_status oid=%c clock=%u query_ticks=%u next_sequence=%hu"
+            " buffered=%c fifo=%c limit_count=%hu", oid=self.oid, cq=cmdqueue)
+        self.query_adxl345_status_cmd = self.mcu.lookup_query_command(
+            "query_adxl345_status oid=%c",
+            "adxl345_status oid=%c clock=%u query_ticks=%u next_sequence=%hu"
+            " buffered=%c fifo=%c limit_count=%hu", oid=self.oid, cq=cmdqueue)
+    def read_reg(self, reg):
+        params = self.spi.spi_transfer([reg | REG_MOD_READ, 0x00])
+        response = bytearray(params['response'])
+        return response[1]
+    def set_reg(self, reg, val, minclock=0):
+        self.spi.spi_send([reg, val & 0xFF], minclock=minclock)
+        stored_val = self.read_reg(reg)
+        if stored_val != val:
+            raise self.printer.command_error(
+                    "Failed to set ADXL345 register [0x%x] to 0x%x: got 0x%x. "
+                    "This is generally indicative of connection problems "
+                    "(e.g. faulty wiring) or a faulty adxl345 chip." % (
+                        reg, val, stored_val))
+    # Measurement collection
+    def is_measuring(self):
+        return self.query_rate > 0
+    def _handle_adxl345_data(self, params):
+        with self.lock:
+            self.raw_samples.append(params)
+    def _extract_samples(self, raw_samples):
+        # Load variables to optimize inner loop below
+        (x_pos, x_scale), (y_pos, y_scale), (z_pos, z_scale) = self.axes_map
+        last_sequence = self.last_sequence
+        time_base, chip_base, inv_freq = self.clock_sync.get_time_translation()
+        # Process every message in raw_samples
+        count = seq = 0
+        samples = [None] * (len(raw_samples) * SAMPLES_PER_BLOCK)
+        for params in raw_samples:
+            seq_diff = (last_sequence - params['sequence']) & 0xffff
+            seq_diff -= (seq_diff & 0x8000) << 1
+            seq = last_sequence - seq_diff
+            d = bytearray(params['data'])
+            msg_cdiff = seq * SAMPLES_PER_BLOCK - chip_base
+            for i in range(len(d) // BYTES_PER_SAMPLE):
+                d_xyz = d[i*BYTES_PER_SAMPLE:(i+1)*BYTES_PER_SAMPLE]
+                xlow, ylow, zlow, xzhigh, yzhigh = d_xyz
+                if yzhigh & 0x80:
+                    self.last_error_count += 1
+                    continue
+                rx = (xlow | ((xzhigh & 0x1f) << 8)) - ((xzhigh & 0x10) << 9)
+                ry = (ylow | ((yzhigh & 0x1f) << 8)) - ((yzhigh & 0x10) << 9)
+                rz = ((zlow | ((xzhigh & 0xe0) << 3) | ((yzhigh & 0xe0) << 6))
+                      - ((yzhigh & 0x40) << 7))
+                raw_xyz = (rx, ry, rz)
+                x = round(raw_xyz[x_pos] * x_scale, 6)
+                y = round(raw_xyz[y_pos] * y_scale, 6)
+                z = round(raw_xyz[z_pos] * z_scale, 6)
+                ptime = round(time_base + (msg_cdiff + i) * inv_freq, 6)
+                samples[count] = (ptime, x, y, z)
+                count += 1
+        self.clock_sync.set_last_chip_clock(seq * SAMPLES_PER_BLOCK + i)
+        del samples[count:]
+        return samples
+    def _update_clock(self, minclock=0):
+        # Query current state
+        for retry in range(5):
+            params = self.query_adxl345_status_cmd.send([self.oid],
+                                                        minclock=minclock)
+            fifo = params['fifo'] & 0x7f
+            if fifo <= 32:
+                break
+        else:
+            raise self.printer.command_error("Unable to query adxl345 fifo")
+        mcu_clock = self.mcu.clock32_to_clock64(params['clock'])
+        sequence = (self.last_sequence & ~0xffff) | params['next_sequence']
+        if sequence < self.last_sequence:
+            sequence += 0x10000
+        self.last_sequence = sequence
+        buffered = params['buffered']
+        limit_count = (self.last_limit_count & ~0xffff) | params['limit_count']
+        if limit_count < self.last_limit_count:
+            limit_count += 0x10000
+        self.last_limit_count = limit_count
+        duration = params['query_ticks']
+        if duration > self.max_query_duration:
+            # Skip measurement as a high query time could skew clock tracking
+            self.max_query_duration = max(2 * self.max_query_duration,
+                                          self.mcu.seconds_to_clock(.000005))
+            return
+        self.max_query_duration = 2 * duration
+        msg_count = (sequence * SAMPLES_PER_BLOCK
+                     + buffered // BYTES_PER_SAMPLE + fifo)
+        # The "chip clock" is the message counter plus .5 for average
+        # inaccuracy of query responses and plus .5 for assumed offset
+        # of adxl345 hw processing time.
+        chip_clock = msg_count + 1
+        self.clock_sync.update(mcu_clock + duration // 2, chip_clock)
+    def _start_measurements(self):
+        if self.is_measuring():
+            return
+        # In case of miswiring, testing ADXL345 device ID prevents treating
+        # noise or wrong signal as a correctly initialized device
+        dev_id = self.read_reg(REG_DEVID)
+        if dev_id != ADXL345_DEV_ID:
+            raise self.printer.command_error(
+                "Invalid adxl345 id (got %x vs %x).\n"
+                "This is generally indicative of connection problems\n"
+                "(e.g. faulty wiring) or a faulty adxl345 chip."
+                % (dev_id, ADXL345_DEV_ID))
+        # Setup chip in requested query rate
+        self.set_reg(REG_POWER_CTL, 0x00)
+        self.set_reg(REG_DATA_FORMAT, 0x0B)
+        self.set_reg(REG_FIFO_CTL, 0x00)
+        self.set_reg(REG_BW_RATE, QUERY_RATES[self.data_rate])
+        self.set_reg(REG_FIFO_CTL, SET_FIFO_CTL)
+        # Setup samples
+        with self.lock:
+            self.raw_samples = []
+        # Start bulk reading
+        systime = self.printer.get_reactor().monotonic()
+        print_time = self.mcu.estimated_print_time(systime) + MIN_MSG_TIME
+        reqclock = self.mcu.print_time_to_clock(print_time)
+        rest_ticks = self.mcu.seconds_to_clock(4. / self.data_rate)
+        self.query_rate = self.data_rate
+        self.query_adxl345_cmd.send([self.oid, reqclock, rest_ticks],
+                                    reqclock=reqclock)
+        logging.info("ADXL345 starting '%s' measurements", self.name)
+        # Initialize clock tracking
+        self.last_sequence = 0
+        self.last_limit_count = self.last_error_count = 0
+        self.clock_sync.reset(reqclock, 0)
+        self.max_query_duration = 1 << 31
+        self._update_clock(minclock=reqclock)
+        self.max_query_duration = 1 << 31
+    def _finish_measurements(self):
+        if not self.is_measuring():
+            return
+        # Halt bulk reading
+        params = self.query_adxl345_end_cmd.send([self.oid, 0, 0])
+        self.query_rate = 0
+        with self.lock:
+            self.raw_samples = []
+        logging.info("ADXL345 finished '%s' measurements", self.name)
+    # API interface
+    def _api_update(self, eventtime):
+        self._update_clock()
+        with self.lock:
+            raw_samples = self.raw_samples
+            self.raw_samples = []
+        if not raw_samples:
+            return {}
+        samples = self._extract_samples(raw_samples)
+        if not samples:
+            return {}
+        return {'data': samples, 'errors': self.last_error_count,
+                'overflows': self.last_limit_count}
+    def _api_startstop(self, is_start):
+        if is_start:
+            self._start_measurements()
+        else:
+            self._finish_measurements()
+    def _handle_dump_adxl345(self, web_request):
+        self.api_dump.add_client(web_request)
+        hdr = ('time', 'x_acceleration', 'y_acceleration', 'z_acceleration')
+        web_request.send({'header': hdr})
+    def start_internal_client(self):
+        cconn = self.api_dump.add_internal_client()
+        return AccelQueryHelper(self.printer, cconn)
+
+def load_config(config):
+    return ADXL345(config)
+
+def load_config_prefix(config):
+    return ADXL345(config)

klippy/extras/angle.py

@@ -0,0 +1,578 @@
+# Support for reading SPI magnetic angle sensors
+#
+# Copyright (C) 2021,2022  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import logging, math, threading
+from . import bus, motion_report
+
+MIN_MSG_TIME = 0.100
+TCODE_ERROR = 0xff
+
+TRINAMIC_DRIVERS = ["tmc2130", "tmc2208", "tmc2209", "tmc2240", "tmc2660", "tmc5160"]
+
+CALIBRATION_BITS = 6 # 64 entries
+ANGLE_BITS = 16 # angles range from 0..65535
+
+class AngleCalibration:
+    def __init__(self, config):
+        self.printer = config.get_printer()
+        self.name = config.get_name()
+        self.stepper_name = config.get('stepper', None)
+        if self.stepper_name is None:
+            # No calibration
+            return
+        try:
+            import numpy
+        except:
+            raise config.error("Angle calibration requires numpy module")
+        sconfig = config.getsection(self.stepper_name)
+        sconfig.getint('microsteps', note_valid=False)
+        self.tmc_module = self.mcu_stepper = None
+        # Current calibration data
+        self.mcu_pos_offset = None
+        self.angle_phase_offset = 0.
+        self.calibration_reversed = False
+        self.calibration = []
+        cal = config.get('calibrate', None)
+        if cal is not None:
+            data = [d.strip() for d in cal.split(',')]
+            angles = [float(d) for d in data if d]
+            self.load_calibration(angles)
+        # Register commands
+        self.printer.register_event_handler("stepper:sync_mcu_position",
+                                            self.handle_sync_mcu_pos)
+        self.printer.register_event_handler("klippy:connect", self.connect)
+        cname = self.name.split()[-1]
+        gcode = self.printer.lookup_object('gcode')
+        gcode.register_mux_command("ANGLE_CALIBRATE", "CHIP",
+                                   cname, self.cmd_ANGLE_CALIBRATE,
+                                   desc=self.cmd_ANGLE_CALIBRATE_help)
+    def handle_sync_mcu_pos(self, mcu_stepper):
+        if mcu_stepper.get_name() == self.stepper_name:
+            self.mcu_pos_offset = None
+    def calc_mcu_pos_offset(self, sample):
+        # Lookup phase information
+        mcu_phase_offset, phases = self.tmc_module.get_phase_offset()
+        if mcu_phase_offset is None:
+            return
+        # Find mcu position at time of sample
+        angle_time, angle_pos = sample
+        mcu_pos = self.mcu_stepper.get_past_mcu_position(angle_time)
+        # Convert angle_pos to mcu_pos units
+        microsteps, full_steps = self.get_microsteps()
+        angle_to_mcu_pos = full_steps * microsteps / float(1<<ANGLE_BITS)
+        angle_mpos = angle_pos * angle_to_mcu_pos
+        # Calculate adjustment for stepper phases
+        phase_diff = ((angle_mpos + self.angle_phase_offset * angle_to_mcu_pos)
+                      - (mcu_pos + mcu_phase_offset)) % phases
+        if phase_diff > phases//2:
+            phase_diff -= phases
+        # Store final offset
+        self.mcu_pos_offset = mcu_pos - (angle_mpos - phase_diff)
+    def apply_calibration(self, samples):
+        calibration = self.calibration
+        if not calibration:
+            return None
+        calibration_reversed = self.calibration_reversed
+        interp_bits = ANGLE_BITS - CALIBRATION_BITS
+        interp_mask = (1 << interp_bits) - 1
+        interp_round = 1 << (interp_bits - 1)
+        for i, (samp_time, angle) in enumerate(samples):
+            bucket = (angle & 0xffff) >> interp_bits
+            cal1 = calibration[bucket]
+            cal2 = calibration[bucket + 1]
+            adj = (angle & interp_mask) * (cal2 - cal1)
+            adj = cal1 + ((adj + interp_round) >> interp_bits)
+            angle_diff = (angle - adj) & 0xffff
+            angle_diff -= (angle_diff & 0x8000) << 1
+            new_angle = angle - angle_diff
+            if calibration_reversed:
+                new_angle = -new_angle
+            samples[i] = (samp_time, new_angle)
+        if self.mcu_pos_offset is None:
+            self.calc_mcu_pos_offset(samples[0])
+            if self.mcu_pos_offset is None:
+                return None
+        return self.mcu_stepper.mcu_to_commanded_position(self.mcu_pos_offset)
+    def load_calibration(self, angles):
+        # Calculate linear intepolation calibration buckets by solving
+        # linear equations
+        angle_max = 1 << ANGLE_BITS
+        calibration_count = 1 << CALIBRATION_BITS
+        bucket_size = angle_max // calibration_count
+        full_steps = len(angles)
+        nominal_step = float(angle_max) / full_steps
+        self.angle_phase_offset = (angles.index(min(angles)) & 3) * nominal_step
+        self.calibration_reversed = angles[-2] > angles[-1]
+        if self.calibration_reversed:
+            angles = list(reversed(angles))
+        first_step = angles.index(min(angles))
+        angles = angles[first_step:] + angles[:first_step]
+        import numpy
+        eqs = numpy.zeros((full_steps, calibration_count))
+        ans = numpy.zeros((full_steps,))
+        for step, angle in enumerate(angles):
+            int_angle = int(angle + .5) % angle_max
+            bucket = int(int_angle / bucket_size)
+            bucket_start = bucket * bucket_size
+            ang_diff = angle - bucket_start
+            ang_diff_per = ang_diff / bucket_size
+            eq = eqs[step]
+            eq[bucket] = 1. - ang_diff_per
+            eq[(bucket + 1) % calibration_count] = ang_diff_per
+            ans[step] = float(step * nominal_step)
+            if bucket + 1 >= calibration_count:
+                ans[step] -= ang_diff_per * angle_max
+        sol = numpy.linalg.lstsq(eqs, ans, rcond=None)[0]
+        isol = [int(s + .5) for s in sol]
+        self.calibration = isol + [isol[0] + angle_max]
+    def lookup_tmc(self):
+        for driver in TRINAMIC_DRIVERS:
+            driver_name = "%s %s" % (driver, self.stepper_name)
+            module = self.printer.lookup_object(driver_name, None)
+            if module is not None:
+                return module
+        raise self.printer.command_error("Unable to find TMC driver for %s"
+                                         % (self.stepper_name,))
+    def connect(self):
+        self.tmc_module = self.lookup_tmc()
+        fmove = self.printer.lookup_object('force_move')
+        self.mcu_stepper = fmove.lookup_stepper(self.stepper_name)
+    def get_microsteps(self):
+        configfile = self.printer.lookup_object('configfile')
+        sconfig = configfile.get_status(None)['settings']
+        stconfig = sconfig.get(self.stepper_name, {})
+        microsteps = stconfig['microsteps']
+        full_steps = stconfig['full_steps_per_rotation']
+        return microsteps, full_steps
+    def get_stepper_phase(self):
+        mcu_phase_offset, phases = self.tmc_module.get_phase_offset()
+        if mcu_phase_offset is None:
+            raise self.printer.command_error("Driver phase not known for %s"
+                                             % (self.stepper_name,))
+        mcu_pos = self.mcu_stepper.get_mcu_position()
+        return (mcu_pos + mcu_phase_offset) % phases
+    def do_calibration_moves(self):
+        move = self.printer.lookup_object('force_move').manual_move
+        # Start data collection
+        angle_sensor = self.printer.lookup_object(self.name)
+        cconn = angle_sensor.start_internal_client()
+        # Move stepper several turns (to allow internal sensor calibration)
+        microsteps, full_steps = self.get_microsteps()
+        mcu_stepper = self.mcu_stepper
+        step_dist = mcu_stepper.get_step_dist()
+        full_step_dist = step_dist * microsteps
+        rotation_dist = full_steps * full_step_dist
+        align_dist = step_dist * self.get_stepper_phase()
+        move_time = 0.010
+        move_speed = full_step_dist / move_time
+        move(mcu_stepper, -(rotation_dist+align_dist), move_speed)
+        move(mcu_stepper, 2. * rotation_dist, move_speed)
+        move(mcu_stepper, -2. * rotation_dist, move_speed)
+        move(mcu_stepper, .5 * rotation_dist - full_step_dist, move_speed)
+        # Move to each full step position
+        toolhead = self.printer.lookup_object('toolhead')
+        times = []
+        samp_dist = full_step_dist
+        for i in range(2 * full_steps):
+            move(mcu_stepper, samp_dist, move_speed)
+            start_query_time = toolhead.get_last_move_time() + 0.050
+            end_query_time = start_query_time + 0.050
+            times.append((start_query_time, end_query_time))
+            toolhead.dwell(0.150)
+            if i == full_steps-1:
+                # Reverse direction and test each full step again
+                move(mcu_stepper, .5 * rotation_dist, move_speed)
+                move(mcu_stepper, -.5 * rotation_dist + samp_dist, move_speed)
+                samp_dist = -samp_dist
+        move(mcu_stepper, .5*rotation_dist + align_dist, move_speed)
+        toolhead.wait_moves()
+        # Finish data collection
+        cconn.finalize()
+        msgs = cconn.get_messages()
+        # Correlate query responses
+        cal = {}
+        step = 0
+        for msg in msgs:
+            for query_time, pos in msg['params']['data']:
+                # Add to step tracking
+                while step < len(times) and query_time > times[step][1]:
+                    step += 1
+                if step < len(times) and query_time >= times[step][0]:
+                    cal.setdefault(step, []).append(pos)
+        if len(cal) != len(times):
+            raise self.printer.command_error(
+                "Failed calibration - incomplete sensor data")
+        fcal = { i: cal[i] for i in range(full_steps) }
+        rcal = { full_steps-i-1: cal[i+full_steps] for i in range(full_steps) }
+        return fcal, rcal
+    def calc_angles(self, meas):
+        total_count = total_variance = 0
+        angles = {}
+        for step, data in meas.items():
+            count = len(data)
+            angle_avg = float(sum(data)) / count
+            angles[step] = angle_avg
+            total_count += count
+            total_variance += sum([(d - angle_avg)**2 for d in data])
+        return angles, math.sqrt(total_variance / total_count), total_count
+    cmd_ANGLE_CALIBRATE_help = "Calibrate angle sensor to stepper motor"
+    def cmd_ANGLE_CALIBRATE(self, gcmd):
+        # Perform calibration movement and capture
+        old_calibration = self.calibration
+        self.calibration = []
+        try:
+            fcal, rcal = self.do_calibration_moves()
+        finally:
+            self.calibration = old_calibration
+        # Calculate each step position average and variance
+        microsteps, full_steps = self.get_microsteps()
+        fangles, fstd, ftotal = self.calc_angles(fcal)
+        rangles, rstd, rtotal = self.calc_angles(rcal)
+        if (len({a: i for i, a in fangles.items()}) != len(fangles)
+            or len({a: i for i, a in rangles.items()}) != len(rangles)):
+            raise self.printer.command_error(
+                "Failed calibration - sensor not updating for each step")
+        merged = { i: fcal[i] + rcal[i] for i in range(full_steps) }
+        angles, std, total = self.calc_angles(merged)
+        gcmd.respond_info("angle: stddev=%.3f (%.3f forward / %.3f reverse)"
+                          " in %d queries" % (std, fstd, rstd, total))
+        # Order data with lowest/highest magnet position first
+        anglist = [angles[i] % 0xffff for i in range(full_steps)]
+        if angles[0] > angles[1]:
+            first_ang = max(anglist)
+        else:
+            first_ang = min(anglist)
+        first_phase = anglist.index(first_ang) & ~3
+        anglist = anglist[first_phase:] + anglist[:first_phase]
+        # Save results
+        cal_contents = []
+        for i, angle in enumerate(anglist):
+            if not i % 8:
+                cal_contents.append('\n')
+            cal_contents.append("%.1f" % (angle,))
+            cal_contents.append(',')
+        cal_contents.pop()
+        configfile = self.printer.lookup_object('configfile')
+        configfile.remove_section(self.name)
+        configfile.set(self.name, 'calibrate', ''.join(cal_contents))
+
+class HelperA1333:
+    SPI_MODE = 3
+    SPI_SPEED = 10000000
+    def __init__(self, config, spi, oid):
+        self.spi = spi
+        self.is_tcode_absolute = False
+        self.last_temperature = None
+    def get_static_delay(self):
+        return .000001
+    def start(self):
+        # Setup for angle query
+        self.spi.spi_transfer([0x32, 0x00])
+
+class HelperAS5047D:
+    SPI_MODE = 1
+    SPI_SPEED = int(1. / .000000350)
+    def __init__(self, config, spi, oid):
+        self.spi = spi
+        self.is_tcode_absolute = False
+        self.last_temperature = None
+    def get_static_delay(self):
+        return .000100
+    def start(self):
+        # Clear any errors from device
+        self.spi.spi_transfer([0xff, 0xfc]) # Read DIAAGC
+        self.spi.spi_transfer([0x40, 0x01]) # Read ERRFL
+        self.spi.spi_transfer([0xc0, 0x00]) # Read NOP
+
+class HelperTLE5012B:
+    SPI_MODE = 1
+    SPI_SPEED = 4000000
+    def __init__(self, config, spi, oid):
+        self.printer = config.get_printer()
+        self.spi = spi
+        self.oid = oid
+        self.is_tcode_absolute = True
+        self.last_temperature = None
+        self.mcu = spi.get_mcu()
+        self.mcu.register_config_callback(self._build_config)
+        self.spi_angle_transfer_cmd = None
+        self.last_chip_mcu_clock = self.last_chip_clock = 0
+        self.chip_freq = 0.
+        name = config.get_name().split()[-1]
+        gcode = self.printer.lookup_object("gcode")
+        gcode.register_mux_command("ANGLE_DEBUG_READ", "CHIP", name,
+                                   self.cmd_ANGLE_DEBUG_READ,
+                                   desc=self.cmd_ANGLE_DEBUG_READ_help)
+        gcode.register_mux_command("ANGLE_DEBUG_WRITE", "CHIP", name,
+                                   self.cmd_ANGLE_DEBUG_WRITE,
+                                   desc=self.cmd_ANGLE_DEBUG_WRITE_help)
+    def _build_config(self):
+        cmdqueue = self.spi.get_command_queue()
+        self.spi_angle_transfer_cmd = self.mcu.lookup_query_command(
+            "spi_angle_transfer oid=%c data=%*s",
+            "spi_angle_transfer_response oid=%c clock=%u response=%*s",
+            oid=self.oid, cq=cmdqueue)
+    def get_tcode_params(self):
+        return self.last_chip_mcu_clock, self.last_chip_clock, self.chip_freq
+    def _calc_crc(self, data):
+        crc = 0xff
+        for d in data:
+            crc ^= d
+            for i in range(8):
+                if crc & 0x80:
+                    crc = (crc << 1) ^ 0x1d
+                else:
+                    crc <<= 1
+        return (~crc) & 0xff
+    def _send_spi(self, msg):
+        for retry in range(5):
+            if msg[0] & 0x04:
+                params = self.spi_angle_transfer_cmd.send([self.oid, msg])
+            else:
+                params = self.spi.spi_transfer(msg)
+            resp = bytearray(params['response'])
+            crc = self._calc_crc(bytearray(msg[:2]) + resp[2:-2])
+            if crc == resp[-1]:
+                return params
+        raise self.printer.command_error("Unable to query tle5012b chip")
+    def _read_reg(self, reg):
+        cw = 0x8000 | ((reg & 0x3f) << 4) | 0x01
+        if reg >= 0x05 and reg <= 0x11:
+            cw |= 0x5000
+        msg = [cw >> 8, cw & 0xff, 0, 0, 0, 0]
+        params = self._send_spi(msg)
+        resp = bytearray(params['response'])
+        return (resp[2] << 8) | resp[3]
+    def _write_reg(self, reg, val):
+        cw = ((reg & 0x3f) << 4) | 0x01
+        if reg >= 0x05 and reg <= 0x11:
+            cw |= 0x5000
+        msg = [cw >> 8, cw & 0xff, (val >> 8) & 0xff, val & 0xff, 0, 0]
+        for retry in range(5):
+            self._send_spi(msg)
+            rval = self._read_reg(reg)
+            if rval == val:
+                return
+        raise self.printer.command_error("Unable to write to tle5012b chip")
+    def _mask_reg(self, reg, off, on):
+        rval = self._read_reg(reg)
+        self._write_reg(reg, (rval & ~off) | on)
+    def _query_clock(self):
+        # Read frame counter (and normalize to a 16bit counter)
+        msg = [0x84, 0x42, 0, 0, 0, 0, 0, 0] # Read with latch, AREV and FSYNC
+        params = self._send_spi(msg)
+        resp = bytearray(params['response'])
+        mcu_clock = self.mcu.clock32_to_clock64(params['clock'])
+        chip_clock = ((resp[2] & 0x7e) << 9) | ((resp[4] & 0x3e) << 4)
+        # Calculate temperature
+        temper = resp[5] - ((resp[4] & 0x01) << 8)
+        self.last_temperature = (temper + 152) / 2.776
+        return mcu_clock, chip_clock
+    def update_clock(self):
+        mcu_clock, chip_clock = self._query_clock()
+        mdiff = mcu_clock - self.last_chip_mcu_clock
+        chip_mclock = self.last_chip_clock + int(mdiff * self.chip_freq + .5)
+        cdiff = (chip_mclock - chip_clock) & 0xffff
+        cdiff -= (cdiff & 0x8000) << 1
+        new_chip_clock = chip_mclock - cdiff
+        self.chip_freq = float(new_chip_clock - self.last_chip_clock) / mdiff
+        self.last_chip_clock = new_chip_clock
+        self.last_chip_mcu_clock = mcu_clock
+    def start(self):
+        # Clear any errors from device
+        self._read_reg(0x00) # Read STAT
+        # Initialize chip (so different chip variants work the same way)
+        self._mask_reg(0x06, 0xc003, 0x4000) # MOD1: 42.7us, IIF disable
+        self._mask_reg(0x08, 0x0007, 0x0001) # MOD2: Predict off, autocal=1
+        self._mask_reg(0x0e, 0x0003, 0x0000) # MOD4: IIF mode
+        # Setup starting clock values
+        mcu_clock, chip_clock = self._query_clock()
+        self.last_chip_clock = chip_clock
+        self.last_chip_mcu_clock = mcu_clock
+        self.chip_freq = float(1<<5) / self.mcu.seconds_to_clock(1. / 750000.)
+        self.update_clock()
+    cmd_ANGLE_DEBUG_READ_help = "Query low-level angle sensor register"
+    def cmd_ANGLE_DEBUG_READ(self, gcmd):
+        reg = gcmd.get("REG", minval=0, maxval=0x30, parser=lambda x: int(x, 0))
+        val = self._read_reg(reg)
+        gcmd.respond_info("ANGLE REG[0x%02x] = 0x%04x" % (reg, val))
+    cmd_ANGLE_DEBUG_WRITE_help = "Set low-level angle sensor register"
+    def cmd_ANGLE_DEBUG_WRITE(self, gcmd):
+        reg = gcmd.get("REG", minval=0, maxval=0x30, parser=lambda x: int(x, 0))
+        val = gcmd.get("VAL", minval=0, maxval=0xffff,
+                       parser=lambda x: int(x, 0))
+        self._write_reg(reg, val)
+
+SAMPLE_PERIOD = 0.000400
+
+class Angle:
+    def __init__(self, config):
+        self.printer = config.get_printer()
+        self.sample_period = config.getfloat('sample_period', SAMPLE_PERIOD,
+                                             above=0.)
+        self.calibration = AngleCalibration(config)
+        # Measurement conversion
+        self.start_clock = self.time_shift = self.sample_ticks = 0
+        self.last_sequence = self.last_angle = 0
+        # Measurement storage (accessed from background thread)
+        self.lock = threading.Lock()
+        self.raw_samples = []
+        # Sensor type
+        sensors = { "a1333": HelperA1333, "as5047d": HelperAS5047D,
+                    "tle5012b": HelperTLE5012B }
+        sensor_type = config.getchoice('sensor_type', {s: s for s in sensors})
+        sensor_class = sensors[sensor_type]
+        self.spi = bus.MCU_SPI_from_config(config, sensor_class.SPI_MODE,
+                                           default_speed=sensor_class.SPI_SPEED)
+        self.mcu = mcu = self.spi.get_mcu()
+        self.oid = oid = mcu.create_oid()
+        self.sensor_helper = sensor_class(config, self.spi, oid)
+        # Setup mcu sensor_spi_angle bulk query code
+        self.query_spi_angle_cmd = self.query_spi_angle_end_cmd = None
+        mcu.add_config_cmd(
+            "config_spi_angle oid=%d spi_oid=%d spi_angle_type=%s"
+            % (oid, self.spi.get_oid(), sensor_type))
+        mcu.add_config_cmd(
+            "query_spi_angle oid=%d clock=0 rest_ticks=0 time_shift=0"
+            % (oid,), on_restart=True)
+        mcu.register_config_callback(self._build_config)
+        mcu.register_response(self._handle_spi_angle_data,
+                              "spi_angle_data", oid)
+        # API server endpoints
+        self.api_dump = motion_report.APIDumpHelper(
+            self.printer, self._api_update, self._api_startstop, 0.100)
+        self.name = config.get_name().split()[1]
+        wh = self.printer.lookup_object('webhooks')
+        wh.register_mux_endpoint("angle/dump_angle", "sensor", self.name,
+                                 self._handle_dump_angle)
+    def _build_config(self):
+        freq = self.mcu.seconds_to_clock(1.)
+        while float(TCODE_ERROR << self.time_shift) / freq < 0.002:
+            self.time_shift += 1
+        cmdqueue = self.spi.get_command_queue()
+        self.query_spi_angle_cmd = self.mcu.lookup_command(
+            "query_spi_angle oid=%c clock=%u rest_ticks=%u time_shift=%c",
+            cq=cmdqueue)
+        self.query_spi_angle_end_cmd = self.mcu.lookup_query_command(
+            "query_spi_angle oid=%c clock=%u rest_ticks=%u time_shift=%c",
+            "spi_angle_end oid=%c sequence=%hu", oid=self.oid, cq=cmdqueue)
+    def get_status(self, eventtime=None):
+        return {'temperature': self.sensor_helper.last_temperature}
+    # Measurement collection
+    def is_measuring(self):
+        return self.start_clock != 0
+    def _handle_spi_angle_data(self, params):
+        with self.lock:
+            self.raw_samples.append(params)
+    def _extract_samples(self, raw_samples):
+        # Load variables to optimize inner loop below
+        sample_ticks = self.sample_ticks
+        start_clock = self.start_clock
+        clock_to_print_time = self.mcu.clock_to_print_time
+        last_sequence = self.last_sequence
+        last_angle = self.last_angle
+        time_shift = 0
+        static_delay = 0.
+        last_chip_mcu_clock = last_chip_clock = chip_freq = inv_chip_freq = 0.
+        is_tcode_absolute = self.sensor_helper.is_tcode_absolute
+        if is_tcode_absolute:
+            tparams = self.sensor_helper.get_tcode_params()
+            last_chip_mcu_clock, last_chip_clock, chip_freq = tparams
+            inv_chip_freq = 1. / chip_freq
+        else:
+            time_shift = self.time_shift
+            static_delay = self.sensor_helper.get_static_delay()
+        # Process every message in raw_samples
+        count = error_count = 0
+        samples = [None] * (len(raw_samples) * 16)
+        for params in raw_samples:
+            seq = (last_sequence & ~0xffff) | params['sequence']
+            if seq < last_sequence:
+                seq += 0x10000
+            last_sequence = seq
+            d = bytearray(params['data'])
+            msg_mclock = start_clock + seq*16*sample_ticks
+            for i in range(len(d) // 3):
+                tcode = d[i*3]
+                if tcode == TCODE_ERROR:
+                    error_count += 1
+                    continue
+                raw_angle = d[i*3 + 1] | (d[i*3 + 2] << 8)
+                angle_diff = (last_angle - raw_angle) & 0xffff
+                angle_diff -= (angle_diff & 0x8000) << 1
+                last_angle -= angle_diff
+                mclock = msg_mclock + i*sample_ticks
+                if is_tcode_absolute:
+                    # tcode is tle5012b frame counter
+                    mdiff = mclock - last_chip_mcu_clock
+                    chip_mclock = last_chip_clock + int(mdiff * chip_freq + .5)
+                    cdiff = ((tcode << 10) - chip_mclock) & 0xffff
+                    cdiff -= (cdiff & 0x8000) << 1
+                    sclock = mclock + (cdiff - 0x800) * inv_chip_freq
+                else:
+                    # tcode is mcu clock offset shifted by time_shift
+                    sclock = mclock + (tcode<<time_shift)
+                ptime = round(clock_to_print_time(sclock) - static_delay, 6)
+                samples[count] = (ptime, last_angle)
+                count += 1
+        self.last_sequence = last_sequence
+        self.last_angle = last_angle
+        del samples[count:]
+        return samples, error_count
+    # API interface
+    def _api_update(self, eventtime):
+        if self.sensor_helper.is_tcode_absolute:
+            self.sensor_helper.update_clock()
+        with self.lock:
+            raw_samples = self.raw_samples
+            self.raw_samples = []
+        if not raw_samples:
+            return {}
+        samples, error_count = self._extract_samples(raw_samples)
+        if not samples:
+            return {}
+        offset = self.calibration.apply_calibration(samples)
+        return {'data': samples, 'errors': error_count,
+                'position_offset': offset}
+    def _start_measurements(self):
+        if self.is_measuring():
+            return
+        logging.info("Starting angle '%s' measurements", self.name)
+        self.sensor_helper.start()
+        # Start bulk reading
+        with self.lock:
+            self.raw_samples = []
+        self.last_sequence = 0
+        systime = self.printer.get_reactor().monotonic()
+        print_time = self.mcu.estimated_print_time(systime) + MIN_MSG_TIME
+        self.start_clock = reqclock = self.mcu.print_time_to_clock(print_time)
+        rest_ticks = self.mcu.seconds_to_clock(self.sample_period)
+        self.sample_ticks = rest_ticks
+        self.query_spi_angle_cmd.send([self.oid, reqclock, rest_ticks,
+                                       self.time_shift], reqclock=reqclock)
+    def _finish_measurements(self):
+        if not self.is_measuring():
+            return
+        # Halt bulk reading
+        params = self.query_spi_angle_end_cmd.send([self.oid, 0, 0, 0])
+        self.start_clock = 0
+        with self.lock:
+            self.raw_samples = []
+        self.sensor_helper.last_temperature = None
+        logging.info("Stopped angle '%s' measurements", self.name)
+    def _api_startstop(self, is_start):
+        if is_start:
+            self._start_measurements()
+        else:
+            self._finish_measurements()
+    def _handle_dump_angle(self, web_request):
+        self.api_dump.add_client(web_request)
+        hdr = ('time', 'angle')
+        web_request.send({'header': hdr})
+    def start_internal_client(self):
+        return self.api_dump.add_internal_client()
+
+def load_config_prefix(config):
+    return Angle(config)

klippy/extras/bed_screws.py

@@ -0,0 +1,109 @@
+# Helper script to adjust bed screws
+#
+# Copyright (C) 2019-2021  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+
+class BedScrews:
+    def __init__(self, config):
+        self.printer = config.get_printer()
+        self.state = None
+        self.current_screw = 0
+        self.accepted_screws = 0
+        self.number_of_screws = 0
+        # Read config
+        screws = []
+        fine_adjust = []
+        for i in range(99):
+            prefix = "screw%d" % (i + 1,)
+            if config.get(prefix, None) is None:
+                break
+            screw_coord = config.getfloatlist(prefix, count=2)
+            screw_name = "screw at %.3f,%.3f" % screw_coord
+            screw_name = config.get(prefix + "_name", screw_name)
+            screws.append((screw_coord, screw_name))
+            pfa = prefix + "_fine_adjust"
+            if config.get(pfa, None) is not None:
+                fine_coord = config.getfloatlist(pfa, count=2)
+                fine_adjust.append((fine_coord, screw_name))
+        if len(screws) < 3:
+            raise config.error("bed_screws: Must have at least three screws")
+        self.number_of_screws = len(screws)
+        self.states = {'adjust': screws, 'fine': fine_adjust}
+        self.speed = config.getfloat('speed', 50., above=0.)
+        self.lift_speed = config.getfloat('probe_speed', 5., above=0.)
+        self.horizontal_move_z = config.getfloat('horizontal_move_z', 5.)
+        self.probe_z = config.getfloat('probe_height', 0.)
+        # Register command
+        self.gcode = self.printer.lookup_object('gcode')
+        self.gcode.register_command("BED_SCREWS_ADJUST",
+                                    self.cmd_BED_SCREWS_ADJUST,
+                                    desc=self.cmd_BED_SCREWS_ADJUST_help)
+    def move(self, coord, speed):
+        self.printer.lookup_object('toolhead').manual_move(coord, speed)
+    def move_to_screw(self, state, screw):
+        # Move up, over, and then down
+        self.move((None, None, self.horizontal_move_z), self.lift_speed)
+        coord, name = self.states[state][screw]
+        self.move((coord[0], coord[1], self.horizontal_move_z), self.speed)
+        self.move((coord[0], coord[1], self.probe_z), self.lift_speed)
+        # Update state
+        self.state = state
+        self.current_screw = screw
+        # Register commands
+        self.gcode.respond_info(
+            "Adjust %s. Then run ACCEPT, ADJUSTED, or ABORT\n"
+            "Use ADJUSTED if a significant screw adjustment is made" % (name,))
+        self.gcode.register_command('ACCEPT', self.cmd_ACCEPT,
+                                    desc=self.cmd_ACCEPT_help)
+        self.gcode.register_command('ADJUSTED', self.cmd_ADJUSTED,
+                                    desc=self.cmd_ADJUSTED_help)
+        self.gcode.register_command('ABORT', self.cmd_ABORT,
+                                    desc=self.cmd_ABORT_help)
+    def unregister_commands(self):
+        self.gcode.register_command('ACCEPT', None)
+        self.gcode.register_command('ADJUSTED', None)
+        self.gcode.register_command('ABORT', None)
+    cmd_BED_SCREWS_ADJUST_help = "Tool to help adjust bed leveling screws"
+    def cmd_BED_SCREWS_ADJUST(self, gcmd):
+        if self.state is not None:
+            raise gcmd.error("Already in bed_screws helper; use ABORT to exit")
+        # reset accepted screws
+        self.accepted_screws = 0
+        self.move((None, None, self.horizontal_move_z), self.speed)
+        self.move_to_screw('adjust', 0)
+    cmd_ACCEPT_help = "Accept bed screw position"
+    def cmd_ACCEPT(self, gcmd):
+        self.unregister_commands()
+        self.accepted_screws = self.accepted_screws + 1
+        if self.current_screw + 1 < len(self.states[self.state]) \
+                and self.accepted_screws < self.number_of_screws:
+            # Continue with next screw
+            self.move_to_screw(self.state, self.current_screw + 1)
+            return
+        if self.accepted_screws < self.number_of_screws:
+            # Retry coarse adjustments
+            self.move_to_screw('adjust', 0)
+            return
+        if self.state == 'adjust' and self.states['fine']:
+            # Reset accepted screws for fine adjustment
+            self.accepted_screws = 0
+            # Perform fine screw adjustments
+            self.move_to_screw('fine', 0)
+            return
+        # Done
+        self.state = None
+        self.move((None, None, self.horizontal_move_z), self.lift_speed)
+        gcmd.respond_info("Bed screws tool completed successfully")
+    cmd_ADJUSTED_help = "Accept bed screw position after notable adjustment"
+    def cmd_ADJUSTED(self, gcmd):
+        self.unregister_commands()
+        self.accepted_screws = -1
+        self.cmd_ACCEPT(gcmd)
+    cmd_ABORT_help = "Abort bed screws tool"
+    def cmd_ABORT(self, gcmd):
+        self.unregister_commands()
+        self.state = None
+
+def load_config(config):
+    return BedScrews(config)

klippy/extras/bed_tilt.py

@@ -0,0 +1,99 @@
+# Bed tilt compensation
+#
+# Copyright (C) 2018  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import logging
+import mathutil
+from . import probe
+
+class BedTilt:
+    def __init__(self, config):
+        self.printer = config.get_printer()
+        self.printer.register_event_handler("klippy:connect",
+                                            self.handle_connect)
+        self.x_adjust = config.getfloat('x_adjust', 0.)
+        self.y_adjust = config.getfloat('y_adjust', 0.)
+        self.z_adjust = config.getfloat('z_adjust', 0.)
+        if config.get('points', None) is not None:
+            BedTiltCalibrate(config, self)
+        self.toolhead = None
+        # Register move transform with g-code class
+        gcode_move = self.printer.load_object(config, 'gcode_move')
+        gcode_move.set_move_transform(self)
+    def handle_connect(self):
+        self.toolhead = self.printer.lookup_object('toolhead')
+    def get_position(self):
+        x, y, z, e = self.toolhead.get_position()
+        return [x, y, z - x*self.x_adjust - y*self.y_adjust - self.z_adjust, e]
+    def move(self, newpos, speed):
+        x, y, z, e = newpos
+        self.toolhead.move([x, y, z + x*self.x_adjust + y*self.y_adjust
+                            + self.z_adjust, e], speed)
+    def update_adjust(self, x_adjust, y_adjust, z_adjust):
+        self.x_adjust = x_adjust
+        self.y_adjust = y_adjust
+        self.z_adjust = z_adjust
+        gcode_move = self.printer.lookup_object('gcode_move')
+        gcode_move.reset_last_position()
+        configfile = self.printer.lookup_object('configfile')
+        configfile.set('bed_tilt', 'x_adjust', "%.6f" % (x_adjust,))
+        configfile.set('bed_tilt', 'y_adjust', "%.6f" % (y_adjust,))
+        configfile.set('bed_tilt', 'z_adjust', "%.6f" % (z_adjust,))
+
+# Helper script to calibrate the bed tilt
+class BedTiltCalibrate:
+    def __init__(self, config, bedtilt):
+        self.printer = config.get_printer()
+        self.bedtilt = bedtilt
+        self.probe_helper = probe.ProbePointsHelper(config, self.probe_finalize)
+        self.probe_helper.minimum_points(3)
+        # Register BED_TILT_CALIBRATE command
+        self.gcode = self.printer.lookup_object('gcode')
+        self.gcode.register_command(
+            'BED_TILT_CALIBRATE', self.cmd_BED_TILT_CALIBRATE,
+            desc=self.cmd_BED_TILT_CALIBRATE_help)
+    cmd_BED_TILT_CALIBRATE_help = "Bed tilt calibration script"
+    def cmd_BED_TILT_CALIBRATE(self, gcmd):
+        self.probe_helper.start_probe(gcmd)
+    def probe_finalize(self, offsets, positions):
+        # Setup for coordinate descent analysis
+        z_offset = offsets[2]
+        logging.info("Calculating bed_tilt with: %s", positions)
+        params = { 'x_adjust': self.bedtilt.x_adjust,
+                   'y_adjust': self.bedtilt.y_adjust,
+                   'z_adjust': z_offset }
+        logging.info("Initial bed_tilt parameters: %s", params)
+        # Perform coordinate descent
+        def adjusted_height(pos, params):
+            x, y, z = pos
+            return (z - x*params['x_adjust'] - y*params['y_adjust']
+                    - params['z_adjust'])
+        def errorfunc(params):
+            total_error = 0.
+            for pos in positions:
+                total_error += adjusted_height(pos, params)**2
+            return total_error
+        new_params = mathutil.coordinate_descent(
+            params.keys(), params, errorfunc)
+        # Update current bed_tilt calculations
+        x_adjust = new_params['x_adjust']
+        y_adjust = new_params['y_adjust']
+        z_adjust = (new_params['z_adjust'] - z_offset
+                    - x_adjust * offsets[0] - y_adjust * offsets[1])
+        self.bedtilt.update_adjust(x_adjust, y_adjust, z_adjust)
+        # Log and report results
+        logging.info("Calculated bed_tilt parameters: %s", new_params)
+        for pos in positions:
+            logging.info("orig: %s new: %s", adjusted_height(pos, params),
+                         adjusted_height(pos, new_params))
+        msg = "x_adjust: %.6f y_adjust: %.6f z_adjust: %.6f" % (
+            x_adjust, y_adjust, z_adjust)
+        self.printer.set_rollover_info("bed_tilt", "bed_tilt: %s" % (msg,))
+        self.gcode.respond_info(
+            "%s\nThe above parameters have been applied to the current\n"
+            "session. The SAVE_CONFIG command will update the printer\n"
+            "config file and restart the printer." % (msg,))
+
+def load_config(config):
+    return BedTilt(config)

klippy/extras/bltouch.py

@@ -0,0 +1,276 @@
+# BLTouch support
+#
+# Copyright (C) 2018-2021  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import logging
+from . import probe
+
+SIGNAL_PERIOD = 0.020
+MIN_CMD_TIME = 5 * SIGNAL_PERIOD
+
+TEST_TIME = 5 * 60.
+RETRY_RESET_TIME = 1.
+ENDSTOP_REST_TIME = .001
+ENDSTOP_SAMPLE_TIME = .000015
+ENDSTOP_SAMPLE_COUNT = 4
+
+Commands = {
+    'pin_down': 0.000650, 'touch_mode': 0.001165,
+    'pin_up': 0.001475, 'self_test': 0.001780, 'reset': 0.002190,
+    'set_5V_output_mode' : 0.001988, 'set_OD_output_mode' : 0.002091,
+    'output_mode_store' : 0.001884,
+}
+
+# BLTouch "endstop" wrapper
+class BLTouchEndstopWrapper:
+    def __init__(self, config):
+        self.printer = config.get_printer()
+        self.printer.register_event_handler("klippy:connect",
+                                            self.handle_connect)
+        self.printer.register_event_handler('klippy:mcu_identify',
+                                            self.handle_mcu_identify)
+        self.position_endstop = config.getfloat('z_offset', minval=0.)
+        self.stow_on_each_sample = config.getboolean('stow_on_each_sample',
+                                                     True)
+        self.probe_touch_mode = config.getboolean('probe_with_touch_mode',
+                                                  False)
+        # Create a pwm object to handle the control pin
+        ppins = self.printer.lookup_object('pins')
+        self.mcu_pwm = ppins.setup_pin('pwm', config.get('control_pin'))
+        self.mcu_pwm.setup_max_duration(0.)
+        self.mcu_pwm.setup_cycle_time(SIGNAL_PERIOD)
+        # Command timing
+        self.next_cmd_time = self.action_end_time = 0.
+        self.finish_home_complete = self.wait_trigger_complete = None
+        # Create an "endstop" object to handle the sensor pin
+        pin = config.get('sensor_pin')
+        pin_params = ppins.lookup_pin(pin, can_invert=True, can_pullup=True)
+        mcu = pin_params['chip']
+        self.mcu_endstop = mcu.setup_pin('endstop', pin_params)
+        # output mode
+        omodes = {'5V': '5V', 'OD': 'OD', None: None}
+        self.output_mode = config.getchoice('set_output_mode', omodes, None)
+        # Setup for sensor test
+        self.next_test_time = 0.
+        self.pin_up_not_triggered = config.getboolean(
+            'pin_up_reports_not_triggered', True)
+        self.pin_up_touch_triggered = config.getboolean(
+            'pin_up_touch_mode_reports_triggered', True)
+        # Calculate pin move time
+        self.pin_move_time = config.getfloat('pin_move_time', 0.680, above=0.)
+        # Wrappers
+        self.get_mcu = self.mcu_endstop.get_mcu
+        self.add_stepper = self.mcu_endstop.add_stepper
+        self.get_steppers = self.mcu_endstop.get_steppers
+        self.home_wait = self.mcu_endstop.home_wait
+        self.query_endstop = self.mcu_endstop.query_endstop
+        # Register BLTOUCH_DEBUG command
+        self.gcode = self.printer.lookup_object('gcode')
+        self.gcode.register_command("BLTOUCH_DEBUG", self.cmd_BLTOUCH_DEBUG,
+                                    desc=self.cmd_BLTOUCH_DEBUG_help)
+        self.gcode.register_command("BLTOUCH_STORE", self.cmd_BLTOUCH_STORE,
+                                    desc=self.cmd_BLTOUCH_STORE_help)
+        # multi probes state
+        self.multi = 'OFF'
+    def handle_mcu_identify(self):
+        kin = self.printer.lookup_object('toolhead').get_kinematics()
+        for stepper in kin.get_steppers():
+            if stepper.is_active_axis('z'):
+                self.add_stepper(stepper)
+    def handle_connect(self):
+        self.sync_mcu_print_time()
+        self.next_cmd_time += 0.200
+        self.set_output_mode(self.output_mode)
+        try:
+            self.raise_probe()
+            self.verify_raise_probe()
+        except self.printer.command_error as e:
+            logging.warning("BLTouch raise probe error: %s", str(e))
+    def sync_mcu_print_time(self):
+        curtime = self.printer.get_reactor().monotonic()
+        est_time = self.mcu_pwm.get_mcu().estimated_print_time(curtime)
+        self.next_cmd_time = max(self.next_cmd_time, est_time + MIN_CMD_TIME)
+    def sync_print_time(self):
+        toolhead = self.printer.lookup_object('toolhead')
+        print_time = toolhead.get_last_move_time()
+        if self.next_cmd_time > print_time:
+            toolhead.dwell(self.next_cmd_time - print_time)
+        else:
+            self.next_cmd_time = print_time
+    def send_cmd(self, cmd, duration=MIN_CMD_TIME):
+        # Translate duration to ticks to avoid any secondary mcu clock skew
+        mcu = self.mcu_pwm.get_mcu()
+        cmd_clock = mcu.print_time_to_clock(self.next_cmd_time)
+        pulse = int((duration - MIN_CMD_TIME) / SIGNAL_PERIOD) * SIGNAL_PERIOD
+        cmd_clock += mcu.seconds_to_clock(max(MIN_CMD_TIME, pulse))
+        end_time = mcu.clock_to_print_time(cmd_clock)
+        # Schedule command followed by PWM disable
+        self.mcu_pwm.set_pwm(self.next_cmd_time, Commands[cmd] / SIGNAL_PERIOD)
+        self.mcu_pwm.set_pwm(end_time, 0.)
+        # Update time tracking
+        self.action_end_time = self.next_cmd_time + duration
+        self.next_cmd_time = max(self.action_end_time, end_time + MIN_CMD_TIME)
+    def verify_state(self, triggered):
+        # Perform endstop check to verify bltouch reports desired state
+        self.mcu_endstop.home_start(self.action_end_time, ENDSTOP_SAMPLE_TIME,
+                                    ENDSTOP_SAMPLE_COUNT, ENDSTOP_REST_TIME,
+                                    triggered=triggered)
+        trigger_time = self.mcu_endstop.home_wait(self.action_end_time + 0.100)
+        return trigger_time > 0.
+    def raise_probe(self):
+        self.sync_mcu_print_time()
+        if not self.pin_up_not_triggered:
+            self.send_cmd('reset')
+        self.send_cmd('pin_up', duration=self.pin_move_time)
+    def verify_raise_probe(self):
+        if not self.pin_up_not_triggered:
+            # No way to verify raise attempt
+            return
+        for retry in range(3):
+            success = self.verify_state(False)
+            if success:
+                # The "probe raised" test completed successfully
+                break
+            if retry >= 2:
+                raise self.printer.command_error(
+                    "BLTouch failed to raise probe")
+            msg = "Failed to verify BLTouch probe is raised; retrying."
+            self.gcode.respond_info(msg)
+            self.sync_mcu_print_time()
+            self.send_cmd('reset', duration=RETRY_RESET_TIME)
+            self.send_cmd('pin_up', duration=self.pin_move_time)
+    def lower_probe(self):
+        self.test_sensor()
+        self.sync_print_time()
+        self.send_cmd('pin_down', duration=self.pin_move_time)
+        if self.probe_touch_mode:
+            self.send_cmd('touch_mode')
+    def test_sensor(self):
+        if not self.pin_up_touch_triggered:
+            # Nothing to test
+            return
+        toolhead = self.printer.lookup_object('toolhead')
+        print_time = toolhead.get_last_move_time()
+        if print_time < self.next_test_time:
+            self.next_test_time = print_time + TEST_TIME
+            return
+        # Raise the bltouch probe and test if probe is raised
+        self.sync_print_time()
+        for retry in range(3):
+            self.send_cmd('pin_up', duration=self.pin_move_time)
+            self.send_cmd('touch_mode')
+            success = self.verify_state(True)
+            self.sync_print_time()
+            if success:
+                # The "bltouch connection" test completed successfully
+                self.next_test_time = print_time + TEST_TIME
+                return
+            msg = "BLTouch failed to verify sensor state"
+            if retry >= 2:
+                raise self.printer.command_error(msg)
+            self.gcode.respond_info(msg + '; retrying.')
+            self.send_cmd('reset', duration=RETRY_RESET_TIME)
+    def multi_probe_begin(self):
+        if self.stow_on_each_sample:
+            return
+        self.multi = 'FIRST'
+    def multi_probe_end(self):
+        if self.stow_on_each_sample:
+            return
+        self.sync_print_time()
+        self.raise_probe()
+        self.verify_raise_probe()
+        self.sync_print_time()
+        self.multi = 'OFF'
+    def probe_prepare(self, hmove):
+        if self.multi == 'OFF' or self.multi == 'FIRST':
+            self.lower_probe()
+            if self.multi == 'FIRST':
+                self.multi = 'ON'
+        self.sync_print_time()
+    def home_start(self, print_time, sample_time, sample_count, rest_time,
+                   triggered=True):
+        rest_time = min(rest_time, ENDSTOP_REST_TIME)
+        self.finish_home_complete = self.mcu_endstop.home_start(
+            print_time, sample_time, sample_count, rest_time, triggered)
+        # Schedule wait_for_trigger callback
+        r = self.printer.get_reactor()
+        self.wait_trigger_complete = r.register_callback(self.wait_for_trigger)
+        return self.finish_home_complete
+    def wait_for_trigger(self, eventtime):
+        self.finish_home_complete.wait()
+        if self.multi == 'OFF':
+            self.raise_probe()
+    def probe_finish(self, hmove):
+        self.wait_trigger_complete.wait()
+        if self.multi == 'OFF':
+            self.verify_raise_probe()
+        self.sync_print_time()
+        if hmove.check_no_movement() is not None:
+            raise self.printer.command_error("BLTouch failed to deploy")
+    def get_position_endstop(self):
+        return self.position_endstop
+    def set_output_mode(self, mode):
+        # If this is inadvertently/purposely issued for a
+        # BLTOUCH pre V3.0 and clones:
+        #   No reaction at all.
+        # BLTOUCH V3.0 and V3.1:
+        #   This will set the mode.
+        if mode is None:
+            return
+        logging.info("BLTouch set output mode: %s", mode)
+        self.sync_mcu_print_time()
+        if mode == '5V':
+            self.send_cmd('set_5V_output_mode')
+        if mode == 'OD':
+            self.send_cmd('set_OD_output_mode')
+    def store_output_mode(self, mode):
+        # If this command is inadvertently/purposely issued for a
+        # BLTOUCH pre V3.0 and clones:
+        #   No reaction at all to this sequence apart from a pin-down/pin-up
+        # BLTOUCH V3.0:
+        #   This will set the mode (twice) and sadly, a pin-up is needed at
+        #   the end, because of the pin-down
+        # BLTOUCH V3.1:
+        #   This will set the mode and store it in the eeprom.
+        #   The pin-up is not needed but does not hurt
+        logging.info("BLTouch store output mode: %s", mode)
+        self.sync_print_time()
+        self.send_cmd('pin_down')
+        if mode == '5V':
+            self.send_cmd('set_5V_output_mode')
+        else:
+            self.send_cmd('set_OD_output_mode')
+        self.send_cmd('output_mode_store')
+        if mode == '5V':
+            self.send_cmd('set_5V_output_mode')
+        else:
+            self.send_cmd('set_OD_output_mode')
+        self.send_cmd('pin_up')
+    cmd_BLTOUCH_DEBUG_help = "Send a command to the bltouch for debugging"
+    def cmd_BLTOUCH_DEBUG(self, gcmd):
+        cmd = gcmd.get('COMMAND', None)
+        if cmd is None or cmd not in Commands:
+            gcmd.respond_info("BLTouch commands: %s" % (
+                ", ".join(sorted([c for c in Commands if c is not None]))))
+            return
+        gcmd.respond_info("Sending BLTOUCH_DEBUG COMMAND=%s" % (cmd,))
+        self.sync_print_time()
+        self.send_cmd(cmd, duration=self.pin_move_time)
+        self.sync_print_time()
+    cmd_BLTOUCH_STORE_help = "Store an output mode in the BLTouch EEPROM"
+    def cmd_BLTOUCH_STORE(self, gcmd):
+        cmd = gcmd.get('MODE', None)
+        if cmd is None or cmd not in ['5V', 'OD']:
+            gcmd.respond_info("BLTouch output modes: 5V, OD")
+            return
+        gcmd.respond_info("Storing BLTouch output mode: %s" % (cmd,))
+        self.sync_print_time()
+        self.store_output_mode(cmd)
+        self.sync_print_time()
+
+def load_config(config):
+    blt = BLTouchEndstopWrapper(config)
+    config.get_printer().add_object('probe', probe.PrinterProbe(config, blt))
+    return blt

klippy/extras/bme280.py

@@ -0,0 +1,480 @@
+# Support for i2c based temperature sensors
+#
+# Copyright (C) 2020  Eric Callahan <arksine.code@gmail.com>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import logging
+from . import bus
+
+REPORT_TIME = .8
+BME280_CHIP_ADDR = 0x76
+BME280_REGS = {
+    'RESET': 0xE0, 'CTRL_HUM': 0xF2,
+    'STATUS': 0xF3, 'CTRL_MEAS': 0xF4, 'CONFIG': 0xF5,
+    'PRESSURE_MSB': 0xF7, 'PRESSURE_LSB': 0xF8, 'PRESSURE_XLSB': 0xF9,
+    'TEMP_MSB': 0xFA, 'TEMP_LSB': 0xFB, 'TEMP_XLSB': 0xFC,
+    'HUM_MSB': 0xFD, 'HUM_LSB': 0xFE, 'CAL_1': 0x88, 'CAL_2': 0xE1
+}
+
+BME680_REGS = {
+    'RESET': 0xE0, 'CTRL_HUM': 0x72, 'CTRL_GAS_1': 0x71, 'CTRL_GAS_0': 0x70,
+    'GAS_WAIT_0': 0x64, 'RES_HEAT_0': 0x5A, 'IDAC_HEAT_0': 0x50,
+    'STATUS': 0x73, 'EAS_STATUS_0': 0x1D, 'CTRL_MEAS': 0x74, 'CONFIG': 0x75,
+    'GAS_R_LSB': 0x2B, 'GAS_R_MSB': 0x2A,
+    'PRESSURE_MSB': 0x1F, 'PRESSURE_LSB': 0x20, 'PRESSURE_XLSB': 0x21,
+    'TEMP_MSB': 0x22, 'TEMP_LSB': 0x23, 'TEMP_XLSB': 0x24,
+    'HUM_MSB': 0x25, 'HUM_LSB': 0x26, 'CAL_1': 0x88, 'CAL_2': 0xE1,
+    'RES_HEAT_VAL': 0x00, 'RES_HEAT_RANGE': 0x02, 'RANGE_SWITCHING_ERROR': 0x04
+}
+
+BME680_GAS_CONSTANTS = {
+    0: (1., 8000000.),
+    1: (1., 4000000.),
+    2: (1., 2000000.),
+    3: (1., 1000000.),
+    4: (1., 499500.4995),
+    5: (0.99, 248262.1648),
+    6: (1., 125000.),
+    7: (0.992, 63004.03226),
+    8: (1., 31281.28128),
+    9: (1., 15625.),
+    10: (0.998, 7812.5),
+    11: (0.995, 3906.25),
+    12: (1., 1953.125),
+    13: (0.99, 976.5625),
+    14: (1., 488.28125),
+    15: (1., 244.140625)
+}
+
+STATUS_MEASURING = 1 << 3
+STATUS_IM_UPDATE = 1
+MODE = 1
+RUN_GAS = 1 << 4
+NB_CONV_0 = 0
+EAS_NEW_DATA = 1 << 7
+GAS_DONE = 1 << 6
+MEASURE_DONE = 1 << 5
+RESET_CHIP_VALUE = 0xB6
+
+BME_CHIPS = {
+    0x58: 'BMP280', 0x60: 'BME280', 0x61: 'BME680'
+}
+BME_CHIP_ID_REG = 0xD0
+
+
+def get_twos_complement(val, bit_size):
+    if val & (1 << (bit_size - 1)):
+        val -= (1 << bit_size)
+    return val
+
+
+def get_unsigned_short(bits):
+    return bits[1] << 8 | bits[0]
+
+
+def get_signed_short(bits):
+    val = get_unsigned_short(bits)
+    return get_twos_complement(val, 16)
+
+
+def get_signed_byte(bits):
+    return get_twos_complement(bits, 8)
+
+
+class BME280:
+    def __init__(self, config):
+        self.printer = config.get_printer()
+        self.name = config.get_name().split()[-1]
+        self.reactor = self.printer.get_reactor()
+        self.i2c = bus.MCU_I2C_from_config(
+            config, default_addr=BME280_CHIP_ADDR, default_speed=100000)
+        self.mcu = self.i2c.get_mcu()
+        self.iir_filter = config.getint('bme280_iir_filter', 1)
+        self.os_temp = config.getint('bme280_oversample_temp', 2)
+        self.os_hum = config.getint('bme280_oversample_hum', 2)
+        self.os_pres = config.getint('bme280_oversample_pressure', 2)
+        self.gas_heat_temp = config.getint('bme280_gas_target_temp', 320)
+        self.gas_heat_duration = config.getint('bme280_gas_heat_duration', 150)
+        logging.info("BMxx80: Oversampling: Temp %dx Humid %dx Pressure %dx" % (
+            pow(2, self.os_temp - 1), pow(2, self.os_hum - 1),
+            pow(2, self.os_pres - 1)))
+        logging.info("BMxx80: IIR: %dx" % (pow(2, self.iir_filter) - 1))
+
+        self.temp = self.pressure = self.humidity = self.gas = self.t_fine = 0.
+        self.min_temp = self.max_temp = self.range_switching_error = 0.
+        self.max_sample_time = None
+        self.dig = self.sample_timer = None
+        self.chip_type = 'BMP280'
+        self.chip_registers = BME280_REGS
+        self.printer.add_object("bme280 " + self.name, self)
+        if self.printer.get_start_args().get('debugoutput') is not None:
+            return
+        self.printer.register_event_handler("klippy:connect",
+                                            self.handle_connect)
+
+    def handle_connect(self):
+        self._init_bmxx80()
+        self.reactor.update_timer(self.sample_timer, self.reactor.NOW)
+
+    def setup_minmax(self, min_temp, max_temp):
+        self.min_temp = min_temp
+        self.max_temp = max_temp
+
+    def setup_callback(self, cb):
+        self._callback = cb
+
+    def get_report_time_delta(self):
+        return REPORT_TIME
+
+    def _init_bmxx80(self):
+        def read_calibration_data_bmp280(calib_data_1):
+            dig = {}
+            dig['T1'] = get_unsigned_short(calib_data_1[0:2])
+            dig['T2'] = get_signed_short(calib_data_1[2:4])
+            dig['T3'] = get_signed_short(calib_data_1[4:6])
+
+            dig['P1'] = get_unsigned_short(calib_data_1[6:8])
+            dig['P2'] = get_signed_short(calib_data_1[8:10])
+            dig['P3'] = get_signed_short(calib_data_1[10:12])
+            dig['P4'] = get_signed_short(calib_data_1[12:14])
+            dig['P5'] = get_signed_short(calib_data_1[14:16])
+            dig['P6'] = get_signed_short(calib_data_1[16:18])
+            dig['P7'] = get_signed_short(calib_data_1[18:20])
+            dig['P8'] = get_signed_short(calib_data_1[20:22])
+            dig['P9'] = get_signed_short(calib_data_1[22:24])
+            return dig
+
+        def read_calibration_data_bme280(calib_data_1, calib_data_2):
+            dig = read_calibration_data_bmp280(calib_data_1)
+            dig['H1'] = calib_data_1[25] & 0xFF
+            dig['H2'] = get_signed_short(calib_data_2[0:2])
+            dig['H3'] = calib_data_2[2] & 0xFF
+            dig['H4'] = get_twos_complement(
+                (calib_data_2[3] << 4) | (calib_data_2[4] & 0x0F), 12)
+            dig['H5'] = get_twos_complement(
+                (calib_data_2[5] << 4) | ((calib_data_2[4] & 0xF0) >> 4), 12)
+            dig['H6'] = get_twos_complement(calib_data_2[6], 8)
+            return dig
+
+        def read_calibration_data_bme680(calib_data_1, calib_data_2):
+            dig = {}
+            dig['T1'] = get_unsigned_short(calib_data_2[8:10])
+            dig['T2'] = get_signed_short(calib_data_1[2:4])
+            dig['T3'] = get_signed_byte(calib_data_1[4])
+
+            dig['P1'] = get_unsigned_short(calib_data_1[6:8])
+            dig['P2'] = get_signed_short(calib_data_1[8:10])
+            dig['P3'] = calib_data_1[10]
+            dig['P4'] = get_signed_short(calib_data_1[12:14])
+            dig['P5'] = get_signed_short(calib_data_1[14:16])
+            dig['P6'] = get_signed_byte(calib_data_1[17])
+            dig['P7'] = get_signed_byte(calib_data_1[16])
+            dig['P8'] = get_signed_short(calib_data_1[20:22])
+            dig['P9'] = get_signed_short(calib_data_1[22:24])
+            dig['P10'] = calib_data_1[24]
+
+            dig['H1'] = get_twos_complement(
+                (calib_data_2[2] << 4) | (calib_data_2[1] & 0x0F), 12)
+            dig['H2'] = get_twos_complement(
+                (calib_data_2[0] << 4) | ((calib_data_2[1] & 0xF0) >> 4), 12)
+            dig['H3'] = get_signed_byte(calib_data_2[3])
+            dig['H4'] = get_signed_byte(calib_data_2[4])
+            dig['H5'] = get_signed_byte(calib_data_2[5])
+            dig['H6'] = calib_data_2[6]
+            dig['H7'] = get_signed_byte(calib_data_2[7])
+
+            dig['G1'] = get_signed_byte(calib_data_2[12])
+            dig['G2'] = get_signed_short(calib_data_2[10:12])
+            dig['G3'] = get_signed_byte(calib_data_2[13])
+            return dig
+
+        chip_id = self.read_id()
+        if chip_id not in BME_CHIPS.keys():
+            logging.info("bme280: Unknown Chip ID received %#x" % chip_id)
+        else:
+            self.chip_type = BME_CHIPS[chip_id]
+            logging.info("bme280: Found Chip %s at %#x" % (
+                self.chip_type, self.i2c.i2c_address))
+
+        # Reset chip
+        self.write_register('RESET', [RESET_CHIP_VALUE])
+        self.reactor.pause(self.reactor.monotonic() + .5)
+
+        # Make sure non-volatile memory has been copied to registers
+        status = self.read_register('STATUS', 1)[0]
+        while status & STATUS_IM_UPDATE:
+            self.reactor.pause(self.reactor.monotonic() + .01)
+            status = self.read_register('STATUS', 1)[0]
+
+        if self.chip_type == 'BME680':
+            self.max_sample_time = 0.5
+            self.sample_timer = self.reactor.register_timer(self._sample_bme680)
+            self.chip_registers = BME680_REGS
+        else:
+            self.max_sample_time = \
+                (1.25 + (2.3 * self.os_temp) + ((2.3 * self.os_pres) + .575)
+                 + ((2.3 * self.os_hum) + .575)) / 1000
+            self.sample_timer = self.reactor.register_timer(self._sample_bme280)
+            self.chip_registers = BME280_REGS
+
+        if self.chip_type in ('BME680', 'BME280'):
+            self.write_register('CONFIG', (self.iir_filter & 0x07) << 2)
+
+        # Read out and calculate the trimming parameters
+        cal_1 = self.read_register('CAL_1', 26)
+        cal_2 = self.read_register('CAL_2', 16)
+        if self.chip_type == 'BME280':
+            self.dig = read_calibration_data_bme280(cal_1, cal_2)
+        elif self.chip_type == 'BMP280':
+            self.dig = read_calibration_data_bmp280(cal_1)
+        elif self.chip_type == 'BME680':
+            self.dig = read_calibration_data_bme680(cal_1, cal_2)
+
+    def _sample_bme280(self, eventtime):
+        # Enter forced mode
+        if self.chip_type == 'BME280':
+            self.write_register('CTRL_HUM', self.os_hum)
+        meas = self.os_temp << 5 | self.os_pres << 2 | MODE
+        self.write_register('CTRL_MEAS', meas)
+
+        try:
+            # wait until results are ready
+            status = self.read_register('STATUS', 1)[0]
+            while status & STATUS_MEASURING:
+                self.reactor.pause(
+                    self.reactor.monotonic() + self.max_sample_time)
+                status = self.read_register('STATUS', 1)[0]
+
+            if self.chip_type == 'BME280':
+                data = self.read_register('PRESSURE_MSB', 8)
+            elif self.chip_type == 'BMP280':
+                data = self.read_register('PRESSURE_MSB', 6)
+            else:
+                return self.reactor.NEVER
+        except Exception:
+            logging.exception("BME280: Error reading data")
+            self.temp = self.pressure = self.humidity = .0
+            return self.reactor.NEVER
+
+        temp_raw = (data[3] << 12) | (data[4] << 4) | (data[5] >> 4)
+        self.temp = self._compensate_temp(temp_raw)
+        pressure_raw = (data[0] << 12) | (data[1] << 4) | (data[2] >> 4)
+        self.pressure = self._compensate_pressure_bme280(pressure_raw) / 100.
+        if self.chip_type == 'BME280':
+            humid_raw = (data[6] << 8) | data[7]
+            self.humidity = self._compensate_humidity_bme280(humid_raw)
+        if self.temp < self.min_temp or self.temp > self.max_temp:
+            self.printer.invoke_shutdown(
+                "BME280 temperature %0.1f outside range of %0.1f:%.01f"
+                % (self.temp, self.min_temp, self.max_temp))
+        measured_time = self.reactor.monotonic()
+        self._callback(self.mcu.estimated_print_time(measured_time), self.temp)
+        return measured_time + REPORT_TIME
+
+    def _sample_bme680(self, eventtime):
+        self.write_register('CTRL_HUM', self.os_hum & 0x07)
+        meas = self.os_temp << 5 | self.os_pres << 2
+        self.write_register('CTRL_MEAS', [meas])
+
+        gas_wait_0 = self._calculate_gas_heater_duration(self.gas_heat_duration)
+        self.write_register('GAS_WAIT_0', [gas_wait_0])
+        res_heat_0 = self._calculate_gas_heater_resistance(self.gas_heat_temp)
+        self.write_register('RES_HEAT_0', [res_heat_0])
+        gas_config = RUN_GAS | NB_CONV_0
+        self.write_register('CTRL_GAS_1', [gas_config])
+
+        def data_ready(stat):
+            new_data = (stat & EAS_NEW_DATA)
+            gas_done = not (stat & GAS_DONE)
+            meas_done = not (stat & MEASURE_DONE)
+            return new_data and gas_done and meas_done
+
+        # Enter forced mode
+        meas = meas | MODE
+        self.write_register('CTRL_MEAS', meas)
+        try:
+            # wait until results are ready
+            status = self.read_register('EAS_STATUS_0', 1)[0]
+            while not data_ready(status):
+                self.reactor.pause(
+                    self.reactor.monotonic() + self.max_sample_time)
+                status = self.read_register('EAS_STATUS_0', 1)[0]
+
+            data = self.read_register('PRESSURE_MSB', 8)
+            gas_data = self.read_register('GAS_R_MSB', 2)
+        except Exception:
+            logging.exception("BME680: Error reading data")
+            self.temp = self.pressure = self.humidity = self.gas = .0
+            return self.reactor.NEVER
+
+        temp_raw = (data[3] << 12) | (data[4] << 4) | (data[5] >> 4)
+        if temp_raw != 0x80000:
+            self.temp = self._compensate_temp(temp_raw)
+        pressure_raw = (data[0] << 12) | (data[1] << 4) | (data[2] >> 4)
+        if pressure_raw != 0x80000:
+            self.pressure = self._compensate_pressure_bme680(
+                pressure_raw) / 100.
+        humid_raw = (data[6] << 8) | data[7]
+        self.humidity = self._compensate_humidity_bme680(humid_raw)
+
+        gas_valid = ((gas_data[1] & 0x20) == 0x20)
+        if gas_valid:
+            gas_heater_stable = ((gas_data[1] & 0x10) == 0x10)
+            if not gas_heater_stable:
+                logging.warning("BME680: Gas heater didn't reach target")
+            gas_raw = (gas_data[0] << 2) | ((gas_data[1] & 0xC0) >> 6)
+            gas_range = (gas_data[1] & 0x0F)
+            self.gas = self._compensate_gas(gas_raw, gas_range)
+
+        if self.temp < self.min_temp or self.temp > self.max_temp:
+            self.printer.invoke_shutdown(
+                "BME680 temperature %0.1f outside range of %0.1f:%.01f"
+                % (self.temp, self.min_temp, self.max_temp))
+        measured_time = self.reactor.monotonic()
+        self._callback(self.mcu.estimated_print_time(measured_time), self.temp)
+        return measured_time + REPORT_TIME * 4
+
+    def _compensate_temp(self, raw_temp):
+        dig = self.dig
+        var1 = ((raw_temp / 16384. - (dig['T1'] / 1024.)) * dig['T2'])
+        var2 = (
+                ((raw_temp / 131072.) - (dig['T1'] / 8192.)) *
+                ((raw_temp / 131072.) - (dig['T1'] / 8192.)) * dig['T3'])
+        self.t_fine = var1 + var2
+        return self.t_fine / 5120.0
+
+    def _compensate_pressure_bme280(self, raw_pressure):
+        dig = self.dig
+        t_fine = self.t_fine
+        var1 = t_fine / 2. - 64000.
+        var2 = var1 * var1 * dig['P6'] / 32768.
+        var2 = var2 + var1 * dig['P5'] * 2.
+        var2 = var2 / 4. + (dig['P4'] * 65536.)
+        var1 = (dig['P3'] * var1 * var1 / 524288. + dig['P2'] * var1) / 524288.
+        var1 = (1. + var1 / 32768.) * dig['P1']
+        if var1 == 0:
+            return 0.
+        else:
+            pressure = 1048576.0 - raw_pressure
+            pressure = ((pressure - var2 / 4096.) * 6250.) / var1
+            var1 = dig['P9'] * pressure * pressure / 2147483648.
+            var2 = pressure * dig['P8'] / 32768.
+            return pressure + (var1 + var2 + dig['P7']) / 16.
+
+    def _compensate_pressure_bme680(self, raw_pressure):
+        dig = self.dig
+        t_fine = self.t_fine
+        var1 = t_fine / 2. - 64000.
+        var2 = var1 * var1 * dig['P6'] / 131072.
+        var2 = var2 + var1 * dig['P5'] * 2.
+        var2 = var2 / 4. + (dig['P4'] * 65536.)
+        var1 = (dig['P3'] * var1 * var1 / 16384. + dig['P2'] * var1) / 524288.
+        var1 = (1. + var1 / 32768.) * dig['P1']
+        if var1 == 0:
+            return 0.
+        else:
+            pressure = 1048576.0 - raw_pressure
+            pressure = ((pressure - var2 / 4096.) * 6250.) / var1
+            var1 = dig['P9'] * pressure * pressure / 2147483648.
+            var2 = pressure * dig['P8'] / 32768.
+            var3 = (pressure / 256.) * (pressure / 256.) * (pressure / 256.) * (
+                    dig['P10'] / 131072.)
+            return pressure + (var1 + var2 + var3 + (dig['P7'] * 128.)) / 16.
+
+    def _compensate_humidity_bme280(self, raw_humidity):
+        dig = self.dig
+        t_fine = self.t_fine
+        humidity = t_fine - 76800.
+        h1 = (
+                raw_humidity - (
+                dig['H4'] * 64. + dig['H5'] / 16384. * humidity))
+        h2 = (dig['H2'] / 65536. * (1. + dig['H6'] / 67108864. * humidity *
+                                    (1. + dig['H3'] / 67108864. * humidity)))
+        humidity = h1 * h2
+        humidity = humidity * (1. - dig['H1'] * humidity / 524288.)
+        return min(100., max(0., humidity))
+
+    def _compensate_humidity_bme680(self, raw_humidity):
+        dig = self.dig
+        temp_comp = self.temp
+
+        var1 = raw_humidity - (
+                (dig['H1'] * 16.) + ((dig['H3'] / 2.) * temp_comp))
+        var2 = var1 * ((dig['H2'] / 262144.) *
+                       (1. + ((dig['H4'] / 16384.) * temp_comp) +
+                        ((dig['H5'] / 1048576.) * temp_comp * temp_comp)))
+        var3 = dig['H6'] / 16384.
+        var4 = dig['H7'] / 2097152.
+        humidity = var2 + ((var3 + (var4 * temp_comp)) * var2 * var2)
+        return min(100., max(0., humidity))
+
+    def _compensate_gas(self, gas_raw, gas_range):
+        gas_switching_error = self.read_register('RANGE_SWITCHING_ERROR', 1)[0]
+        var1 = (1340. + 5. * gas_switching_error) * \
+               BME680_GAS_CONSTANTS[gas_range][0]
+        gas = var1 * BME680_GAS_CONSTANTS[gas_range][1] / (
+                gas_raw - 512. + var1)
+        return gas
+
+    def _calculate_gas_heater_resistance(self, target_temp):
+        amb_temp = self.temp
+        heater_data = self.read_register('RES_HEAT_VAL', 3)
+        res_heat_val = get_signed_byte(heater_data[0])
+        res_heat_range = (heater_data[2] & 0x30) >> 4
+        dig = self.dig
+        var1 = (dig['G1'] / 16.) + 49.
+        var2 = ((dig['G2'] / 32768.) * 0.0005) + 0.00235
+        var3 = dig['G3'] / 1024.
+        var4 = var1 * (1. + (var2 * target_temp))
+        var5 = var4 + (var3 * amb_temp)
+        res_heat = (3.4 * ((var5 * (4. / (4. + res_heat_range))
+                            * (1. / (1. + (res_heat_val * 0.002)))) - 25))
+        return int(res_heat)
+
+    def _calculate_gas_heater_duration(self, duration_ms):
+        if duration_ms >= 4032:
+            duration_reg = 0xff
+        else:
+            factor = 0
+            while duration_ms > 0x3F:
+                duration_ms //= 4
+                factor += 1
+            duration_reg = duration_ms + (factor * 64)
+
+        return duration_reg
+
+    def read_id(self):
+        # read chip id register
+        regs = [BME_CHIP_ID_REG]
+        params = self.i2c.i2c_read(regs, 1)
+        return bytearray(params['response'])[0]
+
+    def read_register(self, reg_name, read_len):
+        # read a single register
+        regs = [self.chip_registers[reg_name]]
+        params = self.i2c.i2c_read(regs, read_len)
+        return bytearray(params['response'])
+
+    def write_register(self, reg_name, data):
+        if type(data) is not list:
+            data = [data]
+        reg = self.chip_registers[reg_name]
+        data.insert(0, reg)
+        self.i2c.i2c_write(data)
+
+    def get_status(self, eventtime):
+        data = {
+            'temperature': round(self.temp, 2),
+            'pressure': self.pressure
+        }
+        if self.chip_type in ('BME280', 'BME680'):
+            data['humidity'] = self.humidity
+        if self.chip_type == 'BME680':
+            data['gas'] = self.gas
+        return data
+
+
+def load_config(config):
+    # Register sensor
+    pheaters = config.get_printer().load_object(config, "heaters")
+    pheaters.add_sensor_factory("BME280", BME280)

klippy/extras/board_pins.py

@@ -0,0 +1,27 @@
+# Support for custom board pin aliases
+#
+# Copyright (C) 2019-2021  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+
+class PrinterBoardAliases:
+    def __init__(self, config):
+        ppins = config.get_printer().lookup_object('pins')
+        mcu_names = config.getlist('mcu', ('mcu',))
+        pin_resolvers = [ppins.get_pin_resolver(n) for n in mcu_names]
+        options = ["aliases"] + config.get_prefix_options("aliases_")
+        for opt in options:
+            aliases = config.getlists(opt, seps=('=', ','), count=2)
+            for name, value in aliases:
+                if value.startswith('<') and value.endswith('>'):
+                    for pin_resolver in pin_resolvers:
+                        pin_resolver.reserve_pin(name, value)
+                else:
+                    for pin_resolver in pin_resolvers:
+                        pin_resolver.alias_pin(name, value)
+
+def load_config(config):
+    return PrinterBoardAliases(config)
+
+def load_config_prefix(config):
+    return PrinterBoardAliases(config)

klippy/extras/bus.py

@@ -0,0 +1,252 @@
+# Helper code for SPI and I2C bus communication
+#
+# Copyright (C) 2018,2019  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import mcu
+
+def resolve_bus_name(mcu, param, bus):
+    # Find enumerations for the given bus
+    enumerations = mcu.get_enumerations()
+    enums = enumerations.get(param, enumerations.get('bus'))
+    if enums is None:
+        if bus is None:
+            return 0
+        return bus
+    # Verify bus is a valid enumeration
+    ppins = mcu.get_printer().lookup_object("pins")
+    mcu_name = mcu.get_name()
+    if bus is None:
+        rev_enums = {v: k for k, v in enums.items()}
+        if 0 not in rev_enums:
+            raise ppins.error("Must specify %s on mcu '%s'" % (param, mcu_name))
+        bus = rev_enums[0]
+    if bus not in enums:
+        raise ppins.error("Unknown %s '%s'" % (param, bus))
+    # Check for reserved bus pins
+    constants = mcu.get_constants()
+    reserve_pins = constants.get('BUS_PINS_%s' % (bus,), None)
+    pin_resolver = ppins.get_pin_resolver(mcu_name)
+    if reserve_pins is not None:
+        for pin in reserve_pins.split(','):
+            pin_resolver.reserve_pin(pin, bus)
+    return bus
+
+
+######################################################################
+# SPI
+######################################################################
+
+# Helper code for working with devices connected to an MCU via an SPI bus
+class MCU_SPI:
+    def __init__(self, mcu, bus, pin, mode, speed, sw_pins=None,
+                 cs_active_high=False):
+        self.mcu = mcu
+        self.bus = bus
+        # Config SPI object (set all CS pins high before spi_set_bus commands)
+        self.oid = mcu.create_oid()
+        if pin is None:
+            mcu.add_config_cmd("config_spi_without_cs oid=%d" % (self.oid,))
+        else:
+            mcu.add_config_cmd("config_spi oid=%d pin=%s cs_active_high=%d"
+                               % (self.oid, pin, cs_active_high))
+        # Generate SPI bus config message
+        if sw_pins is not None:
+            self.config_fmt = (
+                "spi_set_software_bus oid=%d"
+                " miso_pin=%s mosi_pin=%s sclk_pin=%s mode=%d rate=%d"
+                % (self.oid, sw_pins[0], sw_pins[1], sw_pins[2], mode, speed))
+        else:
+            self.config_fmt = (
+                "spi_set_bus oid=%d spi_bus=%%s mode=%d rate=%d"
+                % (self.oid, mode, speed))
+        self.cmd_queue = mcu.alloc_command_queue()
+        mcu.register_config_callback(self.build_config)
+        self.spi_send_cmd = self.spi_transfer_cmd = None
+    def setup_shutdown_msg(self, shutdown_seq):
+        shutdown_msg = "".join(["%02x" % (x,) for x in shutdown_seq])
+        self.mcu.add_config_cmd(
+            "config_spi_shutdown oid=%d spi_oid=%d shutdown_msg=%s"
+            % (self.mcu.create_oid(), self.oid, shutdown_msg))
+    def get_oid(self):
+        return self.oid
+    def get_mcu(self):
+        return self.mcu
+    def get_command_queue(self):
+        return self.cmd_queue
+    def build_config(self):
+        if '%' in self.config_fmt:
+            bus = resolve_bus_name(self.mcu, "spi_bus", self.bus)
+            self.config_fmt = self.config_fmt % (bus,)
+        self.mcu.add_config_cmd(self.config_fmt)
+        self.spi_send_cmd = self.mcu.lookup_command(
+            "spi_send oid=%c data=%*s", cq=self.cmd_queue)
+        self.spi_transfer_cmd = self.mcu.lookup_query_command(
+            "spi_transfer oid=%c data=%*s",
+            "spi_transfer_response oid=%c response=%*s", oid=self.oid,
+            cq=self.cmd_queue)
+    def spi_send(self, data, minclock=0, reqclock=0):
+        if self.spi_send_cmd is None:
+            # Send setup message via mcu initialization
+            data_msg = "".join(["%02x" % (x,) for x in data])
+            self.mcu.add_config_cmd("spi_send oid=%d data=%s" % (
+                self.oid, data_msg), is_init=True)
+            return
+        self.spi_send_cmd.send([self.oid, data],
+                               minclock=minclock, reqclock=reqclock)
+    def spi_transfer(self, data, minclock=0, reqclock=0):
+        return self.spi_transfer_cmd.send([self.oid, data],
+                                          minclock=minclock, reqclock=reqclock)
+    def spi_transfer_with_preface(self, preface_data, data,
+                                  minclock=0, reqclock=0):
+        return self.spi_transfer_cmd.send_with_preface(
+            self.spi_send_cmd, [self.oid, preface_data], [self.oid, data],
+            minclock=minclock, reqclock=reqclock)
+
+# Helper to setup an spi bus from settings in a config section
+def MCU_SPI_from_config(config, mode, pin_option="cs_pin",
+                        default_speed=100000, share_type=None,
+                        cs_active_high=False):
+    # Determine pin from config
+    ppins = config.get_printer().lookup_object("pins")
+    cs_pin = config.get(pin_option)
+    cs_pin_params = ppins.lookup_pin(cs_pin, share_type=share_type)
+    pin = cs_pin_params['pin']
+    if pin == 'None':
+        ppins.reset_pin_sharing(cs_pin_params)
+        pin = None
+    # Load bus parameters
+    mcu = cs_pin_params['chip']
+    speed = config.getint('spi_speed', default_speed, minval=100000)
+    if config.get('spi_software_sclk_pin', None) is not None:
+        sw_pin_names = ['spi_software_%s_pin' % (name,)
+                        for name in ['miso', 'mosi', 'sclk']]
+        sw_pin_params = [ppins.lookup_pin(config.get(name), share_type=name)
+                         for name in sw_pin_names]
+        for pin_params in sw_pin_params:
+            if pin_params['chip'] != mcu:
+                raise ppins.error("%s: spi pins must be on same mcu" % (
+                    config.get_name(),))
+        sw_pins = tuple([pin_params['pin'] for pin_params in sw_pin_params])
+        bus = None
+    else:
+        bus = config.get('spi_bus', None)
+        sw_pins = None
+    # Create MCU_SPI object
+    return MCU_SPI(mcu, bus, pin, mode, speed, sw_pins, cs_active_high)
+
+
+######################################################################
+# I2C
+######################################################################
+
+# Helper code for working with devices connected to an MCU via an I2C bus
+class MCU_I2C:
+    def __init__(self, mcu, bus, addr, speed):
+        self.mcu = mcu
+        self.bus = bus
+        self.i2c_address = addr
+        self.oid = self.mcu.create_oid()
+        self.config_fmt = "config_i2c oid=%d i2c_bus=%%s rate=%d address=%d" % (
+            self.oid, speed, addr)
+        self.cmd_queue = self.mcu.alloc_command_queue()
+        self.mcu.register_config_callback(self.build_config)
+        self.i2c_write_cmd = self.i2c_read_cmd = self.i2c_modify_bits_cmd = None
+    def get_oid(self):
+        return self.oid
+    def get_mcu(self):
+        return self.mcu
+    def get_i2c_address(self):
+        return self.i2c_address
+    def get_command_queue(self):
+        return self.cmd_queue
+    def build_config(self):
+        bus = resolve_bus_name(self.mcu, "i2c_bus", self.bus)
+        self.mcu.add_config_cmd(self.config_fmt % (bus,))
+        self.i2c_write_cmd = self.mcu.lookup_command(
+            "i2c_write oid=%c data=%*s", cq=self.cmd_queue)
+        self.i2c_read_cmd = self.mcu.lookup_query_command(
+            "i2c_read oid=%c reg=%*s read_len=%u",
+            "i2c_read_response oid=%c response=%*s", oid=self.oid,
+            cq=self.cmd_queue)
+        self.i2c_modify_bits_cmd = self.mcu.lookup_command(
+            "i2c_modify_bits oid=%c reg=%*s clear_set_bits=%*s",
+            cq=self.cmd_queue)
+    def i2c_write(self, data, minclock=0, reqclock=0):
+        if self.i2c_write_cmd is None:
+            # Send setup message via mcu initialization
+            data_msg = "".join(["%02x" % (x,) for x in data])
+            self.mcu.add_config_cmd("i2c_write oid=%d data=%s" % (
+                self.oid, data_msg), is_init=True)
+            return
+        self.i2c_write_cmd.send([self.oid, data],
+                                minclock=minclock, reqclock=reqclock)
+    def i2c_read(self, write, read_len):
+        return self.i2c_read_cmd.send([self.oid, write, read_len])
+    def i2c_modify_bits(self, reg, clear_bits, set_bits,
+                        minclock=0, reqclock=0):
+        clearset = clear_bits + set_bits
+        if self.i2c_modify_bits_cmd is None:
+            # Send setup message via mcu initialization
+            reg_msg = "".join(["%02x" % (x,) for x in reg])
+            clearset_msg = "".join(["%02x" % (x,) for x in clearset])
+            self.mcu.add_config_cmd(
+                "i2c_modify_bits oid=%d reg=%s clear_set_bits=%s" % (
+                    self.oid, reg_msg, clearset_msg), is_init=True)
+            return
+        self.i2c_modify_bits_cmd.send([self.oid, reg, clearset],
+                                      minclock=minclock, reqclock=reqclock)
+
+def MCU_I2C_from_config(config, default_addr=None, default_speed=100000):
+    # Load bus parameters
+    printer = config.get_printer()
+    i2c_mcu = mcu.get_printer_mcu(printer, config.get('i2c_mcu', 'mcu'))
+    speed = config.getint('i2c_speed', default_speed, minval=100000)
+    bus = config.get('i2c_bus', None)
+    if default_addr is None:
+        addr = config.getint('i2c_address', minval=0, maxval=127)
+    else:
+        addr = config.getint('i2c_address', default_addr, minval=0, maxval=127)
+    # Create MCU_I2C object
+    return MCU_I2C(i2c_mcu, bus, addr, speed)
+
+
+######################################################################
+# Bus synchronized digital outputs
+######################################################################
+
+# Helper code for a gpio that updates on a cmd_queue
+class MCU_bus_digital_out:
+    def __init__(self, mcu, pin_desc, cmd_queue=None, value=0):
+        self.mcu = mcu
+        self.oid = mcu.create_oid()
+        ppins = mcu.get_printer().lookup_object('pins')
+        pin_params = ppins.lookup_pin(pin_desc)
+        if pin_params['chip'] is not mcu:
+            raise ppins.error("Pin %s must be on mcu %s" % (
+                pin_desc, mcu.get_name()))
+        mcu.add_config_cmd("config_digital_out oid=%d pin=%s value=%d"
+                           " default_value=%d max_duration=%d"
+                           % (self.oid, pin_params['pin'], value, value, 0))
+        mcu.register_config_callback(self.build_config)
+        if cmd_queue is None:
+            cmd_queue = mcu.alloc_command_queue()
+        self.cmd_queue = cmd_queue
+        self.update_pin_cmd = None
+    def get_oid(self):
+        return self.oid
+    def get_mcu(self):
+        return self.mcu
+    def get_command_queue(self):
+        return self.cmd_queue
+    def build_config(self):
+        self.update_pin_cmd = self.mcu.lookup_command(
+            "update_digital_out oid=%c value=%c", cq=self.cmd_queue)
+    def update_digital_out(self, value, minclock=0, reqclock=0):
+        if self.update_pin_cmd is None:
+            # Send setup message via mcu initialization
+            self.mcu.add_config_cmd("update_digital_out oid=%c value=%c"
+                                    % (self.oid, not not value))
+            return
+        self.update_pin_cmd.send([self.oid, not not value],
+                                 minclock=minclock, reqclock=reqclock)

klippy/extras/buttons.py

@@ -0,0 +1,306 @@
+# Support for button detection and callbacks
+#
+# Copyright (C) 2018  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import logging
+
+
+######################################################################
+# Button state tracking
+######################################################################
+
+QUERY_TIME = .002
+RETRANSMIT_COUNT = 50
+
+class MCU_buttons:
+    def __init__(self, printer, mcu):
+        self.reactor = printer.get_reactor()
+        self.mcu = mcu
+        self.mcu.register_config_callback(self.build_config)
+        self.pin_list = []
+        self.callbacks = []
+        self.invert = self.last_button = 0
+        self.ack_cmd = None
+        self.ack_count = 0
+    def setup_buttons(self, pins, callback):
+        mask = 0
+        shift = len(self.pin_list)
+        for pin_params in pins:
+            if pin_params['invert']:
+                self.invert |= 1 << len(self.pin_list)
+            mask |= 1 << len(self.pin_list)
+            self.pin_list.append((pin_params['pin'], pin_params['pullup']))
+        self.callbacks.append((mask, shift, callback))
+    def build_config(self):
+        if not self.pin_list:
+            return
+        self.oid = self.mcu.create_oid()
+        self.mcu.add_config_cmd("config_buttons oid=%d button_count=%d" % (
+            self.oid, len(self.pin_list)))
+        for i, (pin, pull_up) in enumerate(self.pin_list):
+            self.mcu.add_config_cmd(
+                "buttons_add oid=%d pos=%d pin=%s pull_up=%d" % (
+                    self.oid, i, pin, pull_up), is_init=True)
+        cmd_queue = self.mcu.alloc_command_queue()
+        self.ack_cmd = self.mcu.lookup_command(
+            "buttons_ack oid=%c count=%c", cq=cmd_queue)
+        clock = self.mcu.get_query_slot(self.oid)
+        rest_ticks = self.mcu.seconds_to_clock(QUERY_TIME)
+        self.mcu.add_config_cmd(
+            "buttons_query oid=%d clock=%d"
+            " rest_ticks=%d retransmit_count=%d invert=%d" % (
+                self.oid, clock, rest_ticks, RETRANSMIT_COUNT,
+                self.invert), is_init=True)
+        self.mcu.register_response(self.handle_buttons_state,
+                                   "buttons_state", self.oid)
+    def handle_buttons_state(self, params):
+        # Expand the message ack_count from 8-bit
+        ack_count = self.ack_count
+        ack_diff = (ack_count - params['ack_count']) & 0xff
+        if ack_diff & 0x80:
+            ack_diff -= 0x100
+        msg_ack_count = ack_count - ack_diff
+        # Determine new buttons
+        buttons = bytearray(params['state'])
+        new_count = msg_ack_count + len(buttons) - self.ack_count
+        if new_count <= 0:
+            return
+        new_buttons = buttons[-new_count:]
+        # Send ack to MCU
+        self.ack_cmd.send([self.oid, new_count])
+        self.ack_count += new_count
+        # Call self.handle_button() with this event in main thread
+        for nb in new_buttons:
+            self.reactor.register_async_callback(
+                (lambda e, s=self, b=nb: s.handle_button(e, b)))
+    def handle_button(self, eventtime, button):
+        button ^= self.invert
+        changed = button ^ self.last_button
+        for mask, shift, callback in self.callbacks:
+            if changed & mask:
+                callback(eventtime, (button & mask) >> shift)
+        self.last_button = button
+
+
+######################################################################
+# ADC button tracking
+######################################################################
+
+ADC_REPORT_TIME = 0.015
+ADC_DEBOUNCE_TIME = 0.025
+ADC_SAMPLE_TIME = 0.001
+ADC_SAMPLE_COUNT = 6
+
+class MCU_ADC_buttons:
+    def __init__(self, printer, pin, pullup):
+        self.reactor = printer.get_reactor()
+        self.buttons = []
+        self.last_button = None
+        self.last_pressed = None
+        self.last_debouncetime = 0
+        self.pullup = pullup
+        self.pin = pin
+        self.min_value = 999999999999.9
+        self.max_value = 0.
+        ppins = printer.lookup_object('pins')
+        self.mcu_adc = ppins.setup_pin('adc', self.pin)
+        self.mcu_adc.setup_minmax(ADC_SAMPLE_TIME, ADC_SAMPLE_COUNT)
+        self.mcu_adc.setup_adc_callback(ADC_REPORT_TIME, self.adc_callback)
+        query_adc = printer.lookup_object('query_adc')
+        query_adc.register_adc('adc_button:' + pin.strip(), self.mcu_adc)
+
+    def setup_button(self, min_value, max_value, callback):
+        self.min_value = min(self.min_value, min_value)
+        self.max_value = max(self.max_value, max_value)
+        self.buttons.append((min_value, max_value, callback))
+
+    def adc_callback(self, read_time, read_value):
+        adc = max(.00001, min(.99999, read_value))
+        value = self.pullup * adc / (1.0 - adc)
+
+        # Determine button pressed
+        btn = None
+        if self.min_value <= value <= self.max_value:
+            for i, (min_value, max_value, cb) in enumerate(self.buttons):
+                if min_value < value < max_value:
+                    btn = i
+                    break
+
+        # If the button changed, due to noise or pressing:
+        if btn != self.last_button:
+            # reset the debouncing timer
+            self.last_debouncetime = read_time
+
+        # button debounce check & new button pressed
+        if ((read_time - self.last_debouncetime) >= ADC_DEBOUNCE_TIME
+            and self.last_button == btn and self.last_pressed != btn):
+            # release last_pressed
+            if self.last_pressed is not None:
+                self.call_button(self.last_pressed, False)
+                self.last_pressed = None
+            if btn is not None:
+                self.call_button(btn, True)
+                self.last_pressed = btn
+
+        self.last_button = btn
+
+    def call_button(self, button, state):
+        minval, maxval, callback = self.buttons[button]
+        self.reactor.register_async_callback(
+            (lambda e, cb=callback, s=state: cb(e, s)))
+
+
+######################################################################
+# Rotary Encoders
+######################################################################
+
+# Rotary encoder handler https://github.com/brianlow/Rotary
+# Copyright 2011 Ben Buxton (bb@cactii.net).
+# Licenced under the GNU GPL Version 3.
+class BaseRotaryEncoder:
+    R_START     = 0x0
+    R_DIR_CW    = 0x10
+    R_DIR_CCW   = 0x20
+    R_DIR_MSK   = 0x30
+
+    def __init__(self, cw_callback, ccw_callback):
+        self.cw_callback = cw_callback
+        self.ccw_callback = ccw_callback
+        self.encoder_state = self.R_START
+    def encoder_callback(self, eventtime, state):
+        es = self.ENCODER_STATES[self.encoder_state & 0xf][state & 0x3]
+        self.encoder_state = es
+        if es & self.R_DIR_MSK == self.R_DIR_CW:
+            self.cw_callback(eventtime)
+        elif es & self.R_DIR_MSK == self.R_DIR_CCW:
+            self.ccw_callback(eventtime)
+
+class FullStepRotaryEncoder(BaseRotaryEncoder):
+    R_CW_FINAL  = 0x1
+    R_CW_BEGIN  = 0x2
+    R_CW_NEXT   = 0x3
+    R_CCW_BEGIN = 0x4
+    R_CCW_FINAL = 0x5
+    R_CCW_NEXT  = 0x6
+
+    # Use the full-step state table (emits a code at 00 only)
+    ENCODER_STATES = (
+        # R_START
+        (BaseRotaryEncoder.R_START, R_CW_BEGIN, R_CCW_BEGIN,
+         BaseRotaryEncoder.R_START),
+
+        # R_CW_FINAL
+        (R_CW_NEXT, BaseRotaryEncoder.R_START, R_CW_FINAL,
+         BaseRotaryEncoder.R_START | BaseRotaryEncoder.R_DIR_CW),
+
+        # R_CW_BEGIN
+        (R_CW_NEXT, R_CW_BEGIN, BaseRotaryEncoder.R_START,
+         BaseRotaryEncoder.R_START),
+
+        # R_CW_NEXT
+        (R_CW_NEXT, R_CW_BEGIN, R_CW_FINAL, BaseRotaryEncoder.R_START),
+
+        # R_CCW_BEGIN
+        (R_CCW_NEXT, BaseRotaryEncoder.R_START, R_CCW_BEGIN,
+         BaseRotaryEncoder.R_START),
+
+        # R_CCW_FINAL
+        (R_CCW_NEXT, R_CCW_FINAL, BaseRotaryEncoder.R_START,
+         BaseRotaryEncoder.R_START | BaseRotaryEncoder.R_DIR_CCW),
+
+        # R_CCW_NEXT
+        (R_CCW_NEXT, R_CCW_FINAL, R_CCW_BEGIN, BaseRotaryEncoder.R_START)
+    )
+
+class HalfStepRotaryEncoder(BaseRotaryEncoder):
+    # Use the half-step state table (emits a code at 00 and 11)
+    R_CCW_BEGIN   = 0x1
+    R_CW_BEGIN    = 0x2
+    R_START_M     = 0x3
+    R_CW_BEGIN_M  = 0x4
+    R_CCW_BEGIN_M = 0x5
+
+    ENCODER_STATES = (
+        # R_START (00)
+        (R_START_M, R_CW_BEGIN, R_CCW_BEGIN, BaseRotaryEncoder.R_START),
+
+        # R_CCW_BEGIN
+        (R_START_M | BaseRotaryEncoder.R_DIR_CCW, BaseRotaryEncoder.R_START,
+         R_CCW_BEGIN, BaseRotaryEncoder.R_START),
+
+        # R_CW_BEGIN
+        (R_START_M | BaseRotaryEncoder.R_DIR_CW,  R_CW_BEGIN,
+         BaseRotaryEncoder.R_START, BaseRotaryEncoder.R_START),
+
+        # R_START_M (11)
+        (R_START_M, R_CCW_BEGIN_M, R_CW_BEGIN_M,  BaseRotaryEncoder.R_START),
+
+        # R_CW_BEGIN_M
+        (R_START_M, R_START_M, R_CW_BEGIN_M,
+         BaseRotaryEncoder.R_START | BaseRotaryEncoder.R_DIR_CW),
+
+        # R_CCW_BEGIN_M
+        (R_START_M, R_CCW_BEGIN_M, R_START_M,
+         BaseRotaryEncoder.R_START | BaseRotaryEncoder.R_DIR_CCW),
+    )
+
+
+######################################################################
+# Button registration code
+######################################################################
+
+class PrinterButtons:
+    def __init__(self, config):
+        self.printer = config.get_printer()
+        self.printer.load_object(config, 'query_adc')
+        self.mcu_buttons = {}
+        self.adc_buttons = {}
+    def register_adc_button(self, pin, min_val, max_val, pullup, callback):
+        adc_buttons = self.adc_buttons.get(pin)
+        if adc_buttons is None:
+            self.adc_buttons[pin] = adc_buttons = MCU_ADC_buttons(
+                self.printer, pin, pullup)
+        adc_buttons.setup_button(min_val, max_val, callback)
+    def register_adc_button_push(self, pin, min_val, max_val, pullup, callback):
+        def helper(eventtime, state, callback=callback):
+            if state:
+                callback(eventtime)
+        self.register_adc_button(pin, min_val, max_val, pullup, helper)
+    def register_buttons(self, pins, callback):
+        # Parse pins
+        ppins = self.printer.lookup_object('pins')
+        mcu = mcu_name = None
+        pin_params_list = []
+        for pin in pins:
+            pin_params = ppins.lookup_pin(pin, can_invert=True, can_pullup=True)
+            if mcu is not None and pin_params['chip'] != mcu:
+                raise ppins.error("button pins must be on same mcu")
+            mcu = pin_params['chip']
+            mcu_name = pin_params['chip_name']
+            pin_params_list.append(pin_params)
+        # Register pins and callback with the appropriate MCU
+        mcu_buttons = self.mcu_buttons.get(mcu_name)
+        if (mcu_buttons is None
+            or len(mcu_buttons.pin_list) + len(pin_params_list) > 8):
+            self.mcu_buttons[mcu_name] = mcu_buttons = MCU_buttons(
+                self.printer, mcu)
+        mcu_buttons.setup_buttons(pin_params_list, callback)
+    def register_rotary_encoder(self, pin1, pin2, cw_callback, ccw_callback,
+                                steps_per_detent):
+        if steps_per_detent == 2:
+            re = HalfStepRotaryEncoder(cw_callback, ccw_callback)
+        elif steps_per_detent == 4:
+            re = FullStepRotaryEncoder(cw_callback, ccw_callback)
+        else:
+            raise self.printer.config_error(
+                "%d steps per detent not supported" % steps_per_detent)
+        self.register_buttons([pin1, pin2], re.encoder_callback)
+    def register_button_push(self, pin, callback):
+        def helper(eventtime, state, callback=callback):
+            if state:
+                callback(eventtime)
+        self.register_buttons([pin], helper)
+
+def load_config(config):
+    return PrinterButtons(config)

klippy/extras/canbus_ids.py

@@ -0,0 +1,24 @@
+# Support for tracking canbus node ids
+#
+# Copyright (C) 2021  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+
+class PrinterCANBus:
+    def __init__(self, config):
+        self.printer = config.get_printer()
+        self.ids = {}
+    def add_uuid(self, config, canbus_uuid, canbus_iface):
+        if canbus_uuid in self.ids:
+            raise config.error("Duplicate canbus_uuid")
+        new_id = len(self.ids)
+        self.ids[canbus_uuid] = new_id
+        return new_id
+    def get_nodeid(self, canbus_uuid):
+        if canbus_uuid not in self.ids:
+            raise self.printer.config_error("Unknown canbus_uuid %s"
+                                            % (canbus_uuid,))
+        return self.ids[canbus_uuid]
+
+def load_config(config):
+    return PrinterCANBus(config)

klippy/extras/controller_fan.py

@@ -0,0 +1,71 @@
+# Support a fan for cooling the MCU whenever a stepper or heater is on
+#
+# Copyright (C) 2019  Nils Friedchen <nils.friedchen@googlemail.com>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+from . import fan
+
+PIN_MIN_TIME = 0.100
+
+class ControllerFan:
+    def __init__(self, config):
+        self.printer = config.get_printer()
+        self.printer.register_event_handler("klippy:ready", self.handle_ready)
+        self.printer.register_event_handler("klippy:connect",
+                                            self.handle_connect)
+        self.stepper_names = config.getlist("stepper", None)
+        self.stepper_enable = self.printer.load_object(config, 'stepper_enable')
+        self.printer.load_object(config, 'heaters')
+        self.heaters = []
+        self.fan = fan.Fan(config)
+        self.fan_speed = config.getfloat('fan_speed', default=1.,
+                                         minval=0., maxval=1.)
+        self.idle_speed = config.getfloat(
+            'idle_speed', default=self.fan_speed, minval=0., maxval=1.)
+        self.idle_timeout = config.getint("idle_timeout", default=30, minval=0)
+        self.heater_names = config.getlist("heater", ("extruder",))
+        self.last_on = self.idle_timeout
+        self.last_speed = 0.
+    def handle_connect(self):
+        # Heater lookup
+        pheaters = self.printer.lookup_object('heaters')
+        self.heaters = [pheaters.lookup_heater(n) for n in self.heater_names]
+        # Stepper lookup
+        all_steppers = self.stepper_enable.get_steppers()
+        if self.stepper_names is None:
+            self.stepper_names = all_steppers
+            return
+        if not all(x in all_steppers for x in self.stepper_names):
+            raise self.printer.config_error(
+                "One or more of these steppers are unknown: "
+                 "%s (valid steppers are: %s)"
+                % (self.stepper_names, ", ".join(all_steppers)))
+    def handle_ready(self):
+        reactor = self.printer.get_reactor()
+        reactor.register_timer(self.callback, reactor.monotonic()+PIN_MIN_TIME)
+    def get_status(self, eventtime):
+        return self.fan.get_status(eventtime)
+    def callback(self, eventtime):
+        speed = 0.
+        active = False
+        for name in self.stepper_names:
+            active |= self.stepper_enable.lookup_enable(name).is_motor_enabled()
+        for heater in self.heaters:
+            _, target_temp = heater.get_temp(eventtime)
+            if target_temp:
+                active = True
+        if active:
+            self.last_on = 0
+            speed = self.fan_speed
+        elif self.last_on < self.idle_timeout:
+            speed = self.idle_speed
+            self.last_on += 1
+        if speed != self.last_speed:
+            self.last_speed = speed
+            curtime = self.printer.get_reactor().monotonic()
+            print_time = self.fan.get_mcu().estimated_print_time(curtime)
+            self.fan.set_speed(print_time + PIN_MIN_TIME, speed)
+        return eventtime + 1.
+
+def load_config_prefix(config):
+    return ControllerFan(config)

klippy/extras/dac084S085.py

@@ -0,0 +1,25 @@
+# SPI DAC DAC084S085 implementation
+#
+# Copyright (C) 2021  Lorenzo Franco <lorenzo.franco@lorenzing.com>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+
+from . import bus
+
+class dac084S085:
+    def __init__(self, config):
+        self.spi = bus.MCU_SPI_from_config(
+            config, 1, pin_option="enable_pin", default_speed=10000000)
+        scale = config.getfloat('scale', 1., above=0.)
+        for chan, name in enumerate("ABCD"):
+            val = config.getfloat('channel_%s' % (name,), None,
+                                   minval=0., maxval=scale)
+            if val is not None:
+                self.set_register(chan, int(val * 255. / scale))
+    def set_register(self, chan, value):
+        b1 = (chan << 6) | (1 << 4) | ((value >> 4) & 0x0f)
+        b2 = (value << 4) & 0xf0
+        self.spi.spi_send([b1, b2])
+
+def load_config_prefix(config):
+    return dac084S085(config)

klippy/extras/delayed_gcode.py

@@ -0,0 +1,54 @@
+# A simple timer for executing gcode templates
+#
+# Copyright (C) 2019  Eric Callahan <arksine.code@gmail.com>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+
+import logging
+
+class DelayedGcode:
+    def __init__(self, config):
+        self.printer = config.get_printer()
+        self.reactor = self.printer.get_reactor()
+        self.name = config.get_name().split()[1]
+        self.gcode = self.printer.lookup_object('gcode')
+        gcode_macro = self.printer.load_object(config, 'gcode_macro')
+        self.timer_gcode = gcode_macro.load_template(config, 'gcode')
+        self.duration = config.getfloat('initial_duration', 0., minval=0.)
+        self.timer_handler = None
+        self.inside_timer = self.repeat = False
+        self.printer.register_event_handler("klippy:ready", self._handle_ready)
+        self.gcode.register_mux_command(
+            "UPDATE_DELAYED_GCODE", "ID", self.name,
+            self.cmd_UPDATE_DELAYED_GCODE,
+            desc=self.cmd_UPDATE_DELAYED_GCODE_help)
+    def _handle_ready(self):
+        waketime = self.reactor.NEVER
+        if self.duration:
+            waketime = self.reactor.monotonic() + self.duration
+        self.timer_handler = self.reactor.register_timer(
+            self._gcode_timer_event, waketime)
+    def _gcode_timer_event(self, eventtime):
+        self.inside_timer = True
+        try:
+            self.gcode.run_script(self.timer_gcode.render())
+        except Exception:
+            logging.exception("Script running error")
+        nextwake = self.reactor.NEVER
+        if self.repeat:
+            nextwake = eventtime + self.duration
+        self.inside_timer = self.repeat = False
+        return nextwake
+    cmd_UPDATE_DELAYED_GCODE_help = "Update the duration of a delayed_gcode"
+    def cmd_UPDATE_DELAYED_GCODE(self, gcmd):
+        self.duration = gcmd.get_float('DURATION', minval=0.)
+        if self.inside_timer:
+            self.repeat = (self.duration != 0.)
+        else:
+            waketime = self.reactor.NEVER
+            if self.duration:
+                waketime = self.reactor.monotonic() + self.duration
+            self.reactor.update_timer(self.timer_handler, waketime)
+
+def load_config_prefix(config):
+    return DelayedGcode(config)

klippy/extras/delta_calibrate.py

@@ -0,0 +1,287 @@
+# Delta calibration support
+#
+# Copyright (C) 2017-2019  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import math, logging, collections
+import mathutil
+from . import probe
+
+# A "stable position" is a 3-tuple containing the number of steps
+# taken since hitting the endstop on each delta tower.  Delta
+# calibration uses this coordinate system because it allows a position
+# to be described independent of the software parameters.
+
+# Load a stable position from a config entry
+def load_config_stable(config, option):
+    return config.getfloatlist(option, count=3)
+
+
+######################################################################
+# Delta calibration object
+######################################################################
+
+# The angles and distances of the calibration object found in
+# docs/prints/calibrate_size.stl
+MeasureAngles = [210., 270., 330., 30., 90., 150.]
+MeasureOuterRadius = 65
+MeasureRidgeRadius = 5. - .5
+
+# How much to prefer a distance measurement over a height measurement
+MEASURE_WEIGHT = 0.5
+
+# Convert distance measurements made on the calibration object to
+# 3-tuples of (actual_distance, stable_position1, stable_position2)
+def measurements_to_distances(measured_params, delta_params):
+    # Extract params
+    mp = measured_params
+    dp = delta_params
+    scale = mp['SCALE'][0]
+    cpw = mp['CENTER_PILLAR_WIDTHS']
+    center_widths = [cpw[0], cpw[2], cpw[1], cpw[0], cpw[2], cpw[1]]
+    center_dists = [od - cw
+                    for od, cw in zip(mp['CENTER_DISTS'], center_widths)]
+    outer_dists = [
+        od - opw
+        for od, opw in zip(mp['OUTER_DISTS'], mp['OUTER_PILLAR_WIDTHS']) ]
+    # Convert angles in degrees to an XY multiplier
+    obj_angles = list(map(math.radians, MeasureAngles))
+    xy_angles = list(zip(map(math.cos, obj_angles), map(math.sin, obj_angles)))
+    # Calculate stable positions for center measurements
+    inner_ridge = MeasureRidgeRadius * scale
+    inner_pos = [(ax * inner_ridge, ay * inner_ridge, 0.)
+                 for ax, ay in xy_angles]
+    outer_ridge = (MeasureOuterRadius + MeasureRidgeRadius) * scale
+    outer_pos = [(ax * outer_ridge, ay * outer_ridge, 0.)
+                 for ax, ay in xy_angles]
+    center_positions = [
+        (cd, dp.calc_stable_position(ip), dp.calc_stable_position(op))
+        for cd, ip, op in zip(center_dists, inner_pos, outer_pos)]
+    # Calculate positions of outer measurements
+    outer_center = MeasureOuterRadius * scale
+    start_pos = [(ax * outer_center, ay * outer_center) for ax, ay in xy_angles]
+    shifted_angles = xy_angles[2:] + xy_angles[:2]
+    first_pos = [(ax * inner_ridge + spx, ay * inner_ridge + spy, 0.)
+                 for (ax, ay), (spx, spy) in zip(shifted_angles, start_pos)]
+    second_pos = [(ax * outer_ridge + spx, ay * outer_ridge + spy, 0.)
+                  for (ax, ay), (spx, spy) in zip(shifted_angles, start_pos)]
+    outer_positions = [
+        (od, dp.calc_stable_position(fp), dp.calc_stable_position(sp))
+        for od, fp, sp in zip(outer_dists, first_pos, second_pos)]
+    return center_positions + outer_positions
+
+
+######################################################################
+# Delta Calibrate class
+######################################################################
+
+class DeltaCalibrate:
+    def __init__(self, config):
+        self.printer = config.get_printer()
+        self.printer.register_event_handler("klippy:connect",
+                                            self.handle_connect)
+        # Calculate default probing points
+        radius = config.getfloat('radius', above=0.)
+        points = [(0., 0.)]
+        scatter = [.95, .90, .85, .70, .75, .80]
+        for i in range(6):
+            r = math.radians(90. + 60. * i)
+            dist = radius * scatter[i]
+            points.append((math.cos(r) * dist, math.sin(r) * dist))
+        self.probe_helper = probe.ProbePointsHelper(
+            config, self.probe_finalize, default_points=points)
+        self.probe_helper.minimum_points(3)
+        # Restore probe stable positions
+        self.last_probe_positions = []
+        for i in range(999):
+            height = config.getfloat("height%d" % (i,), None)
+            if height is None:
+                break
+            height_pos = load_config_stable(config, "height%d_pos" % (i,))
+            self.last_probe_positions.append((height, height_pos))
+        # Restore manually entered heights
+        self.manual_heights = []
+        for i in range(999):
+            height = config.getfloat("manual_height%d" % (i,), None)
+            if height is None:
+                break
+            height_pos = load_config_stable(config, "manual_height%d_pos"
+                                            % (i,))
+            self.manual_heights.append((height, height_pos))
+        # Restore distance measurements
+        self.delta_analyze_entry = {'SCALE': (1.,)}
+        self.last_distances = []
+        for i in range(999):
+            dist = config.getfloat("distance%d" % (i,), None)
+            if dist is None:
+                break
+            distance_pos1 = load_config_stable(config, "distance%d_pos1" % (i,))
+            distance_pos2 = load_config_stable(config, "distance%d_pos2" % (i,))
+            self.last_distances.append((dist, distance_pos1, distance_pos2))
+        # Register gcode commands
+        self.gcode = self.printer.lookup_object('gcode')
+        self.gcode.register_command('DELTA_CALIBRATE', self.cmd_DELTA_CALIBRATE,
+                                    desc=self.cmd_DELTA_CALIBRATE_help)
+        self.gcode.register_command('DELTA_ANALYZE', self.cmd_DELTA_ANALYZE,
+                                    desc=self.cmd_DELTA_ANALYZE_help)
+    def handle_connect(self):
+        kin = self.printer.lookup_object('toolhead').get_kinematics()
+        if not hasattr(kin, "get_calibration"):
+            raise self.printer.config_error(
+                "Delta calibrate is only for delta printers")
+    def save_state(self, probe_positions, distances, delta_params):
+        # Save main delta parameters
+        configfile = self.printer.lookup_object('configfile')
+        delta_params.save_state(configfile)
+        # Save probe stable positions
+        section = 'delta_calibrate'
+        configfile.remove_section(section)
+        for i, (z_offset, spos) in enumerate(probe_positions):
+            configfile.set(section, "height%d" % (i,), z_offset)
+            configfile.set(section, "height%d_pos" % (i,),
+                           "%.3f,%.3f,%.3f" % tuple(spos))
+        # Save manually entered heights
+        for i, (z_offset, spos) in enumerate(self.manual_heights):
+            configfile.set(section, "manual_height%d" % (i,), z_offset)
+            configfile.set(section, "manual_height%d_pos" % (i,),
+                           "%.3f,%.3f,%.3f" % tuple(spos))
+        # Save distance measurements
+        for i, (dist, spos1, spos2) in enumerate(distances):
+            configfile.set(section, "distance%d" % (i,), dist)
+            configfile.set(section, "distance%d_pos1" % (i,),
+                           "%.3f,%.3f,%.3f" % tuple(spos1))
+            configfile.set(section, "distance%d_pos2" % (i,),
+                           "%.3f,%.3f,%.3f" % tuple(spos2))
+    def probe_finalize(self, offsets, positions):
+        # Convert positions into (z_offset, stable_position) pairs
+        z_offset = offsets[2]
+        kin = self.printer.lookup_object('toolhead').get_kinematics()
+        delta_params = kin.get_calibration()
+        probe_positions = [(z_offset, delta_params.calc_stable_position(p))
+                           for p in positions]
+        # Perform analysis
+        self.calculate_params(probe_positions, self.last_distances)
+    def calculate_params(self, probe_positions, distances):
+        height_positions = self.manual_heights + probe_positions
+        # Setup for coordinate descent analysis
+        kin = self.printer.lookup_object('toolhead').get_kinematics()
+        orig_delta_params = odp = kin.get_calibration()
+        adj_params, params = odp.coordinate_descent_params(distances)
+        logging.info("Calculating delta_calibrate with:\n%s\n%s\n"
+                     "Initial delta_calibrate parameters: %s",
+                     height_positions, distances, params)
+        z_weight = 1.
+        if distances:
+            z_weight = len(distances) / (MEASURE_WEIGHT * len(probe_positions))
+        # Perform coordinate descent
+        def delta_errorfunc(params):
+            try:
+                # Build new delta_params for params under test
+                delta_params = orig_delta_params.new_calibration(params)
+                getpos = delta_params.get_position_from_stable
+                # Calculate z height errors
+                total_error = 0.
+                for z_offset, stable_pos in height_positions:
+                    x, y, z = getpos(stable_pos)
+                    total_error += (z - z_offset)**2
+                total_error *= z_weight
+                # Calculate distance errors
+                for dist, stable_pos1, stable_pos2 in distances:
+                    x1, y1, z1 = getpos(stable_pos1)
+                    x2, y2, z2 = getpos(stable_pos2)
+                    d = math.sqrt((x1-x2)**2 + (y1-y2)**2 + (z1-z2)**2)
+                    total_error += (d - dist)**2
+                return total_error
+            except ValueError:
+                return 9999999999999.9
+        new_params = mathutil.background_coordinate_descent(
+            self.printer, adj_params, params, delta_errorfunc)
+        # Log and report results
+        logging.info("Calculated delta_calibrate parameters: %s", new_params)
+        new_delta_params = orig_delta_params.new_calibration(new_params)
+        for z_offset, spos in height_positions:
+            logging.info("height orig: %.6f new: %.6f goal: %.6f",
+                         orig_delta_params.get_position_from_stable(spos)[2],
+                         new_delta_params.get_position_from_stable(spos)[2],
+                         z_offset)
+        for dist, spos1, spos2 in distances:
+            x1, y1, z1 = orig_delta_params.get_position_from_stable(spos1)
+            x2, y2, z2 = orig_delta_params.get_position_from_stable(spos2)
+            orig_dist = math.sqrt((x1-x2)**2 + (y1-y2)**2 + (z1-z2)**2)
+            x1, y1, z1 = new_delta_params.get_position_from_stable(spos1)
+            x2, y2, z2 = new_delta_params.get_position_from_stable(spos2)
+            new_dist = math.sqrt((x1-x2)**2 + (y1-y2)**2 + (z1-z2)**2)
+            logging.info("distance orig: %.6f new: %.6f goal: %.6f",
+                         orig_dist, new_dist, dist)
+        # Store results for SAVE_CONFIG
+        self.save_state(probe_positions, distances, new_delta_params)
+        self.gcode.respond_info(
+            "The SAVE_CONFIG command will update the printer config file\n"
+            "with these parameters and restart the printer.")
+    cmd_DELTA_CALIBRATE_help = "Delta calibration script"
+    def cmd_DELTA_CALIBRATE(self, gcmd):
+        self.probe_helper.start_probe(gcmd)
+    def add_manual_height(self, height):
+        # Determine current location of toolhead
+        toolhead = self.printer.lookup_object('toolhead')
+        toolhead.flush_step_generation()
+        kin = toolhead.get_kinematics()
+        kin_spos = {s.get_name(): s.get_commanded_position()
+                    for s in kin.get_steppers()}
+        kin_pos = kin.calc_position(kin_spos)
+        # Convert location to a stable position
+        delta_params = kin.get_calibration()
+        stable_pos = tuple(delta_params.calc_stable_position(kin_pos))
+        # Add to list of manual heights
+        self.manual_heights.append((height, stable_pos))
+        self.gcode.respond_info(
+            "Adding manual height: %.3f,%.3f,%.3f is actually z=%.3f"
+            % (kin_pos[0], kin_pos[1], kin_pos[2], height))
+    def do_extended_calibration(self):
+        # Extract distance positions
+        if len(self.delta_analyze_entry) <= 1:
+            distances = self.last_distances
+        elif len(self.delta_analyze_entry) < 5:
+            raise self.gcode.error("Not all measurements provided")
+        else:
+            kin = self.printer.lookup_object('toolhead').get_kinematics()
+            delta_params = kin.get_calibration()
+            distances = measurements_to_distances(
+                self.delta_analyze_entry, delta_params)
+        if not self.last_probe_positions:
+            raise self.gcode.error(
+                "Must run basic calibration with DELTA_CALIBRATE first")
+        # Perform analysis
+        self.calculate_params(self.last_probe_positions, distances)
+    cmd_DELTA_ANALYZE_help = "Extended delta calibration tool"
+    def cmd_DELTA_ANALYZE(self, gcmd):
+        # Check for manual height entry
+        mheight = gcmd.get_float('MANUAL_HEIGHT', None)
+        if mheight is not None:
+            self.add_manual_height(mheight)
+            return
+        # Parse distance measurements
+        args = {'CENTER_DISTS': 6, 'CENTER_PILLAR_WIDTHS': 3,
+                'OUTER_DISTS': 6, 'OUTER_PILLAR_WIDTHS': 6, 'SCALE': 1}
+        for name, count in args.items():
+            data = gcmd.get(name, None)
+            if data is None:
+                continue
+            try:
+                parts = list(map(float, data.split(',')))
+            except:
+                raise gcmd.error("Unable to parse parameter '%s'" % (name,))
+            if len(parts) != count:
+                raise gcmd.error("Parameter '%s' must have %d values"
+                                 % (name, count))
+            self.delta_analyze_entry[name] = parts
+            logging.info("DELTA_ANALYZE %s = %s", name, parts)
+        # Perform analysis if requested
+        action = gcmd.get('CALIBRATE', None)
+        if action is not None:
+            if action != 'extended':
+                raise gcmd.error("Unknown calibrate action")
+            self.do_extended_calibration()
+
+def load_config(config):
+    return DeltaCalibrate(config)

klippy/extras/display/__init__.py

@@ -0,0 +1,21 @@
+# Package definition for the extras/display directory
+#
+# Copyright (C) 2018  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+from . import display
+
+def load_config(config):
+    return display.load_config(config)
+
+def load_config_prefix(config):
+    if not config.has_section('display'):
+        raise config.error(
+            "A primary [display] section must be defined in printer.cfg "
+            "to use auxilary displays")
+    name = config.get_name().split()[-1]
+    if name == "display":
+        raise config.error(
+            "Section name [display display] is not valid. "
+            "Please choose a different postfix.")
+    return display.load_config(config)

klippy/extras/display/display.cfg

@@ -0,0 +1,461 @@
+# This file defines the default layout of the printer's lcd display.
+
+# It is not necessary to edit this file to change the display.
+# Instead, one may override any of the sections defined here by
+# defining a section with the same name in the main printer.cfg config
+# file.
+
+
+######################################################################
+# Helper macros for showing common screen values
+######################################################################
+
+[display_template _heater_temperature]
+param_heater_name: "extruder"
+text:
+  {% if param_heater_name in printer %}
+    {% set heater = printer[param_heater_name] %}
+    # Show glyph
+    {% if param_heater_name == "heater_bed" %}
+      {% if heater.target %}
+        {% set frame = (printer.toolhead.estimated_print_time|int % 2) + 1 %}
+        ~bed_heat{frame}~
+      {% else %}
+        ~bed~
+      {% endif %}
+    {% else %}
+      ~extruder~
+    {% endif %}
+    # Show temperature
+    { "%3.0f" % (heater.temperature,) }
+    # Optionally show target
+    {% if heater.target and (heater.temperature - heater.target)|abs > 2 %}
+      ~right_arrow~
+      { "%0.0f" % (heater.target,) }
+    {% endif %}
+    ~degrees~
+  {% endif %}
+
+[display_template _fan_speed]
+text:
+  {% if 'fan' in printer %}
+    {% set speed = printer.fan.speed %}
+    {% if speed %}
+      {% set frame = (printer.toolhead.estimated_print_time|int % 2) + 1 %}
+      ~fan{frame}~
+    {% else %}
+      ~fan1~
+    {% endif %}
+    { "{:>4.0%}".format(speed) }
+  {% endif %}
+
+[display_template _printing_time]
+text:
+  {% set ptime = printer.idle_timeout.printing_time %}
+  { "%02d:%02d" % (ptime // (60 * 60), (ptime // 60) % 60) }
+
+[display_template _print_status]
+text:
+  {% if printer.display_status.message %}
+    { printer.display_status.message }
+  {% elif printer.idle_timeout.printing_time %}
+    {% set pos = printer.toolhead.position %}
+    { "X%-4.0fY%-4.0fZ%-5.2f" % (pos.x, pos.y, pos.z) }
+  {% else %}
+    Ready
+  {% endif %}
+
+
+######################################################################
+# Default 16x4 display
+######################################################################
+
+[display_data _default_16x4 extruder]
+position: 0, 0
+text:
+  {% set active_extruder = printer.toolhead.extruder %}
+  { render("_heater_temperature", param_heater_name=active_extruder) }
+
+[display_data _default_16x4 fan]
+position: 0, 10
+text: { render("_fan_speed") }
+
+[display_data _default_16x4 heater_bed]
+position: 1, 0
+text: { render("_heater_temperature", param_heater_name="heater_bed") }
+
+[display_data _default_16x4 speed_factor]
+position: 1, 10
+text:
+  ~feedrate~
+  { "{:>4.0%}".format(printer.gcode_move.speed_factor) }
+
+[display_data _default_16x4 print_progress]
+position: 2, 0
+text: { "{:^10.0%}".format(printer.display_status.progress) }
+[display_data _default_16x4 progress_bar]
+position: 2, 1 # Draw graphical progress bar after text is written
+text: { draw_progress_bar(2, 0, 10, printer.display_status.progress) }
+
+[display_data _default_16x4 printing_time]
+position: 2, 10
+text: { "%6s" % (render("_printing_time").strip(),) }
+
+[display_data _default_16x4 print_status]
+position: 3, 0
+text: { render("_print_status") }
+
+
+######################################################################
+# Alternative 16x4 layout for multi-extruders
+######################################################################
+
+[display_data _multiextruder_16x4 extruder]
+position: 0, 0
+text: { render("_heater_temperature", param_heater_name="extruder") }
+
+[display_data _multiextruder_16x4 fan]
+position: 0, 10
+text: { render("_fan_speed") }
+
+[display_data _multiextruder_16x4 extruder1]
+position: 1, 0
+text: { render("_heater_temperature", param_heater_name="extruder1") }
+
+[display_data _multiextruder_16x4 print_progress]
+position: 1, 10
+text: { "{:^6.0%}".format(printer.display_status.progress) }
+[display_data _multiextruder_16x4 progress_bar]
+position: 1, 11 # Draw graphical progress bar after text is written
+text: { draw_progress_bar(1, 10, 6, printer.display_status.progress) }
+
+[display_data _multiextruder_16x4 heater_bed]
+position: 2, 0
+text: { render("_heater_temperature", param_heater_name="heater_bed") }
+
+[display_data _multiextruder_16x4 printing_time]
+position: 2, 10
+text: { "%6s" % (render("_printing_time").strip(),) }
+
+[display_data _multiextruder_16x4 print_status]
+position: 3, 0
+text: { render("_print_status") }
+
+
+######################################################################
+# Default 20x4 display
+######################################################################
+
+[display_data _default_20x4 extruder]
+position: 0, 0
+text: { render("_heater_temperature", param_heater_name="extruder") }
+
+[display_data _default_20x4 heater_bed]
+position: 0, 10
+text: { render("_heater_temperature", param_heater_name="heater_bed") }
+
+[display_data _default_20x4 extruder1]
+position: 1, 0
+text: { render("_heater_temperature", param_heater_name="extruder1") }
+
+[display_data _default_20x4 fan]
+position: 1, 10
+text:
+  {% if 'fan' in printer %}
+    { "Fan {:^4.0%}".format(printer.fan.speed) }
+  {% endif %}
+
+[display_data _default_20x4 speed_factor]
+position: 2, 0
+text:
+  ~feedrate~
+  { "{:^4.0%}".format(printer.gcode_move.speed_factor) }
+
+[display_data _default_20x4 print_progress]
+position: 2, 8
+text:
+  {% if 'virtual_sdcard' in printer and printer.virtual_sdcard.progress %}
+    ~sd~
+  {% else %}
+    ~usb~
+  {% endif %}
+  { "{:^4.0%}".format(printer.display_status.progress) }
+
+[display_data _default_20x4 printing_time]
+position: 2, 14
+text:
+  ~clock~
+  { render("_printing_time") }
+
+[display_data _default_20x4 print_status]
+position: 3, 0
+text: { render("_print_status") }
+
+
+######################################################################
+# Default 16x4 glyphs
+######################################################################
+
+[display_glyph extruder]
+data:
+  ................
+  ................
+  ..************..
+  .....******.....
+  ..************..
+  .....******.....
+  ..************..
+  ................
+  ....********....
+  ....******.*....
+  ....********....
+  ................
+  ......****......
+  .......**.......
+  ................
+  ................
+
+[display_glyph bed]
+data:
+  ................
+  ................
+  ................
+  ................
+  ................
+  ................
+  ................
+  ................
+  ................
+  ................
+  ................
+  ...*********....
+  ..*.........*...
+  .*************..
+  ................
+  ................
+
+[display_glyph bed_heat1]
+data:
+  ................
+  ................
+  ..*....*....*...
+  .*....*....*....
+  ..*....*....*...
+  ...*....*....*..
+  ..*....*....*...
+  .*....*....*....
+  ..*....*....*...
+  ................
+  ................
+  ...*********....
+  ..*.........*...
+  .*************..
+  ................
+  ................
+
+[display_glyph bed_heat2]
+data:
+  ................
+  ................
+  ..*....*....*...
+  ...*....*....*..
+  ..*....*....*...
+  .*....*....*....
+  ..*....*....*...
+  ...*....*....*..
+  ..*....*....*...
+  ................
+  ................
+  ...*********....
+  ..*.........*...
+  .*************..
+  ................
+  ................
+
+[display_glyph fan1]
+data:
+  ................
+  ................
+  ....***.........
+  ...****....**...
+  ...****...****..
+  ....***..*****..
+  .....*....****..
+  .......**.......
+  .......**.......
+  ..****....*.....
+  ..*****..***....
+  ..****...****...
+  ...**....****...
+  .........***....
+  ................
+  ................
+
+[display_glyph fan2]
+data:
+  ................
+  ................
+  .......****.....
+  .......****.....
+  .......***......
+  ..**...**.......
+  ..***...........
+  ..****.**.****..
+  ..****.**.****..
+  ...........***..
+  .......**...**..
+  ......***.......
+  .....****.......
+  .....****.......
+  ................
+  ................
+
+[display_glyph feedrate]
+data:
+  ................
+  ................
+  ***.***.***.**..
+  *...*...*...*.*.
+  **..**..**..*.*.
+  *...*...*...*.*.
+  *...***.***.**..
+  ................
+  **...*..***.***.
+  *.*.*.*..*..*...
+  **..***..*..**..
+  *.*.*.*..*..*...
+  *.*.*.*..*..***.
+  ................
+  ................
+  ................
+
+# In addition to the above glyphs, 16x4 displays also have the
+# following hard-coded single character glyphs: right_arrow, degrees.
+
+
+######################################################################
+# Default 20x4 glyphs
+######################################################################
+
+[display_glyph extruder]
+hd44780_slot: 0
+hd44780_data:
+  ..*..
+  .*.*.
+  .*.*.
+  .*.*.
+  .*.*.
+  *...*
+  *...*
+  .***.
+
+[display_glyph bed]
+hd44780_slot: 1
+hd44780_data:
+  .....
+  *****
+  *.*.*
+  *...*
+  *.*.*
+  *****
+  .....
+  .....
+
+[display_glyph bed_heat1]
+hd44780_slot: 1
+hd44780_data:
+  .*..*
+  *..*.
+  .*..*
+  *..*.
+  .....
+  *****
+  .....
+  .....
+
+[display_glyph bed_heat2]
+hd44780_slot: 1
+hd44780_data:
+  *..*.
+  .*..*
+  *..*.
+  .*..*
+  .....
+  *****
+  .....
+  .....
+
+[display_glyph fan]
+hd44780_slot: 2
+hd44780_data:
+  .....
+  *..**
+  **.*.
+  ..*..
+  .*.**
+  **..*
+  .....
+  .....
+
+[display_glyph feedrate]
+hd44780_slot: 3
+hd44780_data:
+  ***..
+  *....
+  **...
+  *.***
+  ..*.*
+  ..**.
+  ..*.*
+  .....
+
+[display_glyph clock]
+hd44780_slot: 4
+hd44780_data:
+  .....
+  .***.
+  *..**
+  *.*.*
+  *...*
+  .***.
+  .....
+  .....
+
+[display_glyph degrees]
+hd44780_slot: 5
+hd44780_data:
+  .**..
+  *..*.
+  *..*.
+  .**..
+  .....
+  .....
+  .....
+  .....
+
+[display_glyph usb]
+hd44780_slot: 6
+hd44780_data:
+  .***.
+  .***.
+  .***.
+  *****
+  *****
+  *****
+  ..*..
+  ..*..
+
+[display_glyph sd]
+hd44780_slot: 6
+hd44780_data:
+  .....
+  ..***
+  .****
+  *****
+  *****
+  *****
+  *****
+  .....
+
+# In addition to the above glyphs, 20x4 displays also have the
+# following hard-coded glyphs: right_arrow.

klippy/extras/display/display.py

@@ -0,0 +1,271 @@
+# Basic LCD display support
+#
+# Copyright (C) 2018-2022  Kevin O'Connor <kevin@koconnor.net>
+# Copyright (C) 2018  Aleph Objects, Inc <marcio@alephobjects.com>
+# Copyright (C) 2018  Eric Callahan <arksine.code@gmail.com>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import logging, os, ast
+from . import hd44780, hd44780_spi, st7920, uc1701, menu
+
+# Normal time between each screen redraw
+REDRAW_TIME = 0.500
+# Minimum time between screen redraws
+REDRAW_MIN_TIME = 0.100
+
+LCD_chips = {
+    'st7920': st7920.ST7920, 'emulated_st7920': st7920.EmulatedST7920,
+    'hd44780': hd44780.HD44780, 'uc1701': uc1701.UC1701,
+    'ssd1306': uc1701.SSD1306, 'sh1106': uc1701.SH1106,
+    'hd44780_spi': hd44780_spi.hd44780_spi
+}
+
+# Storage of [display_template my_template] config sections
+class DisplayTemplate:
+    def __init__(self, config):
+        self.printer = config.get_printer()
+        name_parts = config.get_name().split()
+        if len(name_parts) != 2:
+            raise config.error("Section name '%s' is not valid"
+                               % (config.get_name(),))
+        self.name = name_parts[1]
+        self.params = {}
+        for option in config.get_prefix_options('param_'):
+            try:
+                self.params[option] = ast.literal_eval(config.get(option))
+            except ValueError as e:
+                raise config.error(
+                    "Option '%s' in section '%s' is not a valid literal" % (
+                        option, config.get_name()))
+        gcode_macro = self.printer.load_object(config, 'gcode_macro')
+        self.template = gcode_macro.load_template(config, 'text')
+    def get_params(self):
+        return self.params
+    def render(self, context, **kwargs):
+        params = dict(self.params)
+        params.update(**kwargs)
+        if len(params) != len(self.params):
+            raise self.printer.command_error(
+                "Invalid parameter to display_template %s" % (self.name,))
+        context = dict(context)
+        context.update(params)
+        return self.template.render(context)
+
+# Store [display_data my_group my_item] sections (one instance per group name)
+class DisplayGroup:
+    def __init__(self, config, name, data_configs):
+        # Load and parse the position of display_data items
+        items = []
+        for c in data_configs:
+            pos = c.get('position')
+            try:
+                row, col = [int(v.strip()) for v in pos.split(',')]
+            except:
+                raise config.error("Unable to parse 'position' in section '%s'"
+                                   % (c.get_name(),))
+            items.append((row, col, c.get_name()))
+        # Load all templates and store sorted by display position
+        configs_by_name = {c.get_name(): c for c in data_configs}
+        printer = config.get_printer()
+        gcode_macro = printer.load_object(config, 'gcode_macro')
+        self.data_items = []
+        for row, col, name in sorted(items):
+            c = configs_by_name[name]
+            if c.get('text'):
+                template = gcode_macro.load_template(c, 'text')
+                self.data_items.append((row, col, template))
+    def show(self, display, templates, eventtime):
+        context = self.data_items[0][2].create_template_context(eventtime)
+        context['draw_progress_bar'] = display.draw_progress_bar
+        def render(name, **kwargs):
+            return templates[name].render(context, **kwargs)
+        context['render'] = render
+        for row, col, template in self.data_items:
+            text = template.render(context)
+            display.draw_text(row, col, text.replace('\n', ''), eventtime)
+        context.clear() # Remove circular references for better gc
+
+# Global cache of DisplayTemplate, DisplayGroup, and glyphs
+class PrinterDisplayTemplate:
+    def __init__(self, config):
+        self.printer = config.get_printer()
+        self.display_templates = {}
+        self.display_data_groups = {}
+        self.display_glyphs = {}
+        self.load_config(config)
+    def get_display_templates(self):
+        return self.display_templates
+    def get_display_data_groups(self):
+        return self.display_data_groups
+    def get_display_glyphs(self):
+        return self.display_glyphs
+    def _parse_glyph(self, config, glyph_name, data, width, height):
+        glyph_data = []
+        for line in data.split('\n'):
+            line = line.strip().replace('.', '0').replace('*', '1')
+            if not line:
+                continue
+            if len(line) != width or line.replace('0', '').replace('1', ''):
+                raise config.error("Invalid glyph line in %s" % (glyph_name,))
+            glyph_data.append(int(line, 2))
+        if len(glyph_data) != height:
+            raise config.error("Glyph %s incorrect lines" % (glyph_name,))
+        return glyph_data
+    def load_config(self, config):
+        # Load default display config file
+        pconfig = self.printer.lookup_object('configfile')
+        filename = os.path.join(os.path.dirname(__file__), 'display.cfg')
+        try:
+            dconfig = pconfig.read_config(filename)
+        except Exception:
+            raise self.printer.config_error("Cannot load config '%s'"
+                                            % (filename,))
+        # Load display_template sections
+        dt_main = config.get_prefix_sections('display_template ')
+        dt_main_names = { c.get_name(): 1 for c in dt_main }
+        dt_def = [c for c in dconfig.get_prefix_sections('display_template ')
+                  if c.get_name() not in dt_main_names]
+        for c in dt_main + dt_def:
+            dt = DisplayTemplate(c)
+            self.display_templates[dt.name] = dt
+        # Load display_data sections
+        dd_main = config.get_prefix_sections('display_data ')
+        dd_main_names = { c.get_name(): 1 for c in dd_main }
+        dd_def = [c for c in dconfig.get_prefix_sections('display_data ')
+                  if c.get_name() not in dd_main_names]
+        groups = {}
+        for c in dd_main + dd_def:
+            name_parts = c.get_name().split()
+            if len(name_parts) != 3:
+                raise config.error("Section name '%s' is not valid"
+                                   % (c.get_name(),))
+            groups.setdefault(name_parts[1], []).append(c)
+        for group_name, data_configs in groups.items():
+            dg = DisplayGroup(config, group_name, data_configs)
+            self.display_data_groups[group_name] = dg
+        # Load display glyphs
+        dg_prefix = 'display_glyph '
+        self.display_glyphs = icons = {}
+        dg_main = config.get_prefix_sections(dg_prefix)
+        dg_main_names = {c.get_name(): 1 for c in dg_main}
+        dg_def = [c for c in dconfig.get_prefix_sections(dg_prefix)
+                  if c.get_name() not in dg_main_names]
+        for dg in dg_main + dg_def:
+            glyph_name = dg.get_name()[len(dg_prefix):]
+            data = dg.get('data', None)
+            if data is not None:
+                idata = self._parse_glyph(config, glyph_name, data, 16, 16)
+                icon1 = [(bits >> 8) & 0xff for bits in idata]
+                icon2 = [bits & 0xff for bits in idata]
+                icons.setdefault(glyph_name, {})['icon16x16'] = (icon1, icon2)
+            data = dg.get('hd44780_data', None)
+            if data is not None:
+                slot = dg.getint('hd44780_slot', minval=0, maxval=7)
+                idata = self._parse_glyph(config, glyph_name, data, 5, 8)
+                icons.setdefault(glyph_name, {})['icon5x8'] = (slot, idata)
+
+def lookup_display_templates(config):
+    printer = config.get_printer()
+    dt = printer.lookup_object("display_template", None)
+    if dt is None:
+        dt = PrinterDisplayTemplate(config)
+        printer.add_object("display_template", dt)
+    return dt
+
+class PrinterLCD:
+    def __init__(self, config):
+        self.printer = config.get_printer()
+        self.reactor = self.printer.get_reactor()
+        # Load low-level lcd handler
+        self.lcd_chip = config.getchoice('lcd_type', LCD_chips)(config)
+        # Load menu and display_status
+        self.menu = None
+        name = config.get_name()
+        if name == 'display':
+            # only load menu for primary display
+            self.menu = menu.MenuManager(config, self)
+        self.printer.load_object(config, "display_status")
+        # Configurable display
+        templates = lookup_display_templates(config)
+        self.display_templates = templates.get_display_templates()
+        self.display_data_groups = templates.get_display_data_groups()
+        self.lcd_chip.set_glyphs(templates.get_display_glyphs())
+        dgroup = "_default_16x4"
+        if self.lcd_chip.get_dimensions()[0] == 20:
+            dgroup = "_default_20x4"
+        dgroup = config.get('display_group', dgroup)
+        self.show_data_group = self.display_data_groups.get(dgroup)
+        if self.show_data_group is None:
+            raise config.error("Unknown display_data group '%s'" % (dgroup,))
+        # Screen updating
+        self.printer.register_event_handler("klippy:ready", self.handle_ready)
+        self.screen_update_timer = self.reactor.register_timer(
+            self.screen_update_event)
+        self.redraw_request_pending = False
+        self.redraw_time = 0.
+        # Register g-code commands
+        gcode = self.printer.lookup_object("gcode")
+        gcode.register_mux_command('SET_DISPLAY_GROUP', 'DISPLAY', name,
+                                   self.cmd_SET_DISPLAY_GROUP,
+                                   desc=self.cmd_SET_DISPLAY_GROUP_help)
+        if name == 'display':
+            gcode.register_mux_command('SET_DISPLAY_GROUP', 'DISPLAY', None,
+                                       self.cmd_SET_DISPLAY_GROUP)
+    def get_dimensions(self):
+        return self.lcd_chip.get_dimensions()
+    def handle_ready(self):
+        self.lcd_chip.init()
+        # Start screen update timer
+        self.reactor.update_timer(self.screen_update_timer, self.reactor.NOW)
+    # Screen updating
+    def screen_update_event(self, eventtime):
+        if self.redraw_request_pending:
+            self.redraw_request_pending = False
+            self.redraw_time = eventtime + REDRAW_MIN_TIME
+        self.lcd_chip.clear()
+        # update menu component
+        if self.menu is not None:
+            ret = self.menu.screen_update_event(eventtime)
+            if ret:
+                self.lcd_chip.flush()
+                return eventtime + REDRAW_TIME
+        # Update normal display
+        try:
+            self.show_data_group.show(self, self.display_templates, eventtime)
+        except:
+            logging.exception("Error during display screen update")
+        self.lcd_chip.flush()
+        return eventtime + REDRAW_TIME
+    def request_redraw(self):
+        if self.redraw_request_pending:
+            return
+        self.redraw_request_pending = True
+        self.reactor.update_timer(self.screen_update_timer, self.redraw_time)
+    def draw_text(self, row, col, mixed_text, eventtime):
+        pos = col
+        for i, text in enumerate(mixed_text.split('~')):
+            if i & 1 == 0:
+                # write text
+                self.lcd_chip.write_text(pos, row, text.encode())
+                pos += len(text)
+            else:
+                # write glyph
+                pos += self.lcd_chip.write_glyph(pos, row, text)
+        return pos
+    def draw_progress_bar(self, row, col, width, value):
+        pixels = -1 << int(width * 8 * (1. - value) + .5)
+        pixels |= (1 << (width * 8 - 1)) | 1
+        for i in range(width):
+            data = [0xff] + [(pixels >> (i * 8)) & 0xff] * 14 + [0xff]
+            self.lcd_chip.write_graphics(col + width - 1 - i, row, data)
+        return ""
+    cmd_SET_DISPLAY_GROUP_help = "Set the active display group"
+    def cmd_SET_DISPLAY_GROUP(self, gcmd):
+        group = gcmd.get('GROUP')
+        new_dg = self.display_data_groups.get(group)
+        if new_dg is None:
+            raise gcmd.error("Unknown display_data group '%s'" % (group,))
+        self.show_data_group = new_dg
+
+def load_config(config):
+    return PrinterLCD(config)

klippy/extras/display/font8x14.py

@@ -0,0 +1,276 @@
+# Fonts for connected displays
+#
+# Copyright (C) 2018  Kevin O'Connor <kevin@koconnor.net>
+# Copyright (C) 2018  Eric Callahan  <arksine.code@gmail.com>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+
+
+######################################################################
+# Font - VGA 8x14, Row Major, MSB, 2 bytes padding
+#
+# Font comes from fntcol16.zip package found at:
+# ftp://ftp.simtel.net/pub/simtelnet/msdos/screen/fntcol16.zip
+# (c) Joseph Gil
+#
+# Indivdual fonts are public domain
+######################################################################
+
+VGA_FONT = [
+    b'\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+    b'\x00\x00\x00\x7e\x81\xa5\x81\x81\xbd\x99\x81\x7e\x00\x00\x00\x00',
+    b'\x00\x00\x00\x7e\xff\xdb\xff\xff\xc3\xe7\xff\x7e\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x6c\xfe\xfe\xfe\xfe\x7c\x38\x10\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x10\x38\x7c\xfe\x7c\x38\x10\x00\x00\x00\x00\x00',
+    b'\x00\x00\x00\x18\x3c\x3c\xe7\xe7\xe7\x18\x18\x3c\x00\x00\x00\x00',
+    b'\x00\x00\x00\x18\x3c\x7e\xff\xff\x7e\x18\x18\x3c\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x00\x18\x3c\x3c\x18\x00\x00\x00\x00\x00\x00',
+    b'\x00\xff\xff\xff\xff\xff\xe7\xc3\xc3\xe7\xff\xff\xff\xff\xff\x00',
+    b'\x00\x00\x00\x00\x00\x3c\x66\x42\x42\x66\x3c\x00\x00\x00\x00\x00',
+    b'\x00\xff\xff\xff\xff\xc3\x99\xbd\xbd\x99\xc3\xff\xff\xff\xff\x00',
+    b'\x00\x00\x00\x1e\x0e\x1a\x32\x78\xcc\xcc\xcc\x78\x00\x00\x00\x00',
+    b'\x00\x00\x00\x3c\x66\x66\x66\x3c\x18\x7e\x18\x18\x00\x00\x00\x00',
+    b'\x00\x00\x00\x3f\x33\x3f\x30\x30\x30\x70\xf0\xe0\x00\x00\x00\x00',
+    b'\x00\x00\x00\x7f\x63\x7f\x63\x63\x63\x67\xe7\xe6\xc0\x00\x00\x00',
+    b'\x00\x00\x00\x18\x18\xdb\x3c\xe7\x3c\xdb\x18\x18\x00\x00\x00\x00',
+    b'\x00\x00\x00\x80\xc0\xe0\xf8\xfe\xf8\xe0\xc0\x80\x00\x00\x00\x00',
+    b'\x00\x00\x00\x02\x06\x0e\x3e\xfe\x3e\x0e\x06\x02\x00\x00\x00\x00',
+    b'\x00\x00\x00\x18\x3c\x7e\x18\x18\x18\x7e\x3c\x18\x00\x00\x00\x00',
+    b'\x00\x00\x00\x66\x66\x66\x66\x66\x66\x00\x66\x66\x00\x00\x00\x00',
+    b'\x00\x00\x00\x7f\xdb\xdb\xdb\x7b\x1b\x1b\x1b\x1b\x00\x00\x00\x00',
+    b'\x00\x00\x7c\xc6\x60\x38\x6c\xc6\xc6\x6c\x38\x0c\xc6\x7c\x00\x00',
+    b'\x00\x00\x00\x00\x00\x00\x00\x00\x00\xfe\xfe\xfe\x00\x00\x00\x00',
+    b'\x00\x00\x00\x18\x3c\x7e\x18\x18\x18\x7e\x3c\x18\x7e\x00\x00\x00',
+    b'\x00\x00\x00\x18\x3c\x7e\x18\x18\x18\x18\x18\x18\x00\x00\x00\x00',
+    b'\x00\x00\x00\x18\x18\x18\x18\x18\x18\x7e\x3c\x18\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x18\x0c\xfe\x0c\x18\x00\x00\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x30\x60\xfe\x60\x30\x00\x00\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x00\xc0\xc0\xc0\xfe\x00\x00\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x28\x6c\xfe\x6c\x28\x00\x00\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x10\x38\x38\x7c\x7c\xfe\xfe\x00\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\xfe\xfe\x7c\x7c\x38\x38\x10\x00\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+    b'\x00\x00\x00\x18\x3c\x3c\x3c\x18\x18\x00\x18\x18\x00\x00\x00\x00',
+    b'\x00\x00\x66\x66\x66\x24\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+    b'\x00\x00\x00\x6c\x6c\xfe\x6c\x6c\x6c\xfe\x6c\x6c\x00\x00\x00\x00',
+    b'\x00\x18\x18\x7c\xc6\xc2\xc0\x7c\x06\x86\xc6\x7c\x18\x18\x00\x00',
+    b'\x00\x00\x00\x00\x00\xc2\xc6\x0c\x18\x30\x66\xc6\x00\x00\x00\x00',
+    b'\x00\x00\x00\x38\x6c\x6c\x38\x76\xdc\xcc\xcc\x76\x00\x00\x00\x00',
+    b'\x00\x00\x30\x30\x30\x60\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+    b'\x00\x00\x00\x0c\x18\x30\x30\x30\x30\x30\x18\x0c\x00\x00\x00\x00',
+    b'\x00\x00\x00\x30\x18\x0c\x0c\x0c\x0c\x0c\x18\x30\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x66\x3c\xff\x3c\x66\x00\x00\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x18\x18\x7e\x18\x18\x00\x00\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x00\x00\x00\x00\x18\x18\x18\x30\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x00\x00\xfe\x00\x00\x00\x00\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x18\x18\x00\x00\x00\x00',
+    b'\x00\x00\x00\x02\x06\x0c\x18\x30\x60\xc0\x80\x00\x00\x00\x00\x00',
+    b'\x00\x00\x00\x7c\xc6\xce\xde\xf6\xe6\xc6\xc6\x7c\x00\x00\x00\x00',
+    b'\x00\x00\x00\x18\x38\x78\x18\x18\x18\x18\x18\x7e\x00\x00\x00\x00',
+    b'\x00\x00\x00\x7c\xc6\x06\x0c\x18\x30\x60\xc6\xfe\x00\x00\x00\x00',
+    b'\x00\x00\x00\x7c\xc6\x06\x06\x3c\x06\x06\xc6\x7c\x00\x00\x00\x00',
+    b'\x00\x00\x00\x0c\x1c\x3c\x6c\xcc\xfe\x0c\x0c\x1e\x00\x00\x00\x00',
+    b'\x00\x00\x00\xfe\xc0\xc0\xc0\xfc\x06\x06\xc6\x7c\x00\x00\x00\x00',
+    b'\x00\x00\x00\x38\x60\xc0\xc0\xfc\xc6\xc6\xc6\x7c\x00\x00\x00\x00',
+    b'\x00\x00\x00\xfe\xc6\x06\x0c\x18\x30\x30\x30\x30\x00\x00\x00\x00',
+    b'\x00\x00\x00\x7c\xc6\xc6\xc6\x7c\xc6\xc6\xc6\x7c\x00\x00\x00\x00',
+    b'\x00\x00\x00\x7c\xc6\xc6\xc6\x7e\x06\x06\x0c\x78\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x18\x18\x00\x00\x00\x18\x18\x00\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x18\x18\x00\x00\x00\x18\x18\x30\x00\x00\x00\x00',
+    b'\x00\x00\x00\x06\x0c\x18\x30\x60\x30\x18\x0c\x06\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x00\x7e\x00\x00\x7e\x00\x00\x00\x00\x00\x00',
+    b'\x00\x00\x00\x60\x30\x18\x0c\x06\x0c\x18\x30\x60\x00\x00\x00\x00',
+    b'\x00\x00\x00\x7c\xc6\xc6\x0c\x18\x18\x00\x18\x18\x00\x00\x00\x00',
+    b'\x00\x00\x00\x7c\xc6\xc6\xde\xde\xde\xdc\xc0\x7c\x00\x00\x00\x00',
+    b'\x00\x00\x00\x10\x38\x6c\xc6\xc6\xfe\xc6\xc6\xc6\x00\x00\x00\x00',
+    b'\x00\x00\x00\xfc\x66\x66\x66\x7c\x66\x66\x66\xfc\x00\x00\x00\x00',
+    b'\x00\x00\x00\x3c\x66\xc2\xc0\xc0\xc0\xc2\x66\x3c\x00\x00\x00\x00',
+    b'\x00\x00\x00\xf8\x6c\x66\x66\x66\x66\x66\x6c\xf8\x00\x00\x00\x00',
+    b'\x00\x00\x00\xfe\x66\x62\x68\x78\x68\x62\x66\xfe\x00\x00\x00\x00',
+    b'\x00\x00\x00\xfe\x66\x62\x68\x78\x68\x60\x60\xf0\x00\x00\x00\x00',
+    b'\x00\x00\x00\x3c\x66\xc2\xc0\xc0\xde\xc6\x66\x3a\x00\x00\x00\x00',
+    b'\x00\x00\x00\xc6\xc6\xc6\xc6\xfe\xc6\xc6\xc6\xc6\x00\x00\x00\x00',
+    b'\x00\x00\x00\x3c\x18\x18\x18\x18\x18\x18\x18\x3c\x00\x00\x00\x00',
+    b'\x00\x00\x00\x1e\x0c\x0c\x0c\x0c\x0c\xcc\xcc\x78\x00\x00\x00\x00',
+    b'\x00\x00\x00\xe6\x66\x6c\x6c\x78\x6c\x6c\x66\xe6\x00\x00\x00\x00',
+    b'\x00\x00\x00\xf0\x60\x60\x60\x60\x60\x62\x66\xfe\x00\x00\x00\x00',
+    b'\x00\x00\x00\xc6\xee\xfe\xfe\xd6\xc6\xc6\xc6\xc6\x00\x00\x00\x00',
+    b'\x00\x00\x00\xc6\xe6\xf6\xfe\xde\xce\xc6\xc6\xc6\x00\x00\x00\x00',
+    b'\x00\x00\x00\x38\x6c\xc6\xc6\xc6\xc6\xc6\x6c\x38\x00\x00\x00\x00',
+    b'\x00\x00\x00\xfc\x66\x66\x66\x7c\x60\x60\x60\xf0\x00\x00\x00\x00',
+    b'\x00\x00\x00\x7c\xc6\xc6\xc6\xc6\xd6\xde\x7c\x0c\x0e\x00\x00\x00',
+    b'\x00\x00\x00\xfc\x66\x66\x66\x7c\x6c\x66\x66\xe6\x00\x00\x00\x00',
+    b'\x00\x00\x00\x7c\xc6\xc6\x60\x38\x0c\xc6\xc6\x7c\x00\x00\x00\x00',
+    b'\x00\x00\x00\x7e\x7e\x5a\x18\x18\x18\x18\x18\x3c\x00\x00\x00\x00',
+    b'\x00\x00\x00\xc6\xc6\xc6\xc6\xc6\xc6\xc6\xc6\x7c\x00\x00\x00\x00',
+    b'\x00\x00\x00\xc6\xc6\xc6\xc6\xc6\xc6\x6c\x38\x10\x00\x00\x00\x00',
+    b'\x00\x00\x00\xc6\xc6\xc6\xc6\xd6\xd6\xfe\x7c\x6c\x00\x00\x00\x00',
+    b'\x00\x00\x00\xc6\xc6\x6c\x38\x38\x38\x6c\xc6\xc6\x00\x00\x00\x00',
+    b'\x00\x00\x00\x66\x66\x66\x66\x3c\x18\x18\x18\x3c\x00\x00\x00\x00',
+    b'\x00\x00\x00\xfe\xc6\x8c\x18\x30\x60\xc2\xc6\xfe\x00\x00\x00\x00',
+    b'\x00\x00\x00\x3c\x30\x30\x30\x30\x30\x30\x30\x3c\x00\x00\x00\x00',
+    b'\x00\x00\x00\x80\xc0\xe0\x70\x38\x1c\x0e\x06\x02\x00\x00\x00\x00',
+    b'\x00\x00\x00\x3c\x0c\x0c\x0c\x0c\x0c\x0c\x0c\x3c\x00\x00\x00\x00',
+    b'\x00\x10\x38\x6c\xc6\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xff\x00\x00',
+    b'\x00\x30\x30\x18\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x00\x78\x0c\x7c\xcc\xcc\x76\x00\x00\x00\x00',
+    b'\x00\x00\x00\xe0\x60\x60\x78\x6c\x66\x66\x66\x7c\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x00\x7c\xc6\xc0\xc0\xc6\x7c\x00\x00\x00\x00',
+    b'\x00\x00\x00\x1c\x0c\x0c\x3c\x6c\xcc\xcc\xcc\x76\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x00\x7c\xc6\xfe\xc0\xc6\x7c\x00\x00\x00\x00',
+    b'\x00\x00\x00\x38\x6c\x64\x60\xf0\x60\x60\x60\xf0\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x00\x76\xcc\xcc\xcc\x7c\x0c\xcc\x78\x00\x00',
+    b'\x00\x00\x00\xe0\x60\x60\x6c\x76\x66\x66\x66\xe6\x00\x00\x00\x00',
+    b'\x00\x00\x00\x18\x18\x00\x38\x18\x18\x18\x18\x3c\x00\x00\x00\x00',
+    b'\x00\x00\x00\x06\x06\x00\x0e\x06\x06\x06\x06\x66\x66\x3c\x00\x00',
+    b'\x00\x00\x00\xe0\x60\x60\x66\x6c\x78\x6c\x66\xe6\x00\x00\x00\x00',
+    b'\x00\x00\x00\x38\x18\x18\x18\x18\x18\x18\x18\x3c\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x00\xec\xfe\xd6\xd6\xd6\xc6\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x00\xdc\x66\x66\x66\x66\x66\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x00\x7c\xc6\xc6\xc6\xc6\x7c\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x00\xdc\x66\x66\x66\x7c\x60\x60\xf0\x00\x00',
+    b'\x00\x00\x00\x00\x00\x00\x76\xcc\xcc\xcc\x7c\x0c\x0c\x1e\x00\x00',
+    b'\x00\x00\x00\x00\x00\x00\xdc\x76\x66\x60\x60\xf0\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x00\x7c\xc6\x70\x1c\xc6\x7c\x00\x00\x00\x00',
+    b'\x00\x00\x00\x10\x30\x30\xfc\x30\x30\x30\x36\x1c\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x00\xcc\xcc\xcc\xcc\xcc\x76\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x00\x66\x66\x66\x66\x3c\x18\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x00\xc6\xc6\xd6\xd6\xfe\x6c\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x00\xc6\x6c\x38\x38\x6c\xc6\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x00\xc6\xc6\xc6\xc6\x7e\x06\x0c\xf8\x00\x00',
+    b'\x00\x00\x00\x00\x00\x00\xfe\xcc\x18\x30\x66\xfe\x00\x00\x00\x00',
+    b'\x00\x00\x00\x0e\x18\x18\x18\x70\x18\x18\x18\x0e\x00\x00\x00\x00',
+    b'\x00\x00\x00\x18\x18\x18\x18\x00\x18\x18\x18\x18\x00\x00\x00\x00',
+    b'\x00\x00\x00\x70\x18\x18\x18\x0e\x18\x18\x18\x70\x00\x00\x00\x00',
+    b'\x00\x00\x00\x76\xdc\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x10\x38\x6c\xc6\xc6\xfe\x00\x00\x00\x00\x00',
+    b'\x00\x00\x00\x3c\x66\xc2\xc0\xc0\xc2\x66\x3c\x0c\x06\x7c\x00\x00',
+    b'\x00\x00\x00\xcc\xcc\x00\xcc\xcc\xcc\xcc\xcc\x76\x00\x00\x00\x00',
+    b'\x00\x00\x0c\x18\x30\x00\x7c\xc6\xfe\xc0\xc6\x7c\x00\x00\x00\x00',
+    b'\x00\x00\x10\x38\x6c\x00\x78\x0c\x7c\xcc\xcc\x76\x00\x00\x00\x00',
+    b'\x00\x00\x00\xcc\xcc\x00\x78\x0c\x7c\xcc\xcc\x76\x00\x00\x00\x00',
+    b'\x00\x00\x60\x30\x18\x00\x78\x0c\x7c\xcc\xcc\x76\x00\x00\x00\x00',
+    b'\x00\x00\x38\x6c\x38\x00\x78\x0c\x7c\xcc\xcc\x76\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x3c\x66\x60\x66\x3c\x0c\x06\x3c\x00\x00\x00',
+    b'\x00\x00\x10\x38\x6c\x00\x7c\xc6\xfe\xc0\xc6\x7c\x00\x00\x00\x00',
+    b'\x00\x00\x00\xcc\xcc\x00\x7c\xc6\xfe\xc0\xc6\x7c\x00\x00\x00\x00',
+    b'\x00\x00\x60\x30\x18\x00\x7c\xc6\xfe\xc0\xc6\x7c\x00\x00\x00\x00',
+    b'\x00\x00\x00\x66\x66\x00\x38\x18\x18\x18\x18\x3c\x00\x00\x00\x00',
+    b'\x00\x00\x18\x3c\x66\x00\x38\x18\x18\x18\x18\x3c\x00\x00\x00\x00',
+    b'\x00\x00\x60\x30\x18\x00\x38\x18\x18\x18\x18\x3c\x00\x00\x00\x00',
+    b'\x00\x00\xc6\xc6\x10\x38\x6c\xc6\xc6\xfe\xc6\xc6\x00\x00\x00\x00',
+    b'\x00\x38\x6c\x38\x00\x38\x6c\xc6\xc6\xfe\xc6\xc6\x00\x00\x00\x00',
+    b'\x00\x18\x30\x60\x00\xfe\x66\x60\x7c\x60\x66\xfe\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\xcc\x76\x36\x7e\xd8\xd8\x6e\x00\x00\x00\x00',
+    b'\x00\x00\x00\x3e\x6c\xcc\xcc\xfe\xcc\xcc\xcc\xce\x00\x00\x00\x00',
+    b'\x00\x00\x10\x38\x6c\x00\x7c\xc6\xc6\xc6\xc6\x7c\x00\x00\x00\x00',
+    b'\x00\x00\x00\xc6\xc6\x00\x7c\xc6\xc6\xc6\xc6\x7c\x00\x00\x00\x00',
+    b'\x00\x00\x60\x30\x18\x00\x7c\xc6\xc6\xc6\xc6\x7c\x00\x00\x00\x00',
+    b'\x00\x00\x30\x78\xcc\x00\xcc\xcc\xcc\xcc\xcc\x76\x00\x00\x00\x00',
+    b'\x00\x00\x60\x30\x18\x00\xcc\xcc\xcc\xcc\xcc\x76\x00\x00\x00\x00',
+    b'\x00\x00\x00\xc6\xc6\x00\xc6\xc6\xc6\xc6\x7e\x06\x0c\x78\x00\x00',
+    b'\x00\x00\xc6\xc6\x38\x6c\xc6\xc6\xc6\xc6\x6c\x38\x00\x00\x00\x00',
+    b'\x00\x00\xc6\xc6\x00\xc6\xc6\xc6\xc6\xc6\xc6\x7c\x00\x00\x00\x00',
+    b'\x00\x00\x18\x18\x3c\x66\x60\x60\x66\x3c\x18\x18\x00\x00\x00\x00',
+    b'\x00\x00\x38\x6c\x64\x60\xf0\x60\x60\x60\xe6\xfc\x00\x00\x00\x00',
+    b'\x00\x00\x00\x66\x66\x3c\x18\x7e\x18\x7e\x18\x18\x00\x00\x00\x00',
+    b'\x00\x00\xf8\xcc\xcc\xf8\xc4\xcc\xde\xcc\xcc\xc6\x00\x00\x00\x00',
+    b'\x00\x00\x0e\x1b\x18\x18\x18\x7e\x18\x18\x18\x18\xd8\x70\x00\x00',
+    b'\x00\x00\x18\x30\x60\x00\x78\x0c\x7c\xcc\xcc\x76\x00\x00\x00\x00',
+    b'\x00\x00\x0c\x18\x30\x00\x38\x18\x18\x18\x18\x3c\x00\x00\x00\x00',
+    b'\x00\x00\x18\x30\x60\x00\x7c\xc6\xc6\xc6\xc6\x7c\x00\x00\x00\x00',
+    b'\x00\x00\x18\x30\x60\x00\xcc\xcc\xcc\xcc\xcc\x76\x00\x00\x00\x00',
+    b'\x00\x00\x00\x76\xdc\x00\xdc\x66\x66\x66\x66\x66\x00\x00\x00\x00',
+    b'\x00\x76\xdc\x00\xc6\xe6\xf6\xfe\xde\xce\xc6\xc6\x00\x00\x00\x00',
+    b'\x00\x00\x3c\x6c\x6c\x3e\x00\x7e\x00\x00\x00\x00\x00\x00\x00\x00',
+    b'\x00\x00\x38\x6c\x6c\x38\x00\x7c\x00\x00\x00\x00\x00\x00\x00\x00',
+    b'\x00\x00\x00\x30\x30\x00\x30\x30\x60\xc6\xc6\x7c\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x00\x00\xfe\xc0\xc0\xc0\x00\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x00\x00\xfe\x06\x06\x06\x00\x00\x00\x00\x00',
+    b'\x00\x00\xc0\xc0\xc6\xcc\xd8\x30\x60\xdc\x86\x0c\x18\x3e\x00\x00',
+    b'\x00\x00\xc0\xc0\xc6\xcc\xd8\x30\x66\xce\x9e\x3e\x06\x06\x00\x00',
+    b'\x00\x00\x00\x18\x18\x00\x18\x18\x3c\x3c\x3c\x18\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x36\x6c\xd8\x6c\x36\x00\x00\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\xd8\x6c\x36\x6c\xd8\x00\x00\x00\x00\x00\x00',
+    b'\x00\x11\x44\x11\x44\x11\x44\x11\x44\x11\x44\x11\x44\x11\x44\x00',
+    b'\x00\x55\xaa\x55\xaa\x55\xaa\x55\xaa\x55\xaa\x55\xaa\x55\xaa\x00',
+    b'\x00\xdd\x77\xdd\x77\xdd\x77\xdd\x77\xdd\x77\xdd\x77\xdd\x77\x00',
+    b'\x00\x18\x18\x18\x18\x18\x18\x18\x18\x18\x18\x18\x18\x18\x18\x00',
+    b'\x00\x18\x18\x18\x18\x18\x18\x18\xf8\x18\x18\x18\x18\x18\x18\x00',
+    b'\x00\x18\x18\x18\x18\x18\xf8\x18\xf8\x18\x18\x18\x18\x18\x18\x00',
+    b'\x00\x36\x36\x36\x36\x36\x36\x36\xf6\x36\x36\x36\x36\x36\x36\x00',
+    b'\x00\x00\x00\x00\x00\x00\x00\x00\xfe\x36\x36\x36\x36\x36\x36\x00',
+    b'\x00\x00\x00\x00\x00\x00\xf8\x18\xf8\x18\x18\x18\x18\x18\x18\x00',
+    b'\x00\x36\x36\x36\x36\x36\xf6\x06\xf6\x36\x36\x36\x36\x36\x36\x00',
+    b'\x00\x36\x36\x36\x36\x36\x36\x36\x36\x36\x36\x36\x36\x36\x36\x00',
+    b'\x00\x00\x00\x00\x00\x00\xfe\x06\xf6\x36\x36\x36\x36\x36\x36\x00',
+    b'\x00\x36\x36\x36\x36\x36\xf6\x06\xfe\x00\x00\x00\x00\x00\x00\x00',
+    b'\x00\x36\x36\x36\x36\x36\x36\x36\xfe\x00\x00\x00\x00\x00\x00\x00',
+    b'\x00\x18\x18\x18\x18\x18\xf8\x18\xf8\x00\x00\x00\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x00\x00\x00\xf8\x18\x18\x18\x18\x18\x18\x00',
+    b'\x00\x18\x18\x18\x18\x18\x18\x18\x1f\x00\x00\x00\x00\x00\x00\x00',
+    b'\x00\x18\x18\x18\x18\x18\x18\x18\xff\x00\x00\x00\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x00\x00\x00\xff\x18\x18\x18\x18\x18\x18\x00',
+    b'\x00\x18\x18\x18\x18\x18\x18\x18\x1f\x18\x18\x18\x18\x18\x18\x00',
+    b'\x00\x00\x00\x00\x00\x00\x00\x00\xff\x00\x00\x00\x00\x00\x00\x00',
+    b'\x00\x18\x18\x18\x18\x18\x18\x18\xff\x18\x18\x18\x18\x18\x18\x00',
+    b'\x00\x18\x18\x18\x18\x18\x1f\x18\x1f\x18\x18\x18\x18\x18\x18\x00',
+    b'\x00\x36\x36\x36\x36\x36\x36\x36\x37\x36\x36\x36\x36\x36\x36\x00',
+    b'\x00\x36\x36\x36\x36\x36\x37\x30\x3f\x00\x00\x00\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x00\x3f\x30\x37\x36\x36\x36\x36\x36\x36\x00',
+    b'\x00\x36\x36\x36\x36\x36\xf7\x00\xff\x00\x00\x00\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x00\xff\x00\xf7\x36\x36\x36\x36\x36\x36\x00',
+    b'\x00\x36\x36\x36\x36\x36\x37\x30\x37\x36\x36\x36\x36\x36\x36\x00',
+    b'\x00\x00\x00\x00\x00\x00\xff\x00\xff\x00\x00\x00\x00\x00\x00\x00',
+    b'\x00\x36\x36\x36\x36\x36\xf7\x00\xf7\x36\x36\x36\x36\x36\x36\x00',
+    b'\x00\x18\x18\x18\x18\x18\xff\x00\xff\x00\x00\x00\x00\x00\x00\x00',
+    b'\x00\x36\x36\x36\x36\x36\x36\x36\xff\x00\x00\x00\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x00\xff\x00\xff\x18\x18\x18\x18\x18\x18\x00',
+    b'\x00\x00\x00\x00\x00\x00\x00\x00\xff\x36\x36\x36\x36\x36\x36\x00',
+    b'\x00\x36\x36\x36\x36\x36\x36\x36\x3f\x00\x00\x00\x00\x00\x00\x00',
+    b'\x00\x18\x18\x18\x18\x18\x1f\x18\x1f\x00\x00\x00\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x00\x1f\x18\x1f\x18\x18\x18\x18\x18\x18\x00',
+    b'\x00\x00\x00\x00\x00\x00\x00\x00\x3f\x36\x36\x36\x36\x36\x36\x00',
+    b'\x00\x36\x36\x36\x36\x36\x36\x36\xff\x36\x36\x36\x36\x36\x36\x00',
+    b'\x00\x18\x18\x18\x18\x18\xff\x18\xff\x18\x18\x18\x18\x18\x18\x00',
+    b'\x00\x18\x18\x18\x18\x18\x18\x18\xf8\x00\x00\x00\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x00\x00\x00\x1f\x18\x18\x18\x18\x18\x18\x00',
+    b'\x00\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\x00',
+    b'\x00\x00\x00\x00\x00\x00\x00\x00\xff\xff\xff\xff\xff\xff\xff\x00',
+    b'\x00\xf0\xf0\xf0\xf0\xf0\xf0\xf0\xf0\xf0\xf0\xf0\xf0\xf0\xf0\x00',
+    b'\x00\x0f\x0f\x0f\x0f\x0f\x0f\x0f\x0f\x0f\x0f\x0f\x0f\x0f\x0f\x00',
+    b'\x00\xff\xff\xff\xff\xff\xff\xff\x00\x00\x00\x00\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x00\x76\xdc\xd8\xd8\xdc\x76\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x7c\xc6\xfc\xc6\xc6\xfc\xc0\xc0\x40\x00\x00',
+    b'\x00\x00\x00\xfe\xc6\xc6\xc0\xc0\xc0\xc0\xc0\xc0\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\xfe\x6c\x6c\x6c\x6c\x6c\x6c\x00\x00\x00\x00',
+    b'\x00\x00\x00\xfe\xc6\x60\x30\x18\x30\x60\xc6\xfe\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x00\x7e\xd8\xd8\xd8\xd8\x70\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x66\x66\x66\x66\x7c\x60\x60\xc0\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x76\xdc\x18\x18\x18\x18\x18\x00\x00\x00\x00',
+    b'\x00\x00\x00\x7e\x18\x3c\x66\x66\x66\x3c\x18\x7e\x00\x00\x00\x00',
+    b'\x00\x00\x00\x38\x6c\xc6\xc6\xfe\xc6\xc6\x6c\x38\x00\x00\x00\x00',
+    b'\x00\x00\x00\x38\x6c\xc6\xc6\xc6\x6c\x6c\x6c\xee\x00\x00\x00\x00',
+    b'\x00\x00\x00\x1e\x30\x18\x0c\x3e\x66\x66\x66\x3c\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x00\x7e\xdb\xdb\x7e\x00\x00\x00\x00\x00\x00',
+    b'\x00\x00\x00\x03\x06\x7e\xdb\xdb\xf3\x7e\x60\xc0\x00\x00\x00\x00',
+    b'\x00\x00\x00\x1c\x30\x60\x60\x7c\x60\x60\x30\x1c\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x7c\xc6\xc6\xc6\xc6\xc6\xc6\xc6\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\xfe\x00\x00\xfe\x00\x00\xfe\x00\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x18\x18\x7e\x18\x18\x00\x00\xff\x00\x00\x00\x00',
+    b'\x00\x00\x00\x30\x18\x0c\x06\x0c\x18\x30\x00\x7e\x00\x00\x00\x00',
+    b'\x00\x00\x00\x0c\x18\x30\x60\x30\x18\x0c\x00\x7e\x00\x00\x00\x00',
+    b'\x00\x00\x00\x0e\x1b\x1b\x18\x18\x18\x18\x18\x18\x18\x18\x18\x00',
+    b'\x00\x18\x18\x18\x18\x18\x18\x18\x18\xd8\xd8\x70\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x18\x18\x00\x7e\x00\x18\x18\x00\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x76\xdc\x00\x76\xdc\x00\x00\x00\x00\x00\x00',
+    b'\x00\x00\x38\x6c\x6c\x38\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x00\x00\x18\x18\x00\x00\x00\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x00\x00\x00\x18\x00\x00\x00\x00\x00\x00\x00',
+    b'\x00\x00\x0f\x0c\x0c\x0c\x0c\x0c\xec\x6c\x3c\x1c\x00\x00\x00\x00',
+    b'\x00\x00\xd8\x6c\x6c\x6c\x6c\x6c\x00\x00\x00\x00\x00\x00\x00\x00',
+    b'\x00\x00\x70\xd8\x30\x60\xc8\xf8\x00\x00\x00\x00\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x7c\x7c\x7c\x7c\x7c\x7c\x00\x00\x00\x00\x00',
+    b'\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'
+]

klippy/extras/display/hd44780.py

@@ -0,0 +1,134 @@
+# Support for HD44780 (20x4 text) LCD displays
+#
+# Copyright (C) 2018  Kevin O'Connor <kevin@koconnor.net>
+# Copyright (C) 2018  Eric Callahan <arksine.code@gmail.com>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import logging
+
+BACKGROUND_PRIORITY_CLOCK = 0x7fffffff00000000
+LINE_LENGTH_DEFAULT=20
+LINE_LENGTH_OPTIONS={16:16, 20:20}
+
+TextGlyphs = { 'right_arrow': b'\x7e' }
+
+HD44780_DELAY = .000040
+
+class HD44780:
+    def __init__(self, config):
+        self.printer = config.get_printer()
+        # pin config
+        ppins = self.printer.lookup_object('pins')
+        pins = [ppins.lookup_pin(config.get(name + '_pin'))
+                for name in ['rs', 'e', 'd4', 'd5', 'd6', 'd7']]
+        self.hd44780_protocol_init = config.getboolean('hd44780_protocol_init',
+                                                       True)
+        self.line_length = config.getchoice('line_length', LINE_LENGTH_OPTIONS,
+                                            LINE_LENGTH_DEFAULT)
+        mcu = None
+        for pin_params in pins:
+            if mcu is not None and pin_params['chip'] != mcu:
+                raise ppins.error("hd44780 all pins must be on same mcu")
+            mcu = pin_params['chip']
+        self.pins = [pin_params['pin'] for pin_params in pins]
+        self.mcu = mcu
+        self.oid = self.mcu.create_oid()
+        self.mcu.register_config_callback(self.build_config)
+        self.send_data_cmd = self.send_cmds_cmd = None
+        self.icons = {}
+        # framebuffers
+        self.text_framebuffers = [bytearray(b' '*2*self.line_length),
+                                  bytearray(b' '*2*self.line_length)]
+        self.glyph_framebuffer = bytearray(64)
+        self.all_framebuffers = [
+            # Text framebuffers
+            (self.text_framebuffers[0], bytearray(b'~'*2*self.line_length),
+             0x80),
+            (self.text_framebuffers[1], bytearray(b'~'*2*self.line_length),
+             0xc0),
+            # Glyph framebuffer
+            (self.glyph_framebuffer, bytearray(b'~'*64), 0x40) ]
+    def build_config(self):
+        self.mcu.add_config_cmd(
+            "config_hd44780 oid=%d rs_pin=%s e_pin=%s"
+            " d4_pin=%s d5_pin=%s d6_pin=%s d7_pin=%s delay_ticks=%d" % (
+                self.oid, self.pins[0], self.pins[1],
+                self.pins[2], self.pins[3], self.pins[4], self.pins[5],
+                self.mcu.seconds_to_clock(HD44780_DELAY)))
+        cmd_queue = self.mcu.alloc_command_queue()
+        self.send_cmds_cmd = self.mcu.lookup_command(
+            "hd44780_send_cmds oid=%c cmds=%*s", cq=cmd_queue)
+        self.send_data_cmd = self.mcu.lookup_command(
+            "hd44780_send_data oid=%c data=%*s", cq=cmd_queue)
+    def send(self, cmds, is_data=False):
+        cmd_type = self.send_cmds_cmd
+        if is_data:
+            cmd_type = self.send_data_cmd
+        cmd_type.send([self.oid, cmds], reqclock=BACKGROUND_PRIORITY_CLOCK)
+        #logging.debug("hd44780 %d %s", is_data, repr(cmds))
+    def flush(self):
+        # Find all differences in the framebuffers and send them to the chip
+        for new_data, old_data, fb_id in self.all_framebuffers:
+            if new_data == old_data:
+                continue
+            # Find the position of all changed bytes in this framebuffer
+            diffs = [[i, 1] for i, (n, o) in enumerate(zip(new_data, old_data))
+                     if n != o]
+            # Batch together changes that are close to each other
+            for i in range(len(diffs)-2, -1, -1):
+                pos, count = diffs[i]
+                nextpos, nextcount = diffs[i+1]
+                if pos + 4 >= nextpos and nextcount < 16:
+                    diffs[i][1] = nextcount + (nextpos - pos)
+                    del diffs[i+1]
+            # Transmit changes
+            for pos, count in diffs:
+                chip_pos = pos
+                self.send([fb_id + chip_pos])
+                self.send(new_data[pos:pos+count], is_data=True)
+            old_data[:] = new_data
+    def init(self):
+        curtime = self.printer.get_reactor().monotonic()
+        print_time = self.mcu.estimated_print_time(curtime)
+        # Program 4bit / 2-line mode and then issue 0x02 "Home" command
+        if self.hd44780_protocol_init:
+            init = [[0x33], [0x33], [0x32], [0x28, 0x28, 0x02]]
+        else:
+            init = [[0x02]]
+        # Reset (set positive direction ; enable display and hide cursor)
+        init.append([0x06, 0x0c])
+        for i, cmds in enumerate(init):
+            minclock = self.mcu.print_time_to_clock(print_time + i * .100)
+            self.send_cmds_cmd.send([self.oid, cmds], minclock=minclock)
+        self.flush()
+    def write_text(self, x, y, data):
+        if x + len(data) > self.line_length:
+            data = data[:self.line_length - min(x, self.line_length)]
+        pos = x + ((y & 0x02) >> 1) * self.line_length
+        self.text_framebuffers[y & 1][pos:pos+len(data)] = data
+    def set_glyphs(self, glyphs):
+        for glyph_name, glyph_data in glyphs.items():
+            data = glyph_data.get('icon5x8')
+            if data is not None:
+                self.icons[glyph_name] = data
+    def write_glyph(self, x, y, glyph_name):
+        data = self.icons.get(glyph_name)
+        if data is not None:
+            slot, bits = data
+            self.write_text(x, y, [slot])
+            self.glyph_framebuffer[slot * 8:(slot + 1) * 8] = bits
+            return 1
+        char = TextGlyphs.get(glyph_name)
+        if char is not None:
+            # Draw character
+            self.write_text(x, y, char)
+            return 1
+        return 0
+    def write_graphics(self, x, y, data):
+        pass
+    def clear(self):
+        spaces = b' ' * 2*self.line_length
+        self.text_framebuffers[0][:] = spaces
+        self.text_framebuffers[1][:] = spaces
+    def get_dimensions(self):
+        return (self.line_length, 4)

klippy/extras/display/hd44780_spi.py

@@ -0,0 +1,125 @@
+# Support for HD44780 (20x4 text) LCD displays
+#
+# Copyright (C) 2018  Kevin O'Connor <kevin@koconnor.net>
+# Copyright (C) 2018  Eric Callahan <arksine.code@gmail.com>
+# Copyright (C) 2021  Marc-Andre Denis <marcadenis@msn.com>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import logging
+from .. import bus
+
+LINE_LENGTH_DEFAULT=20
+LINE_LENGTH_OPTIONS={16:16, 20:20}
+
+TextGlyphs = { 'right_arrow': b'\x7e' }
+
+
+
+class hd44780_spi:
+    def __init__(self, config):
+        self.printer = config.get_printer()
+        self.hd44780_protocol_init = config.getboolean('hd44780_protocol_init',
+                                                       True)
+        # spi config
+        self.spi = bus.MCU_SPI_from_config(
+            config, 0x00, pin_option="latch_pin")
+        self.mcu = self.spi.get_mcu()
+        #self.spi.spi_send([0x01,0xa0])
+        self.data_mask = (1<<1)
+        self.command_mask = 0
+        self.enable_mask = (1<<3)
+
+        self.icons = {}
+        self.line_length = config.getchoice('line_length', LINE_LENGTH_OPTIONS,
+                                            LINE_LENGTH_DEFAULT)
+
+        # framebuffers
+        self.text_framebuffers = [bytearray(b' '*2*self.line_length),
+                                  bytearray(b' '*2*self.line_length)]
+        self.glyph_framebuffer = bytearray(64)
+        self.all_framebuffers = [
+            # Text framebuffers
+            (self.text_framebuffers[0], bytearray(b'~'*2*self.line_length),
+             0x80),
+            (self.text_framebuffers[1], bytearray(b'~'*2*self.line_length),
+             0xc0),
+            # Glyph framebuffer
+            (self.glyph_framebuffer, bytearray(b'~'*64), 0x40) ]
+    def send_4_bits(self, cmd, is_data, minclock):
+        if is_data:
+            mask = self.data_mask
+        else:
+            mask = self.command_mask
+        self.spi.spi_send([(cmd & 0xF0) | mask], minclock)
+        self.spi.spi_send([(cmd & 0xF0) | mask | self.enable_mask], minclock)
+        self.spi.spi_send([(cmd & 0xF0) | mask], minclock)
+    def send(self, cmds, is_data=False, minclock=0):
+        for data in cmds:
+            self.send_4_bits(data,is_data,minclock)
+            self.send_4_bits(data<<4,is_data,minclock)
+    def flush(self):
+        # Find all differences in the framebuffers and send them to the chip
+        for new_data, old_data, fb_id in self.all_framebuffers:
+            if new_data == old_data:
+                continue
+            # Find the position of all changed bytes in this framebuffer
+            diffs = [[i, 1] for i, (n, o) in enumerate(zip(new_data, old_data))
+                     if n != o]
+            # Batch together changes that are close to each other
+            for i in range(len(diffs)-2, -1, -1):
+                pos, count = diffs[i]
+                nextpos, nextcount = diffs[i+1]
+                if pos + 4 >= nextpos and nextcount < 16:
+                    diffs[i][1] = nextcount + (nextpos - pos)
+                    del diffs[i+1]
+            # Transmit changes
+            for pos, count in diffs:
+                chip_pos = pos
+                self.send([fb_id + chip_pos])
+                self.send(new_data[pos:pos+count], is_data=True)
+            old_data[:] = new_data
+    def init(self):
+        curtime = self.printer.get_reactor().monotonic()
+        print_time = self.mcu.estimated_print_time(curtime)
+        # Program 4bit / 2-line mode and then issue 0x02 "Home" command
+        if self.hd44780_protocol_init:
+            init = [[0x33], [0x33], [0x32], [0x28, 0x28, 0x02]]
+        else:
+            init = [[0x02]]
+        # Reset (set positive direction ; enable display and hide cursor)
+        init.append([0x06, 0x0c])
+        for i, cmds in enumerate(init):
+            minclock = self.mcu.print_time_to_clock(print_time + i * .100)
+            self.send(cmds, minclock=minclock)
+        self.flush()
+    def write_text(self, x, y, data):
+        if x + len(data) > self.line_length:
+            data = data[:self.line_length - min(x, self.line_length)]
+        pos = x + ((y & 0x02) >> 1) * self.line_length
+        self.text_framebuffers[y & 1][pos:pos+len(data)] = data
+    def set_glyphs(self, glyphs):
+        for glyph_name, glyph_data in glyphs.items():
+            data = glyph_data.get('icon5x8')
+            if data is not None:
+                self.icons[glyph_name] = data
+    def write_glyph(self, x, y, glyph_name):
+        data = self.icons.get(glyph_name)
+        if data is not None:
+            slot, bits = data
+            self.write_text(x, y, [slot])
+            self.glyph_framebuffer[slot * 8:(slot + 1) * 8] = bits
+            return 1
+        char = TextGlyphs.get(glyph_name)
+        if char is not None:
+            # Draw character
+            self.write_text(x, y, char)
+            return 1
+        return 0
+    def write_graphics(self, x, y, data):
+        pass
+    def clear(self):
+        spaces = b' ' * 2*self.line_length
+        self.text_framebuffers[0][:] = spaces
+        self.text_framebuffers[1][:] = spaces
+    def get_dimensions(self):
+        return (self.line_length, 4)

klippy/extras/display/menu.cfg

@@ -0,0 +1,761 @@
+# This file defines the default layout of the printer's menu.
+
+# It is not necessary to edit this file to change the menu. Instead,
+# one may override any of the sections defined here by defining a
+# section with the same name in the main printer.cfg config file.
+
+### DEFAULT MENU ###
+# Main
+#   + Tune
+#       + Speed: 000%
+#       + Flow: 000%
+#       + Offset Z:00.00
+#   + OctoPrint
+#       + Pause printing
+#       + Resume printing
+#       + Abort printing
+#   + SD Card
+#       + Start printing
+#       + Resume printing
+#       + Pause printing
+#       + Cancel printing
+#       + ... (files)
+#   + Control
+#       + Home All
+#       + Home Z
+#       + Home X/Y
+#       + Steppers off
+#       + Fan: OFF
+#       + Fan speed: 000%
+#       + Lights: OFF
+#       + Lights: 000%
+#       + Move 10mm
+#           + Move X:000.0
+#           + Move Y:000.0
+#           + Move Z:000.0
+#           + Move E:+000.0
+#       + Move 1mm
+#           + Move X:000.0
+#           + Move Y:000.0
+#           + Move Z:000.0
+#           + Move E:+000.0
+#       + Move 0.1mm
+#           + Move X:000.0
+#           + Move Y:000.0
+#           + Move Z:000.0
+#           + Move E:+000.0
+#   + Temperature
+#       + Ex0:000 (0000)
+#       + Ex1:000 (0000)
+#       + Bed:000 (0000)
+#       + Preheat PLA
+#           + Preheat all
+#           + Preheat hotend
+#           + Preheat hotbed
+#       + Preheat ABS
+#           + Preheat all
+#           + Preheat hotend
+#           + Preheat hotbed
+#       + Cooldown
+#           + Cooldown all
+#           + Cooldown hotend
+#           + Cooldown hotbed
+#   + Filament
+#       + Ex0:000 (0000)
+#       + Load Fil. fast
+#       + Load Fil. slow
+#       + Unload Fil.fast
+#       + Unload Fil.slow
+#       + Feed: 000.0
+#   + Setup
+#       + Save config
+#       + Restart
+#           + Restart host
+#           + Restart FW
+#       + PID tuning
+#           + Tune Hotend PID
+#           + Tune Hotbed PID
+#       + Calibration
+#           + Delta cal. auto
+#           + Delta cal. man
+#               + Start probing
+#               + Move Z: 000.00
+#               + Test Z: ++
+#               + Accept
+#               + Abort
+#           + Bed probe
+#       + Dump parameters
+
+### menu main ###
+[menu __main]
+type: list
+name: Main
+
+### menu tune ###
+[menu __main __tune]
+type: list
+enable: {printer.idle_timeout.state == "Printing"}
+name: Tune
+
+[menu __main __tune __speed]
+type: input
+name: Speed: {'%3d' % (menu.input*100)}%
+input: {printer.gcode_move.speed_factor}
+input_min: 0.01
+input_max: 5
+input_step: 0.01
+realtime: True
+gcode:
+    M220 S{'%d' % (menu.input*100)}
+
+[menu __main __tune __flow]
+type: input
+name: Flow: {'%3d' % (menu.input*100)}%
+input: {printer.gcode_move.extrude_factor}
+input_min: 0.01
+input_max: 2
+input_step: 0.01
+realtime: True
+gcode:
+    M221 S{'%d' % (menu.input*100)}
+
+[menu __main __tune __offsetz]
+type: input
+name: Offset Z:{'%05.3f' % menu.input}
+input: {printer.gcode_move.homing_origin.z}
+input_min: -5
+input_max: 5
+input_step: 0.005
+realtime: True
+gcode:
+    SET_GCODE_OFFSET Z={'%.3f' % menu.input} MOVE=1
+
+
+### menu octoprint ###
+[menu __main __octoprint]
+type: list
+name: OctoPrint
+
+[menu __main __octoprint __pause]
+type: command
+enable: {printer.idle_timeout.state == "Printing"}
+name: Pause printing
+gcode:
+    {action_respond_info('action:pause')}
+
+[menu __main __octoprint __resume]
+type: command
+enable: {not printer.idle_timeout.state == "Printing"}
+name: Resume printing
+gcode:
+    {action_respond_info('action:resume')}
+
+[menu __main __octoprint __abort]
+type: command
+enable: {printer.idle_timeout.state == "Printing"}
+name: Abort printing
+gcode:
+    {action_respond_info('action:cancel')}
+
+### menu virtual sdcard ###
+[menu __main __sdcard]
+type: vsdlist
+enable: {('virtual_sdcard' in printer)}
+name: SD Card
+
+[menu __main __sdcard __start]
+type: command
+enable: {('virtual_sdcard' in printer) and printer.virtual_sdcard.file_path and not printer.virtual_sdcard.is_active}
+name: Start printing
+gcode: M24
+
+[menu __main __sdcard __resume]
+type: command
+enable: {('virtual_sdcard' in printer) and printer.print_stats.state == "paused"}
+name: Resume printing
+gcode:
+    {% if "pause_resume" in printer %}
+        RESUME
+    {% else %}
+        M24
+    {% endif %}
+
+[menu __main __sdcard __pause]
+type: command
+enable: {('virtual_sdcard' in printer) and printer.print_stats.state == "printing"}
+name: Pause printing
+gcode:
+    {% if "pause_resume" in printer %}
+        PAUSE
+    {% else %}
+        M25
+    {% endif %}
+
+[menu __main __sdcard __cancel]
+type: command
+enable: {('virtual_sdcard' in printer) and (printer.print_stats.state == "printing" or printer.print_stats.state == "paused")}
+name: Cancel printing
+gcode:
+    {% if 'pause_resume' in printer %}
+        CANCEL_PRINT
+    {% else %}
+        M25
+        M27
+        M26 S0
+        TURN_OFF_HEATERS
+        {% if printer.toolhead.position.z <= printer.toolhead.axis_maximum.z - 5 %}
+            G91
+            G0 Z5 F1000
+            G90
+        {% endif %}
+    {% endif %}
+
+### menu control ###
+[menu __main __control]
+type: list
+name: Control
+
+[menu __main __control __home]
+type: command
+enable: {not printer.idle_timeout.state == "Printing"}
+name: Home All
+gcode: G28
+
+[menu __main __control __homez]
+type: command
+enable: {not printer.idle_timeout.state == "Printing"}
+name: Home Z
+gcode: G28 Z
+
+[menu __main __control __homexy]
+type: command
+enable: {not printer.idle_timeout.state == "Printing"}
+name: Home X/Y
+gcode: G28 X Y
+
+[menu __main __control __disable]
+type: command
+name: Steppers off
+gcode:
+    M84
+    M18
+
+[menu __main __control __fanonoff]
+type: input
+enable: {'fan' in printer}
+name: Fan: {'ON ' if menu.input else 'OFF'}
+input: {printer.fan.speed}
+input_min: 0
+input_max: 1
+input_step: 1
+gcode:
+    M106 S{255 if menu.input else 0}
+
+[menu __main __control __fanspeed]
+type: input
+enable: {'fan' in printer}
+name: Fan speed: {'%3d' % (menu.input*100)}%
+input: {printer.fan.speed}
+input_min: 0
+input_max: 1
+input_step: 0.01
+gcode:
+    M106 S{'%d' % (menu.input*255)}
+
+[menu __main __control __caselightonoff]
+type: input
+enable: {'output_pin caselight' in printer}
+name: Lights: {'ON ' if menu.input else 'OFF'}
+input: {printer['output_pin caselight'].value}
+input_min: 0
+input_max: 1
+input_step: 1
+gcode:
+    SET_PIN PIN=caselight VALUE={1 if menu.input else 0}
+
+[menu __main __control __caselightpwm]
+type: input
+enable: {'output_pin caselight' in printer}
+name: Lights: {'%3d' % (menu.input*100)}%
+input: {printer['output_pin caselight'].value}
+input_min: 0.0
+input_max: 1.0
+input_step: 0.01
+gcode:
+    SET_PIN PIN=caselight VALUE={menu.input}
+
+### menu move 10mm ###
+[menu __main __control __move_10mm]
+type: list
+enable: {not printer.idle_timeout.state == "Printing"}
+name: Move 10mm
+
+[menu __main __control __move_10mm __axis_x]
+type: input
+name: Move X:{'%05.1f' % menu.input}
+input: {printer.gcode_move.gcode_position.x}
+input_min: {printer.toolhead.axis_minimum.x}
+input_max: {printer.toolhead.axis_maximum.x}
+input_step: 10.0
+gcode:
+    SAVE_GCODE_STATE NAME=__move__axis
+    G90
+    G1 X{menu.input}
+    RESTORE_GCODE_STATE NAME=__move__axis
+
+[menu __main __control __move_10mm __axis_y]
+type: input
+name: Move Y:{'%05.1f' % menu.input}
+input: {printer.gcode_move.gcode_position.y}
+input_min: {printer.toolhead.axis_minimum.y}
+input_max: {printer.toolhead.axis_maximum.y}
+input_step: 10.0
+gcode:
+    SAVE_GCODE_STATE NAME=__move__axis
+    G90
+    G1 Y{menu.input}
+    RESTORE_GCODE_STATE NAME=__move__axis
+
+[menu __main __control __move_10mm __axis_z]
+type: input
+enable: {not printer.idle_timeout.state == "Printing"}
+name: Move Z:{'%05.1f' % menu.input}
+input: {printer.gcode_move.gcode_position.z}
+input_min: 0
+input_max: {printer.toolhead.axis_maximum.z}
+input_step: 10.0
+gcode:
+    SAVE_GCODE_STATE NAME=__move__axis
+    G90
+    G1 Z{menu.input}
+    RESTORE_GCODE_STATE NAME=__move__axis
+
+[menu __main __control __move_10mm __axis_e]
+type: input
+enable: {not printer.idle_timeout.state == "Printing"}
+name: Move E:{'%+06.1f' % menu.input}
+input: 0
+input_min: -{printer.configfile.config.extruder.max_extrude_only_distance|default(50)}
+input_max: {printer.configfile.config.extruder.max_extrude_only_distance|default(50)}
+input_step: 10.0
+gcode:
+    SAVE_GCODE_STATE NAME=__move__axis
+    M83
+    G1 E{menu.input} F240
+    RESTORE_GCODE_STATE NAME=__move__axis
+
+### menu move 1mm ###
+[menu __main __control __move_1mm]
+type: list
+enable: {not printer.idle_timeout.state == "Printing"}
+name: Move 1mm
+
+[menu __main __control __move_1mm __axis_x]
+type: input
+name: Move X:{'%05.1f' % menu.input}
+input: {printer.gcode_move.gcode_position.x}
+input_min: {printer.toolhead.axis_minimum.x}
+input_max: {printer.toolhead.axis_maximum.x}
+input_step: 1.0
+gcode:
+    SAVE_GCODE_STATE NAME=__move__axis
+    G90
+    G1 X{menu.input}
+    RESTORE_GCODE_STATE NAME=__move__axis
+
+[menu __main __control __move_1mm __axis_y]
+type: input
+name: Move Y:{'%05.1f' % menu.input}
+input: {printer.gcode_move.gcode_position.y}
+input_min: {printer.toolhead.axis_minimum.y}
+input_max: {printer.toolhead.axis_maximum.y}
+input_step: 1.0
+gcode:
+    SAVE_GCODE_STATE NAME=__move__axis
+    G90
+    G1 Y{menu.input}
+    RESTORE_GCODE_STATE NAME=__move__axis
+
+[menu __main __control __move_1mm __axis_z]
+type: input
+enable: {not printer.idle_timeout.state == "Printing"}
+name: Move Z:{'%05.1f' % menu.input}
+input: {printer.gcode_move.gcode_position.z}
+input_min: 0
+input_max: {printer.toolhead.axis_maximum.z}
+input_step: 1.0
+gcode:
+    SAVE_GCODE_STATE NAME=__move__axis
+    G90
+    G1 Z{menu.input}
+    RESTORE_GCODE_STATE NAME=__move__axis
+
+[menu __main __control __move_1mm __axis_e]
+type: input
+enable: {not printer.idle_timeout.state == "Printing"}
+name: Move E:{'%+06.1f' % menu.input}
+input: 0
+input_min: -{printer.configfile.config.extruder.max_extrude_only_distance|default(50)}
+input_max: {printer.configfile.config.extruder.max_extrude_only_distance|default(50)}
+input_step: 1.0
+gcode:
+    SAVE_GCODE_STATE NAME=__move__axis
+    M83
+    G1 E{menu.input} F240
+    RESTORE_GCODE_STATE NAME=__move__axis
+
+### menu move 0.1mm ###
+[menu __main __control __move_01mm]
+type: list
+enable: {not printer.idle_timeout.state == "Printing"}
+name: Move 0.1mm
+
+[menu __main __control __move_01mm __axis_x]
+type: input
+name: Move X:{'%05.1f' % menu.input}
+input: {printer.gcode_move.gcode_position.x}
+input_min: {printer.toolhead.axis_minimum.x}
+input_max: {printer.toolhead.axis_maximum.x}
+input_step: 0.1
+gcode:
+    SAVE_GCODE_STATE NAME=__move__axis
+    G90
+    G1 X{menu.input}
+    RESTORE_GCODE_STATE NAME=__move__axis
+
+[menu __main __control __move_01mm __axis_y]
+type: input
+name: Move Y:{'%05.1f' % menu.input}
+input: {printer.gcode_move.gcode_position.y}
+input_min: {printer.toolhead.axis_minimum.y}
+input_max: {printer.toolhead.axis_maximum.y}
+input_step: 0.1
+gcode:
+    SAVE_GCODE_STATE NAME=__move__axis
+    G90
+    G1 Y{menu.input}
+    RESTORE_GCODE_STATE NAME=__move__axis
+
+[menu __main __control __move_01mm __axis_z]
+type: input
+enable: {not printer.idle_timeout.state == "Printing"}
+name: Move Z:{'%05.1f' % menu.input}
+input: {printer.gcode_move.gcode_position.z}
+input_min: 0
+input_max: {printer.toolhead.axis_maximum.z}
+input_step: 0.1
+gcode:
+    SAVE_GCODE_STATE NAME=__move__axis
+    G90
+    G1 Z{menu.input}
+    RESTORE_GCODE_STATE NAME=__move__axis
+
+[menu __main __control __move_01mm __axis_e]
+type: input
+enable: {not printer.idle_timeout.state == "Printing"}
+name: Move E:{'%+06.1f' % menu.input}
+input: 0
+input_min: -{printer.configfile.config.extruder.max_extrude_only_distance|default(50)}
+input_max: {printer.configfile.config.extruder.max_extrude_only_distance|default(50)}
+input_step: 0.1
+gcode:
+    SAVE_GCODE_STATE NAME=__move__axis
+    M83
+    G1 E{menu.input} F240
+    RESTORE_GCODE_STATE NAME=__move__axis
+
+### menu temperature ###
+[menu __main __temp]
+type: list
+name: Temperature
+
+[menu __main __temp __hotend0_target]
+type: input
+enable: {('extruder' in printer) and ('extruder' in printer.heaters.available_heaters)}
+name: {"Ex0:%3.0f (%4.0f)" % (menu.input, printer.extruder.temperature)}
+input: {printer.extruder.target}
+input_min: 0
+input_max: {printer.configfile.config.extruder.max_temp}
+input_step: 1
+gcode: M104 T0 S{'%.0f' % menu.input}
+
+[menu __main __temp __hotend1_target]
+type: input
+enable: {('extruder1' in printer) and ('extruder1' in printer.heaters.available_heaters)}
+name: {"Ex1:%3.0f (%4.0f)" % (menu.input, printer.extruder1.temperature)}
+input: {printer.extruder1.target}
+input_min: 0
+input_max: {printer.configfile.config.extruder1.max_temp}
+input_step: 1
+gcode: M104 T1 S{'%.0f' % menu.input}
+
+[menu __main __temp __hotbed_target]
+type: input
+enable: {'heater_bed' in printer}
+name: {"Bed:%3.0f (%4.0f)" % (menu.input, printer.heater_bed.temperature)}
+input: {printer.heater_bed.target}
+input_min: 0
+input_max: {printer.configfile.config.heater_bed.max_temp}
+input_step: 1
+gcode: M140 S{'%.0f' % menu.input}
+
+[menu __main __temp __preheat_pla]
+type: list
+name: Preheat PLA
+
+[menu __main __temp __preheat_pla __all]
+type: command
+enable: {('extruder' in printer) and ('heater_bed' in printer)}
+name: Preheat all
+gcode:
+    M140 S60
+    M104 S200
+
+[menu __main __temp __preheat_pla __hotend]
+type: command
+enable: {'extruder' in printer}
+name: Preheat hotend
+gcode: M104 S200
+
+[menu __main __temp __preheat_pla __hotbed]
+type: command
+enable: {'heater_bed' in printer}
+name: Preheat hotbed
+gcode: M140 S60
+
+[menu __main __temp __preheat_abs]
+type: list
+name: Preheat ABS
+
+[menu __main __temp __preheat_abs __all]
+type: command
+enable: {('extruder' in printer) and ('heater_bed' in printer)}
+name: Preheat all
+gcode:
+    M140 S110
+    M104 S245
+
+[menu __main __temp __preheat_abs __hotend]
+type: command
+enable: {'extruder' in printer}
+name: Preheat hotend
+gcode: M104 S245
+
+[menu __main __temp __preheat_abs __hotbed]
+type: command
+enable: {'heater_bed' in printer}
+name: Preheat hotbed
+gcode: M140 S110
+
+[menu __main __temp __cooldown]
+type: list
+name: Cooldown
+
+[menu __main __temp __cooldown __all]
+type: command
+enable: {('extruder' in printer) and ('heater_bed' in printer)}
+name: Cooldown all
+gcode:
+    M104 S0
+    M140 S0
+
+[menu __main __temp __cooldown __hotend]
+type: command
+enable: {'extruder' in printer}
+name: Cooldown hotend
+gcode: M104 S0
+
+[menu __main __temp __cooldown __hotbed]
+type: command
+enable: {'heater_bed' in printer}
+name: Cooldown hotbed
+gcode: M140 S0
+
+### menu filament ###
+
+[menu __main __filament]
+type: list
+name: Filament
+
+[menu __main __filament __hotend0_target]
+type: input
+enable: {'extruder' in printer}
+name: {"Ex0:%3.0f (%4.0f)" % (menu.input, printer.extruder.temperature)}
+input: {printer.extruder.target}
+input_min: 0
+input_max: {printer.configfile.config.extruder.max_temp}
+input_step: 1
+gcode: M104 T0 S{'%.0f' % menu.input}
+
+[menu __main __filament __loadf]
+type: command
+name: Load Fil. fast
+gcode:
+    SAVE_GCODE_STATE NAME=__filament__load
+    M83
+    G1 E50 F960
+    RESTORE_GCODE_STATE NAME=__filament__load
+
+[menu __main __filament __loads]
+type: command
+name: Load Fil. slow
+gcode:
+    SAVE_GCODE_STATE NAME=__filament__load
+    M83
+    G1 E50 F240
+    RESTORE_GCODE_STATE NAME=__filament__load
+
+[menu __main __filament __unloadf]
+type: command
+name: Unload Fil.fast
+gcode:
+    SAVE_GCODE_STATE NAME=__filament__load
+    M83
+    G1 E-50 F960
+    RESTORE_GCODE_STATE NAME=__filament__load
+
+[menu __main __filament __unloads]
+type: command
+name: Unload Fil.slow
+gcode:
+    SAVE_GCODE_STATE NAME=__filament__load
+    M83
+    G1 E-50 F240
+    RESTORE_GCODE_STATE NAME=__filament__load
+
+[menu __main __filament __feed]
+type: input
+name: Feed: {'%.1f' % menu.input}
+input: 5
+input_step: 0.1
+gcode:
+    SAVE_GCODE_STATE NAME=__filament__load
+    M83
+    G1 E{'%.1f' % menu.input} F60
+    RESTORE_GCODE_STATE NAME=__filament__load
+
+### menu setup ###
+[menu __main __setup]
+type: list
+enable: {not printer.idle_timeout.state == "Printing"}
+name: Setup
+
+[menu __main __setup __save_config]
+type: command
+name: Save config
+gcode: SAVE_CONFIG
+
+[menu __main __setup __restart]
+type: list
+name: Restart
+
+[menu __main __setup __restart __host_restart]
+type: command
+enable: {not printer.idle_timeout.state == "Printing"}
+name: Restart host
+gcode: RESTART
+
+[menu __main __setup __restart __firmware_restart]
+type: command
+enable: {not printer.idle_timeout.state == "Printing"}
+name: Restart FW
+gcode: FIRMWARE_RESTART
+
+[menu __main __setup __tuning]
+type: list
+name: PID tuning
+
+[menu __main __setup __tuning __hotend_pid_tuning]
+type: command
+enable: {(not printer.idle_timeout.state == "Printing") and ('extruder' in printer)}
+name: Tune Hotend PID
+gcode: PID_CALIBRATE HEATER=extruder TARGET=210 WRITE_FILE=1
+
+[menu __main __setup __tuning __hotbed_pid_tuning]
+type: command
+enable: {(not printer.idle_timeout.state == "Printing") and ('heater_bed' in printer)}
+name: Tune Hotbed PID
+gcode: PID_CALIBRATE HEATER=heater_bed TARGET=60 WRITE_FILE=1
+
+[menu __main __setup __calib]
+type: list
+name: Calibration
+
+[menu __main __setup __calib __delta_calib_auto]
+type: command
+enable: {not printer.idle_timeout.state == "Printing"}
+name: Delta cal. auto
+gcode:
+    G28
+    DELTA_CALIBRATE
+
+[menu __main __setup __calib __delta_calib_man]
+type: list
+enable: {not printer.idle_timeout.state == "Printing"}
+name: Delta cal. man
+
+[menu __main __setup __calib __bedprobe]
+type: command
+enable: {not printer.idle_timeout.state == "Printing"}
+name: Bed probe
+gcode: PROBE
+
+[menu __main __setup __calib __delta_calib_man __start]
+type: command
+name: Start probing
+gcode:
+    G28
+    DELTA_CALIBRATE METHOD=manual
+
+[menu __main __setup __calib __delta_calib_man __move_z]
+type: input
+name: Move Z: {'%03.2f' % menu.input}
+input: {printer.gcode_move.gcode_position.z}
+input_step: 1
+realtime: True
+gcode:
+    {%- if menu.event == 'change' -%}
+        G1 Z{'%.2f' % menu.input}
+    {%- elif menu.event == 'long_click' -%}
+        G1 Z{'%.2f' % menu.input}
+        SAVE_GCODE_STATE NAME=__move__axis
+        G91
+        G1 Z2
+        G1 Z-2
+        RESTORE_GCODE_STATE NAME=__move__axis
+    {%- endif -%}
+
+[menu __main __setup __calib __delta_calib_man __test_z]
+type: input
+name: Test Z: {['++','+','+.01','+.05','+.1','+.5','-.5','-.1','-.05','-.01','-','--'][menu.input|int]}
+input: 6
+input_min: 0
+input_max: 11
+input_step: 1
+gcode:
+    {%- if menu.event == 'long_click' -%}
+        TESTZ Z={['++','+','+.01','+.05','+.1','+.5','-.5','-.1','-.05','-.01','-','--'][menu.input|int]}
+    {%- endif -%}
+
+[menu __main __setup __calib __delta_calib_man __accept]
+type: command
+name: Accept
+gcode: ACCEPT
+
+[menu __main __setup __calib __delta_calib_man __abort]
+type: command
+name: Abort
+gcode: ABORT
+
+[menu __main __setup __dump]
+type: command
+name: Dump parameters
+gcode:
+   {% for name1 in printer %}
+      {% for name2 in printer[name1] %}
+         { action_respond_info("printer['%s'].%s = %s"
+                               % (name1, name2, printer[name1][name2])) }
+      {% else %}
+         { action_respond_info("printer['%s'] = %s" % (name1, printer[name1])) }
+      {% endfor %}
+   {% endfor %}

klippy/extras/display/menu_keys.py

@@ -0,0 +1,108 @@
+# -*- coding: utf-8 -*-
+# Support for menu button press tracking
+#
+# Copyright (C) 2018  Janar Sööt <janar.soot@gmail.com>
+# Copyright (C) 2020  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+
+LONG_PRESS_DURATION = 0.800
+TIMER_DELAY = .200
+
+class MenuKeys:
+    def __init__(self, config, callback):
+        self.printer = config.get_printer()
+        self.reactor = self.printer.get_reactor()
+        self.callback = callback
+        buttons = self.printer.load_object(config, "buttons")
+        # Register rotary encoder
+        encoder_pins = config.get('encoder_pins', None)
+        encoder_steps_per_detent = config.getchoice('encoder_steps_per_detent',
+                                                    {2: 2, 4: 4}, 4)
+        if encoder_pins is not None:
+            try:
+                pin1, pin2 = encoder_pins.split(',')
+            except:
+                raise config.error("Unable to parse encoder_pins")
+            buttons.register_rotary_encoder(pin1.strip(), pin2.strip(),
+                                            self.encoder_cw_callback,
+                                            self.encoder_ccw_callback,
+                                            encoder_steps_per_detent)
+        self.encoder_fast_rate = config.getfloat('encoder_fast_rate',
+                                                 .030, above=0.)
+        self.last_encoder_cw_eventtime = 0
+        self.last_encoder_ccw_eventtime = 0
+        # Register click button
+        self.is_short_click = False
+        self.click_timer = self.reactor.register_timer(self.long_click_event)
+        self.register_button(config, 'click_pin', self.click_callback, False)
+        # Register other buttons
+        self.register_button(config, 'back_pin', self.back_callback)
+        self.register_button(config, 'up_pin', self.up_callback)
+        self.register_button(config, 'down_pin', self.down_callback)
+        self.register_button(config, 'kill_pin', self.kill_callback)
+
+    def register_button(self, config, name, callback, push_only=True):
+        pin = config.get(name, None)
+        if pin is None:
+            return
+        buttons = self.printer.lookup_object("buttons")
+        if config.get('analog_range_' + name, None) is None:
+            if push_only:
+                buttons.register_button_push(pin, callback)
+            else:
+                buttons.register_buttons([pin], callback)
+            return
+        amin, amax = config.getfloatlist('analog_range_' + name, count=2)
+        pullup = config.getfloat('analog_pullup_resistor', 4700., above=0.)
+        if push_only:
+            buttons.register_adc_button_push(pin, amin, amax, pullup, callback)
+        else:
+            buttons.register_adc_button(pin, amin, amax, pullup, callback)
+
+    # Rotary encoder callbacks
+    def encoder_cw_callback(self, eventtime):
+        fast_rate = ((eventtime - self.last_encoder_cw_eventtime)
+                     <= self.encoder_fast_rate)
+        self.last_encoder_cw_eventtime = eventtime
+        if fast_rate:
+            self.callback('fast_up', eventtime)
+        else:
+            self.callback('up', eventtime)
+
+    def encoder_ccw_callback(self, eventtime):
+        fast_rate = ((eventtime - self.last_encoder_ccw_eventtime)
+                     <= self.encoder_fast_rate)
+        self.last_encoder_ccw_eventtime = eventtime
+        if fast_rate:
+            self.callback('fast_down', eventtime)
+        else:
+            self.callback('down', eventtime)
+
+    # Click handling
+    def long_click_event(self, eventtime):
+        self.is_short_click = False
+        self.callback('long_click', eventtime)
+        return self.reactor.NEVER
+
+    def click_callback(self, eventtime, state):
+        if state:
+            self.is_short_click = True
+            self.reactor.update_timer(self.click_timer,
+                                      eventtime + LONG_PRESS_DURATION)
+        elif self.is_short_click:
+            self.reactor.update_timer(self.click_timer, self.reactor.NEVER)
+            self.callback('click', eventtime)
+
+    # Other button callbacks
+    def back_callback(self, eventtime):
+        self.callback('back', eventtime)
+
+    def up_callback(self, eventtime):
+        self.callback('up', eventtime)
+
+    def down_callback(self, eventtime):
+        self.callback('down', eventtime)
+
+    def kill_callback(self, eventtime):
+        self.printer.invoke_shutdown("Shutdown due to kill button!")

klippy/extras/display/st7920.py

@@ -0,0 +1,257 @@
+# Support for ST7920 (128x64 graphics) LCD displays
+#
+# Copyright (C) 2018  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import logging
+from .. import bus
+from . import font8x14
+
+BACKGROUND_PRIORITY_CLOCK = 0x7fffffff00000000
+
+# Spec says 72us, but faster is possible in practice
+ST7920_CMD_DELAY  = .000020
+ST7920_SYNC_DELAY = .000045
+
+TextGlyphs = { 'right_arrow': b'\x1a' }
+CharGlyphs = { 'degrees': bytearray(font8x14.VGA_FONT[0xf8]) }
+
+class DisplayBase:
+    def __init__(self):
+        # framebuffers
+        self.text_framebuffer = bytearray(b' '*64)
+        self.glyph_framebuffer = bytearray(128)
+        self.graphics_framebuffers = [bytearray(32) for i in range(32)]
+        self.all_framebuffers = [
+            # Text framebuffer
+            (self.text_framebuffer, bytearray(b'~'*64), 0x80),
+            # Glyph framebuffer
+            (self.glyph_framebuffer, bytearray(b'~'*128), 0x40),
+            # Graphics framebuffers
+            ] + [(self.graphics_framebuffers[i], bytearray(b'~'*32), i)
+                 for i in range(32)]
+        self.cached_glyphs = {}
+        self.icons = {}
+    def flush(self):
+        # Find all differences in the framebuffers and send them to the chip
+        for new_data, old_data, fb_id in self.all_framebuffers:
+            if new_data == old_data:
+                continue
+            # Find the position of all changed bytes in this framebuffer
+            diffs = [[i, 1] for i, (n, o) in enumerate(zip(new_data, old_data))
+                     if n != o]
+            # Batch together changes that are close to each other
+            for i in range(len(diffs)-2, -1, -1):
+                pos, count = diffs[i]
+                nextpos, nextcount = diffs[i+1]
+                if pos + 5 >= nextpos and nextcount < 16:
+                    diffs[i][1] = nextcount + (nextpos - pos)
+                    del diffs[i+1]
+            # Transmit changes
+            for pos, count in diffs:
+                count += pos & 0x01
+                count += count & 0x01
+                pos = pos & ~0x01
+                chip_pos = pos >> 1
+                if fb_id < 0x40:
+                    # Graphics framebuffer update
+                    self.send([0x80 + fb_id, 0x80 + chip_pos], is_extended=True)
+                else:
+                    self.send([fb_id + chip_pos])
+                self.send(new_data[pos:pos+count], is_data=True)
+            old_data[:] = new_data
+    def init(self):
+        cmds = [0x24, # Enter extended mode
+                0x40, # Clear vertical scroll address
+                0x02, # Enable CGRAM access
+                0x26, # Enable graphics
+                0x22, # Leave extended mode
+                0x02, # Home the display
+                0x06, # Set positive update direction
+                0x0c] # Enable display and hide cursor
+        self.send(cmds)
+        self.flush()
+    def cache_glyph(self, glyph_name, base_glyph_name, glyph_id):
+        icon = self.icons.get(glyph_name)
+        base_icon = self.icons.get(base_glyph_name)
+        if icon is None or base_icon is None:
+            return
+        all_bits = zip(icon[0], icon[1], base_icon[0], base_icon[1])
+        for i, (ic1, ic2, b1, b2) in enumerate(all_bits):
+            x1, x2 = ic1 ^ b1, ic2 ^ b2
+            pos = glyph_id*32 + i*2
+            self.glyph_framebuffer[pos:pos+2] = [x1, x2]
+            self.all_framebuffers[1][1][pos:pos+2] = [x1 ^ 1, x2 ^ 1]
+        self.cached_glyphs[glyph_name] = (base_glyph_name, (0, glyph_id*2))
+    def set_glyphs(self, glyphs):
+        for glyph_name, glyph_data in glyphs.items():
+            icon = glyph_data.get('icon16x16')
+            if icon is not None:
+                self.icons[glyph_name] = icon
+        # Setup animated glyphs
+        self.cache_glyph('fan2', 'fan1', 0)
+        self.cache_glyph('bed_heat2', 'bed_heat1', 1)
+    def write_text(self, x, y, data):
+        if x + len(data) > 16:
+            data = data[:16 - min(x, 16)]
+        pos = [0, 32, 16, 48][y] + x
+        self.text_framebuffer[pos:pos+len(data)] = data
+    def write_graphics(self, x, y, data):
+        if x >= 16 or y >= 4 or len(data) != 16:
+            return
+        gfx_fb = y * 16
+        if gfx_fb >= 32:
+            gfx_fb -= 32
+            x += 16
+        for i, bits in enumerate(data):
+            self.graphics_framebuffers[gfx_fb + i][x] = bits
+    def write_glyph(self, x, y, glyph_name):
+        glyph_id = self.cached_glyphs.get(glyph_name)
+        if glyph_id is not None and x & 1 == 0:
+            # Render cached icon using character generator
+            glyph_name = glyph_id[0]
+            self.write_text(x, y, glyph_id[1])
+        icon = self.icons.get(glyph_name)
+        if icon is not None:
+            # Draw icon in graphics mode
+            self.write_graphics(x, y, icon[0])
+            self.write_graphics(x + 1, y, icon[1])
+            return 2
+        char = TextGlyphs.get(glyph_name)
+        if char is not None:
+            # Draw character
+            self.write_text(x, y, char)
+            return 1
+        font = CharGlyphs.get(glyph_name)
+        if font is not None:
+            # Draw single width character
+            self.write_graphics(x, y, font)
+            return 1
+        return 0
+    def clear(self):
+        self.text_framebuffer[:] = b' '*64
+        zeros = bytearray(32)
+        for gfb in self.graphics_framebuffers:
+            gfb[:] = zeros
+    def get_dimensions(self):
+        return (16, 4)
+
+# Display driver for stock ST7920 displays
+class ST7920(DisplayBase):
+    def __init__(self, config):
+        printer = config.get_printer()
+        # pin config
+        ppins = printer.lookup_object('pins')
+        pins = [ppins.lookup_pin(config.get(name + '_pin'))
+                for name in ['cs', 'sclk', 'sid']]
+        mcu = None
+        for pin_params in pins:
+            if mcu is not None and pin_params['chip'] != mcu:
+                raise ppins.error("st7920 all pins must be on same mcu")
+            mcu = pin_params['chip']
+        self.pins = [pin_params['pin'] for pin_params in pins]
+        # prepare send functions
+        self.mcu = mcu
+        self.oid = self.mcu.create_oid()
+        self.mcu.register_config_callback(self.build_config)
+        self.send_data_cmd = self.send_cmds_cmd = None
+        self.is_extended = False
+        # init display base
+        DisplayBase.__init__(self)
+    def build_config(self):
+        # configure send functions
+        self.mcu.add_config_cmd(
+            "config_st7920 oid=%u cs_pin=%s sclk_pin=%s sid_pin=%s"
+            " sync_delay_ticks=%d cmd_delay_ticks=%d" % (
+                self.oid, self.pins[0], self.pins[1], self.pins[2],
+                self.mcu.seconds_to_clock(ST7920_SYNC_DELAY),
+                self.mcu.seconds_to_clock(ST7920_CMD_DELAY)))
+        cmd_queue = self.mcu.alloc_command_queue()
+        self.send_cmds_cmd = self.mcu.lookup_command(
+            "st7920_send_cmds oid=%c cmds=%*s", cq=cmd_queue)
+        self.send_data_cmd = self.mcu.lookup_command(
+            "st7920_send_data oid=%c data=%*s", cq=cmd_queue)
+    def send(self, cmds, is_data=False, is_extended=False):
+        cmd_type = self.send_cmds_cmd
+        if is_data:
+            cmd_type = self.send_data_cmd
+        elif self.is_extended != is_extended:
+            add_cmd = 0x22
+            if is_extended:
+                add_cmd = 0x26
+            cmds = [add_cmd] + cmds
+            self.is_extended = is_extended
+        cmd_type.send([self.oid, cmds], reqclock=BACKGROUND_PRIORITY_CLOCK)
+        #logging.debug("st7920 %d %s", is_data, repr(cmds))
+
+# Helper code for toggling the en pin on startup
+class EnableHelper:
+    def __init__(self, pin_desc, spi):
+        self.en_pin = bus.MCU_bus_digital_out(spi.get_mcu(), pin_desc,
+                                              spi.get_command_queue())
+    def init(self):
+        mcu = self.en_pin.get_mcu()
+        curtime = mcu.get_printer().get_reactor().monotonic()
+        print_time = mcu.estimated_print_time(curtime)
+        # Toggle enable pin
+        minclock = mcu.print_time_to_clock(print_time + .100)
+        self.en_pin.update_digital_out(0, minclock=minclock)
+        minclock = mcu.print_time_to_clock(print_time + .200)
+        self.en_pin.update_digital_out(1, minclock=minclock)
+        # Force a delay to any subsequent commands on the command queue
+        minclock = mcu.print_time_to_clock(print_time + .300)
+        self.en_pin.update_digital_out(1, minclock=minclock)
+
+# Display driver for displays that emulate the ST7920 in software.
+# These displays rely on the CS pin to be toggled in order to initialize the
+# SPI correctly. This display driver uses a software SPI with an unused pin
+# as the MISO pin.
+class EmulatedST7920(DisplayBase):
+    def __init__(self, config):
+        # create software spi
+        ppins = config.get_printer().lookup_object('pins')
+        sw_pin_names = ['spi_software_%s_pin' % (name,)
+                        for name in ['miso', 'mosi', 'sclk']]
+        sw_pin_params = [ppins.lookup_pin(config.get(name), share_type=name)
+                         for name in sw_pin_names]
+        mcu = None
+        for pin_params in sw_pin_params:
+            if mcu is not None and pin_params['chip'] != mcu:
+                raise ppins.error("%s: spi pins must be on same mcu" % (
+                    config.get_name(),))
+            mcu = pin_params['chip']
+        sw_pins = tuple([pin_params['pin'] for pin_params in sw_pin_params])
+        speed = config.getint('spi_speed', 1000000, minval=100000)
+        self.spi = bus.MCU_SPI(mcu, None, None, 0, speed, sw_pins)
+        # create enable helper
+        self.en_helper = EnableHelper(config.get("en_pin"), self.spi)
+        self.en_set = False
+        # init display base
+        self.is_extended = False
+        DisplayBase.__init__(self)
+    def send(self, cmds, is_data=False, is_extended=False):
+        # setup sync byte and check for exten mode switch
+        sync_byte = 0xfa
+        if not is_data:
+            sync_byte = 0xf8
+            if self.is_extended != is_extended:
+                add_cmd = 0x22
+                if is_extended:
+                    add_cmd = 0x26
+                cmds = [add_cmd] + cmds
+                self.is_extended = is_extended
+        # copy data to ST7920 data format
+        spi_data = [0] * (2 * len(cmds) + 1)
+        spi_data[0] = sync_byte
+        i = 1
+        for b in cmds:
+            spi_data[i] = b & 0xF0
+            spi_data[i + 1] = (b & 0x0F) << 4
+            i = i + 2
+        # check if enable pin has been set
+        if not self.en_set:
+            self.en_helper.init()
+            self.en_set = True
+        # send data
+        self.spi.spi_send(spi_data, reqclock=BACKGROUND_PRIORITY_CLOCK)
+        #logging.debug("st7920 %s", repr(spi_data))

klippy/extras/display/uc1701.py

@@ -0,0 +1,240 @@
+# Support for UC1701 (and similar) 128x64 graphics LCD displays
+#
+# Copyright (C) 2018-2019  Kevin O'Connor <kevin@koconnor.net>
+# Copyright (C) 2018  Eric Callahan  <arksine.code@gmail.com>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import logging
+from .. import bus
+from . import font8x14
+
+BACKGROUND_PRIORITY_CLOCK = 0x7fffffff00000000
+
+TextGlyphs = { 'right_arrow': b'\x1a', 'degrees': b'\xf8' }
+
+class DisplayBase:
+    def __init__(self, io, columns=128, x_offset=0):
+        self.send = io.send
+        # framebuffers
+        self.columns = columns
+        self.x_offset = x_offset
+        self.vram = [bytearray(self.columns) for i in range(8)]
+        self.all_framebuffers = [(self.vram[i], bytearray(b'~'*self.columns), i)
+                                 for i in range(8)]
+        # Cache fonts and icons in display byte order
+        self.font = [self._swizzle_bits(bytearray(c))
+                     for c in font8x14.VGA_FONT]
+        self.icons = {}
+    def flush(self):
+        # Find all differences in the framebuffers and send them to the chip
+        for new_data, old_data, page in self.all_framebuffers:
+            if new_data == old_data:
+                continue
+            # Find the position of all changed bytes in this framebuffer
+            diffs = [[i, 1] for i, (n, o) in enumerate(zip(new_data, old_data))
+                     if n != o]
+            # Batch together changes that are close to each other
+            for i in range(len(diffs)-2, -1, -1):
+                pos, count = diffs[i]
+                nextpos, nextcount = diffs[i+1]
+                if pos + 5 >= nextpos and nextcount < 16:
+                    diffs[i][1] = nextcount + (nextpos - pos)
+                    del diffs[i+1]
+            # Transmit changes
+            for col_pos, count in diffs:
+                # Set Position registers
+                ra = 0xb0 | (page & 0x0F)
+                ca_msb = 0x10 | ((col_pos >> 4) & 0x0F)
+                ca_lsb = col_pos & 0x0F
+                self.send([ra, ca_msb, ca_lsb])
+                # Send Data
+                self.send(new_data[col_pos:col_pos+count], is_data=True)
+            old_data[:] = new_data
+    def _swizzle_bits(self, data):
+        # Convert from "rows of pixels" format to "columns of pixels"
+        top = bot = 0
+        for row in range(8):
+            spaced = (data[row] * 0x8040201008040201) & 0x8080808080808080
+            top |= spaced >> (7 - row)
+            spaced = (data[row + 8] * 0x8040201008040201) & 0x8080808080808080
+            bot |= spaced >> (7 - row)
+        bits_top = [(top >> s) & 0xff for s in range(0, 64, 8)]
+        bits_bot = [(bot >> s) & 0xff for s in range(0, 64, 8)]
+        return (bytearray(bits_top), bytearray(bits_bot))
+    def set_glyphs(self, glyphs):
+        for glyph_name, glyph_data in glyphs.items():
+            icon = glyph_data.get('icon16x16')
+            if icon is not None:
+                top1, bot1 = self._swizzle_bits(icon[0])
+                top2, bot2 = self._swizzle_bits(icon[1])
+                self.icons[glyph_name] = (top1 + top2, bot1 + bot2)
+    def write_text(self, x, y, data):
+        if x + len(data) > 16:
+            data = data[:16 - min(x, 16)]
+        pix_x = x * 8
+        pix_x += self.x_offset
+        page_top = self.vram[y * 2]
+        page_bot = self.vram[y * 2 + 1]
+        for c in bytearray(data):
+            bits_top, bits_bot = self.font[c]
+            page_top[pix_x:pix_x+8] = bits_top
+            page_bot[pix_x:pix_x+8] = bits_bot
+            pix_x += 8
+    def write_graphics(self, x, y, data):
+        if x >= 16 or y >= 4 or len(data) != 16:
+            return
+        bits_top, bits_bot = self._swizzle_bits(data)
+        pix_x = x * 8
+        pix_x += self.x_offset
+        page_top = self.vram[y * 2]
+        page_bot = self.vram[y * 2 + 1]
+        for i in range(8):
+            page_top[pix_x + i] ^= bits_top[i]
+            page_bot[pix_x + i] ^= bits_bot[i]
+    def write_glyph(self, x, y, glyph_name):
+        icon = self.icons.get(glyph_name)
+        if icon is not None and x < 15:
+            # Draw icon in graphics mode
+            pix_x = x * 8
+            pix_x += self.x_offset
+            page_idx = y * 2
+            self.vram[page_idx][pix_x:pix_x+16] = icon[0]
+            self.vram[page_idx + 1][pix_x:pix_x+16] = icon[1]
+            return 2
+        char = TextGlyphs.get(glyph_name)
+        if char is not None:
+            # Draw character
+            self.write_text(x, y, char)
+            return 1
+        return 0
+    def clear(self):
+        zeros = bytearray(self.columns)
+        for page in self.vram:
+            page[:] = zeros
+    def get_dimensions(self):
+        return (16, 4)
+
+# IO wrapper for "4 wire" spi bus (spi bus with an extra data/control line)
+class SPI4wire:
+    def __init__(self, config, data_pin_name):
+        self.spi = bus.MCU_SPI_from_config(config, 0, default_speed=10000000)
+        dc_pin = config.get(data_pin_name)
+        self.mcu_dc = bus.MCU_bus_digital_out(self.spi.get_mcu(), dc_pin,
+                                              self.spi.get_command_queue())
+    def send(self, cmds, is_data=False):
+        self.mcu_dc.update_digital_out(is_data,
+                                       reqclock=BACKGROUND_PRIORITY_CLOCK)
+        self.spi.spi_send(cmds, reqclock=BACKGROUND_PRIORITY_CLOCK)
+
+# IO wrapper for i2c bus
+class I2C:
+    def __init__(self, config, default_addr):
+        self.i2c = bus.MCU_I2C_from_config(config, default_addr=default_addr,
+                                           default_speed=400000)
+    def send(self, cmds, is_data=False):
+        if is_data:
+            hdr = 0x40
+        else:
+            hdr = 0x00
+        cmds = bytearray(cmds)
+        cmds.insert(0, hdr)
+        self.i2c.i2c_write(cmds, reqclock=BACKGROUND_PRIORITY_CLOCK)
+
+# Helper code for toggling a reset pin on startup
+class ResetHelper:
+    def __init__(self, pin_desc, io_bus):
+        self.mcu_reset = None
+        if pin_desc is None:
+            return
+        self.mcu_reset = bus.MCU_bus_digital_out(io_bus.get_mcu(), pin_desc,
+                                                 io_bus.get_command_queue())
+    def init(self):
+        if self.mcu_reset is None:
+            return
+        mcu = self.mcu_reset.get_mcu()
+        curtime = mcu.get_printer().get_reactor().monotonic()
+        print_time = mcu.estimated_print_time(curtime)
+        # Toggle reset
+        minclock = mcu.print_time_to_clock(print_time + .100)
+        self.mcu_reset.update_digital_out(0, minclock=minclock)
+        minclock = mcu.print_time_to_clock(print_time + .200)
+        self.mcu_reset.update_digital_out(1, minclock=minclock)
+        # Force a delay to any subsequent commands on the command queue
+        minclock = mcu.print_time_to_clock(print_time + .300)
+        self.mcu_reset.update_digital_out(1, minclock=minclock)
+
+# The UC1701 is a "4-wire" SPI display device
+class UC1701(DisplayBase):
+    def __init__(self, config):
+        io = SPI4wire(config, "a0_pin")
+        DisplayBase.__init__(self, io)
+        self.contrast = config.getint('contrast', 40, minval=0, maxval=63)
+        self.reset = ResetHelper(config.get("rst_pin", None), io.spi)
+    def init(self):
+        self.reset.init()
+        init_cmds = [0xE2, # System reset
+                     0x40, # Set display to start at line 0
+                     0xA0, # Set SEG direction
+                     0xC8, # Set COM Direction
+                     0xA2, # Set Bias = 1/9
+                     0x2C, # Boost ON
+                     0x2E, # Voltage regulator on
+                     0x2F, # Voltage follower on
+                     0xF8, # Set booster ratio
+                     0x00, # Booster ratio value (4x)
+                     0x23, # Set resistor ratio (3)
+                     0x81, # Set Electronic Volume
+                     self.contrast, # Electronic Volume value
+                     0xAC, # Set static indicator off
+                     0x00, # NOP
+                     0xA6, # Disable Inverse
+                     0xAF] # Set display enable
+        self.send(init_cmds)
+        self.send([0xA5]) # display all
+        self.send([0xA4]) # normal display
+        self.flush()
+
+# The SSD1306 supports both i2c and "4-wire" spi
+class SSD1306(DisplayBase):
+    def __init__(self, config, columns=128, x_offset=0):
+        cs_pin = config.get("cs_pin", None)
+        if cs_pin is None:
+            io = I2C(config, 60)
+            io_bus = io.i2c
+        else:
+            io = SPI4wire(config, "dc_pin")
+            io_bus = io.spi
+        self.reset = ResetHelper(config.get("reset_pin", None), io_bus)
+        DisplayBase.__init__(self, io, columns, x_offset)
+        self.contrast = config.getint('contrast', 239, minval=0, maxval=255)
+        self.vcomh = config.getint('vcomh', 0, minval=0, maxval=63)
+        self.invert = config.getboolean('invert', False)
+    def init(self):
+        self.reset.init()
+        init_cmds = [
+            0xAE,       # Display off
+            0xD5, 0x80, # Set oscillator frequency
+            0xA8, 0x3f, # Set multiplex ratio
+            0xD3, 0x00, # Set display offset
+            0x40,       # Set display start line
+            0x8D, 0x14, # Charge pump setting
+            0x20, 0x02, # Set Memory addressing mode
+            0xA1,       # Set Segment re-map
+            0xC8,       # Set COM output scan direction
+            0xDA, 0x12, # Set COM pins hardware configuration
+            0x81, self.contrast, # Set contrast control
+            0xD9, 0xA1, # Set pre-charge period
+            0xDB, self.vcomh, # Set VCOMH deselect level
+            0x2E,       # Deactivate scroll
+            0xA4,       # Output ram to display
+            0xA7 if self.invert else 0xA6, # Set normal/invert
+            0xAF,       # Display on
+        ]
+        self.send(init_cmds)
+        self.flush()
+
+# the SH1106 is SSD1306 compatible with up to 132 columns
+class SH1106(SSD1306):
+    def __init__(self, config):
+        x_offset = config.getint('x_offset', 0, minval=0, maxval=3)
+        SSD1306.__init__(self, config, 132, x_offset=x_offset)

klippy/extras/display_status.py

@@ -0,0 +1,50 @@
+# Module to handle M73 and M117 display status commands
+#
+# Copyright (C) 2018-2020  Kevin O'Connor <kevin@koconnor.net>
+# Copyright (C) 2018  Eric Callahan <arksine.code@gmail.com>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+
+M73_TIMEOUT = 5.
+
+class DisplayStatus:
+    def __init__(self, config):
+        self.printer = config.get_printer()
+        self.expire_progress = 0.
+        self.progress = self.message = None
+        # Register commands
+        gcode = self.printer.lookup_object('gcode')
+        gcode.register_command('M73', self.cmd_M73)
+        gcode.register_command('M117', self.cmd_M117)
+        gcode.register_command(
+            'SET_DISPLAY_TEXT', self.cmd_SET_DISPLAY_TEXT,
+            desc=self.cmd_SET_DISPLAY_TEXT_help)
+    def get_status(self, eventtime):
+        progress = self.progress
+        if progress is not None and eventtime > self.expire_progress:
+            idle_timeout = self.printer.lookup_object('idle_timeout')
+            idle_timeout_info = idle_timeout.get_status(eventtime)
+            if idle_timeout_info['state'] != "Printing":
+                self.progress = progress = None
+        if progress is None:
+            progress = 0.
+            sdcard = self.printer.lookup_object('virtual_sdcard', None)
+            if sdcard is not None:
+                progress = sdcard.get_status(eventtime)['progress']
+        return { 'progress': progress, 'message': self.message }
+    def cmd_M73(self, gcmd):
+        progress = gcmd.get_float('P', None)
+        if progress is not None:
+            progress = progress / 100.
+            self.progress = min(1., max(0., progress))
+            curtime = self.printer.get_reactor().monotonic()
+            self.expire_progress = curtime + M73_TIMEOUT
+    def cmd_M117(self, gcmd):
+        msg = gcmd.get_raw_command_parameters() or None
+        self.message = msg
+    cmd_SET_DISPLAY_TEXT_help = "Set or clear the display message"
+    def cmd_SET_DISPLAY_TEXT(self, gcmd):
+        self.message = gcmd.get("MSG", None)
+
+def load_config(config):
+    return DisplayStatus(config)

klippy/extras/dotstar.py

@@ -0,0 +1,58 @@
+# Support for "dotstar" leds
+#
+# Copyright (C) 2019-2022  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+from . import bus
+
+BACKGROUND_PRIORITY_CLOCK = 0x7fffffff00000000
+
+class PrinterDotstar:
+    def __init__(self, config):
+        self.printer = printer = config.get_printer()
+        name = config.get_name().split()[1]
+        # Configure a software spi bus
+        ppins = printer.lookup_object('pins')
+        data_pin_params = ppins.lookup_pin(config.get('data_pin'))
+        clock_pin_params = ppins.lookup_pin(config.get('clock_pin'))
+        mcu = data_pin_params['chip']
+        if mcu is not clock_pin_params['chip']:
+            raise config.error("Dotstar pins must be on same mcu")
+        sw_spi_pins = (data_pin_params['pin'], data_pin_params['pin'],
+                       clock_pin_params['pin'])
+        self.spi = bus.MCU_SPI(mcu, None, None, 0, 500000, sw_spi_pins)
+        # Initialize color data
+        self.chain_count = config.getint('chain_count', 1, minval=1)
+        pled = printer.load_object(config, "led")
+        self.led_helper = pled.setup_helper(config, self.update_leds,
+                                            self.chain_count)
+        self.prev_data = None
+        # Register commands
+        printer.register_event_handler("klippy:connect", self.handle_connect)
+    def handle_connect(self):
+        self.update_leds(self.led_helper.get_status()['color_data'], None)
+    def update_leds(self, led_state, print_time):
+        if led_state == self.prev_data:
+            return
+        self.prev_data = led_state
+        # Build data to send
+        data = [0] * ((len(led_state) + 2) * 4)
+        for i, (red, green, blue, white) in enumerate(led_state):
+            idx = (i + 1) * 4
+            data[idx] = 0xff
+            data[idx+1] = int(blue * 255. + .5)
+            data[idx+2] = int(green * 255. + .5)
+            data[idx+3] = int(red * 255. + .5)
+        data[-4] = data[-3] = data[-2] = data[-1] = 0xff
+        # Transmit update
+        minclock = 0
+        if print_time is not None:
+            minclock = self.spi.get_mcu().print_time_to_clock(print_time)
+        for d in [data[i:i+20] for i in range(0, len(data), 20)]:
+            self.spi.spi_send(d, minclock=minclock,
+                              reqclock=BACKGROUND_PRIORITY_CLOCK)
+    def get_status(self, eventtime):
+        return self.led_helper.get_status(eventtime)
+
+def load_config_prefix(config):
+    return PrinterDotstar(config)

klippy/extras/ds18b20.py

@@ -0,0 +1,80 @@
+# Support for 1-wire based temperature sensors
+#
+# Copyright (C) 2020 Alan Lord <alanslists@gmail.com>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import logging
+import mcu
+
+DS18_REPORT_TIME = 3.0
+# Temperature can be sampled at any time but conversion time is ~750ms, so
+# setting the time too low will not make the reports come faster.
+DS18_MIN_REPORT_TIME = 1.0
+DS18_MAX_CONSECUTIVE_ERRORS = 4
+
+class DS18B20:
+    def __init__(self, config):
+        self.printer = config.get_printer()
+        self.name = config.get_name().split()[-1]
+        self.sensor_id = bytearray(config.get("serial_no").encode())
+        self.temp = self.min_temp = self.max_temp = 0.0
+        self._report_clock = 0
+        self.report_time = config.getfloat(
+            'ds18_report_time',
+            DS18_REPORT_TIME,
+            minval=DS18_MIN_REPORT_TIME
+        )
+        self._mcu = mcu.get_printer_mcu(self.printer, config.get('sensor_mcu'))
+        self.oid = self._mcu.create_oid()
+        self._mcu.register_response(self._handle_ds18b20_response,
+            "ds18b20_result", self.oid)
+        self._mcu.register_config_callback(self._build_config)
+
+    def _build_config(self):
+        sid = "".join(["%02x" % (x,) for x in self.sensor_id])
+        self._mcu.add_config_cmd(
+            "config_ds18b20 oid=%d serial=%s max_error_count=%d"
+            % (self.oid, sid, DS18_MAX_CONSECUTIVE_ERRORS))
+
+        clock = self._mcu.get_query_slot(self.oid)
+        self._report_clock = self._mcu.seconds_to_clock(self.report_time)
+        self._mcu.add_config_cmd("query_ds18b20 oid=%d clock=%u rest_ticks=%u"
+            " min_value=%d max_value=%d" % (
+                self.oid, clock, self._report_clock,
+                self.min_temp * 1000, self.max_temp * 1000), is_init=True)
+
+    def _handle_ds18b20_response(self, params):
+        temp = params['value'] / 1000.0
+
+        if params["fault"]:
+            logging.info("ds18b20 reports fault %d (temp=%0.1f)",
+                         params["fault"], temp)
+            return
+
+        next_clock      = self._mcu.clock32_to_clock64(params['next_clock'])
+        last_read_clock = next_clock - self._report_clock
+        last_read_time  = self._mcu.clock_to_print_time(last_read_clock)
+        self._callback(last_read_time, temp)
+
+    def setup_minmax(self, min_temp, max_temp):
+        self.min_temp = min_temp
+        self.max_temp = max_temp
+
+    def fault(self, msg):
+        self.printer.invoke_async_shutdown(msg)
+
+    def get_report_time_delta(self):
+        return self.report_time
+
+    def setup_callback(self, cb):
+        self._callback = cb
+
+    def get_status(self, eventtime):
+        return {
+            'temperature': round(self.temp, 2),
+        }
+
+def load_config(config):
+    # Register sensor
+    pheaters = config.get_printer().load_object(config, "heaters")
+    pheaters.add_sensor_factory("DS18B20", DS18B20)

klippy/extras/duplicate_pin_override.py

@@ -0,0 +1,15 @@
+# Tool to disable config checks for duplicate pins
+#
+# Copyright (C) 2021  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+
+class PrinterDupPinOverride:
+    def __init__(self, config):
+        printer = config.get_printer()
+        ppins = printer.lookup_object('pins')
+        for pin_desc in config.getlist('pins'):
+            ppins.allow_multi_use_pin(pin_desc)
+
+def load_config(config):
+    return PrinterDupPinOverride(config)

klippy/extras/endstop_phase.py

@@ -0,0 +1,231 @@
+# Endstop accuracy improvement via stepper phase tracking
+#
+# Copyright (C) 2016-2021  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import math, logging
+import stepper
+
+TRINAMIC_DRIVERS = ["tmc2130", "tmc2208", "tmc2209", "tmc2240", "tmc2660", "tmc5160"]
+
+# Calculate the trigger phase of a stepper motor
+class PhaseCalc:
+    def __init__(self, printer, name, phases=None):
+        self.printer = printer
+        self.name = name
+        self.phases = phases
+        self.tmc_module = None
+        # Statistics tracking for ENDSTOP_PHASE_CALIBRATE
+        self.phase_history = self.last_phase = self.last_mcu_position = None
+        self.is_primary = self.stats_only = False
+    def lookup_tmc(self):
+        for driver in TRINAMIC_DRIVERS:
+            driver_name = "%s %s" % (driver, self.name)
+            module = self.printer.lookup_object(driver_name, None)
+            if module is not None:
+                self.tmc_module = module
+                if self.phases is None:
+                    phase_offset, self.phases = module.get_phase_offset()
+                break
+        if self.phases is not None:
+            self.phase_history = [0] * self.phases
+    def convert_phase(self, driver_phase, driver_phases):
+        phases = self.phases
+        return (int(float(driver_phase) / driver_phases * phases + .5) % phases)
+    def calc_phase(self, stepper, trig_mcu_pos):
+        mcu_phase_offset = 0
+        if self.tmc_module is not None:
+            mcu_phase_offset, phases = self.tmc_module.get_phase_offset()
+            if mcu_phase_offset is None:
+                if self.printer.get_start_args().get('debugoutput') is None:
+                    raise self.printer.command_error("Stepper %s phase unknown"
+                                                     % (self.name,))
+                mcu_phase_offset = 0
+        phase = (trig_mcu_pos + mcu_phase_offset) % self.phases
+        self.phase_history[phase] += 1
+        self.last_phase = phase
+        self.last_mcu_position = trig_mcu_pos
+        return phase
+
+# Adjusted endstop trigger positions
+class EndstopPhase:
+    def __init__(self, config):
+        self.printer = config.get_printer()
+        self.name = config.get_name().split()[1]
+        # Obtain step_distance and microsteps from stepper config section
+        sconfig = config.getsection(self.name)
+        rotation_dist, steps_per_rotation = stepper.parse_step_distance(sconfig)
+        self.step_dist = rotation_dist / steps_per_rotation
+        self.phases = sconfig.getint("microsteps", note_valid=False) * 4
+        self.phase_calc = PhaseCalc(self.printer, self.name, self.phases)
+        # Register event handlers
+        self.printer.register_event_handler("klippy:connect",
+                                            self.phase_calc.lookup_tmc)
+        self.printer.register_event_handler("homing:home_rails_end",
+                                            self.handle_home_rails_end)
+        self.printer.load_object(config, "endstop_phase")
+        # Read config
+        self.endstop_phase = None
+        trigger_phase = config.get('trigger_phase', None)
+        if trigger_phase is not None:
+            p, ps = config.getintlist('trigger_phase', sep='/', count=2)
+            if p >= ps:
+                raise config.error("Invalid trigger_phase '%s'"
+                                   % (trigger_phase,))
+            self.endstop_phase = self.phase_calc.convert_phase(p, ps)
+        self.endstop_align_zero = config.getboolean('endstop_align_zero', False)
+        self.endstop_accuracy = config.getfloat('endstop_accuracy', None,
+                                                above=0.)
+        # Determine endstop accuracy
+        if self.endstop_accuracy is None:
+            self.endstop_phase_accuracy = self.phases//2 - 1
+        elif self.endstop_phase is not None:
+            self.endstop_phase_accuracy = int(
+                math.ceil(self.endstop_accuracy * .5 / self.step_dist))
+        else:
+            self.endstop_phase_accuracy = int(
+                math.ceil(self.endstop_accuracy / self.step_dist))
+        if self.endstop_phase_accuracy >= self.phases // 2:
+            raise config.error("Endstop for %s is not accurate enough for"
+                               " stepper phase adjustment" % (self.name,))
+        if self.printer.get_start_args().get('debugoutput') is not None:
+            self.endstop_phase_accuracy = self.phases
+    def align_endstop(self, rail):
+        if not self.endstop_align_zero or self.endstop_phase is None:
+            return 0.
+        # Adjust the endstop position so 0.0 is always at a full step
+        microsteps = self.phases // 4
+        half_microsteps = microsteps // 2
+        phase_offset = (((self.endstop_phase + half_microsteps) % microsteps)
+                        - half_microsteps) * self.step_dist
+        full_step = microsteps * self.step_dist
+        pe = rail.get_homing_info().position_endstop
+        return int(pe / full_step + .5) * full_step - pe + phase_offset
+    def get_homed_offset(self, stepper, trig_mcu_pos):
+        phase = self.phase_calc.calc_phase(stepper, trig_mcu_pos)
+        if self.endstop_phase is None:
+            logging.info("Setting %s endstop phase to %d", self.name, phase)
+            self.endstop_phase = phase
+            return 0.
+        delta = (phase - self.endstop_phase) % self.phases
+        if delta >= self.phases - self.endstop_phase_accuracy:
+            delta -= self.phases
+        elif delta > self.endstop_phase_accuracy:
+            raise self.printer.command_error(
+                "Endstop %s incorrect phase (got %d vs %d)" % (
+                    self.name, phase, self.endstop_phase))
+        return delta * self.step_dist
+    def handle_home_rails_end(self, homing_state, rails):
+        for rail in rails:
+            stepper = rail.get_steppers()[0]
+            if stepper.get_name() == self.name:
+                trig_mcu_pos = homing_state.get_trigger_position(self.name)
+                align = self.align_endstop(rail)
+                offset = self.get_homed_offset(stepper, trig_mcu_pos)
+                homing_state.set_stepper_adjustment(self.name, align + offset)
+                return
+
+# Support for ENDSTOP_PHASE_CALIBRATE command
+class EndstopPhases:
+    def __init__(self, config):
+        self.printer = config.get_printer()
+        self.tracking = {}
+        # Register handlers
+        self.printer.register_event_handler("homing:home_rails_end",
+                                            self.handle_home_rails_end)
+        self.gcode = self.printer.lookup_object('gcode')
+        self.gcode.register_command("ENDSTOP_PHASE_CALIBRATE",
+                                    self.cmd_ENDSTOP_PHASE_CALIBRATE,
+                                    desc=self.cmd_ENDSTOP_PHASE_CALIBRATE_help)
+    def update_stepper(self, stepper, trig_mcu_pos, is_primary):
+        stepper_name = stepper.get_name()
+        phase_calc = self.tracking.get(stepper_name)
+        if phase_calc is None:
+            # Check if stepper has an endstop_phase config section defined
+            mod_name = "endstop_phase %s" % (stepper_name,)
+            m = self.printer.lookup_object(mod_name, None)
+            if m is not None:
+                phase_calc = m.phase_calc
+            else:
+                # Create new PhaseCalc tracker
+                phase_calc = PhaseCalc(self.printer, stepper_name)
+                phase_calc.stats_only = True
+                phase_calc.lookup_tmc()
+            self.tracking[stepper_name] = phase_calc
+        if phase_calc.phase_history is None:
+            return
+        if is_primary:
+            phase_calc.is_primary = True
+        if phase_calc.stats_only:
+            phase_calc.calc_phase(stepper, trig_mcu_pos)
+    def handle_home_rails_end(self, homing_state, rails):
+        for rail in rails:
+            is_primary = True
+            for stepper in rail.get_steppers():
+                sname = stepper.get_name()
+                trig_mcu_pos = homing_state.get_trigger_position(sname)
+                self.update_stepper(stepper, trig_mcu_pos, is_primary)
+                is_primary = False
+    cmd_ENDSTOP_PHASE_CALIBRATE_help = "Calibrate stepper phase"
+    def cmd_ENDSTOP_PHASE_CALIBRATE(self, gcmd):
+        stepper_name = gcmd.get('STEPPER', None)
+        if stepper_name is None:
+            self.report_stats()
+            return
+        phase_calc = self.tracking.get(stepper_name)
+        if phase_calc is None or phase_calc.phase_history is None:
+            raise gcmd.error("Stats not available for stepper %s"
+                             % (stepper_name,))
+        endstop_phase, phases = self.generate_stats(stepper_name, phase_calc)
+        if not phase_calc.is_primary:
+            return
+        configfile = self.printer.lookup_object('configfile')
+        section = 'endstop_phase %s' % (stepper_name,)
+        configfile.remove_section(section)
+        configfile.set(section, "trigger_phase",
+                       "%s/%s" % (endstop_phase, phases))
+        gcmd.respond_info(
+            "The SAVE_CONFIG command will update the printer config\n"
+            "file with these parameters and restart the printer.")
+    def generate_stats(self, stepper_name, phase_calc):
+        phase_history = phase_calc.phase_history
+        wph = phase_history + phase_history
+        count = sum(phase_history)
+        phases = len(phase_history)
+        half_phases = phases // 2
+        res = []
+        for i in range(phases):
+            phase = i + half_phases
+            cost = sum([wph[j] * abs(j-phase) for j in range(i, i+phases)])
+            res.append((cost, phase))
+        res.sort()
+        best = res[0][1]
+        found = [j for j in range(best - half_phases, best + half_phases)
+                 if wph[j]]
+        best_phase = best % phases
+        lo, hi = found[0] % phases, found[-1] % phases
+        self.gcode.respond_info("%s: trigger_phase=%d/%d (range %d to %d)"
+                                % (stepper_name, best_phase, phases, lo, hi))
+        return best_phase, phases
+    def report_stats(self):
+        if not self.tracking:
+            self.gcode.respond_info(
+                "No steppers found. (Be sure to home at least once.)")
+            return
+        for stepper_name in sorted(self.tracking.keys()):
+            phase_calc = self.tracking[stepper_name]
+            if phase_calc is None or not phase_calc.is_primary:
+                continue
+            self.generate_stats(stepper_name, phase_calc)
+    def get_status(self, eventtime):
+        lh = { name: {'phase': pc.last_phase, 'phases': pc.phases,
+                      'mcu_position': pc.last_mcu_position}
+               for name, pc in self.tracking.items()
+               if pc.phase_history is not None }
+        return { 'last_home': lh }
+
+def load_config_prefix(config):
+    return EndstopPhase(config)
+
+def load_config(config):
+    return EndstopPhases(config)

klippy/extras/exclude_object.py

@@ -0,0 +1,302 @@
+# Exclude moves toward and inside objects
+#
+# Copyright (C) 2019  Eric Callahan <arksine.code@gmail.com>
+# Copyright (C) 2021  Troy Jacobson <troy.d.jacobson@gmail.com>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+
+import logging
+import json
+
+class ExcludeObject:
+    def __init__(self, config):
+        self.printer = config.get_printer()
+        self.gcode = self.printer.lookup_object('gcode')
+        self.gcode_move = self.printer.load_object(config, 'gcode_move')
+        self.printer.register_event_handler('klippy:connect',
+                                        self._handle_connect)
+        self.printer.register_event_handler("virtual_sdcard:reset_file",
+                                            self._reset_file)
+        self.next_transform = None
+        self.last_position_extruded = [0., 0., 0., 0.]
+        self.last_position_excluded = [0., 0., 0., 0.]
+
+        self._reset_state()
+        self.gcode.register_command(
+            'EXCLUDE_OBJECT_START', self.cmd_EXCLUDE_OBJECT_START,
+            desc=self.cmd_EXCLUDE_OBJECT_START_help)
+        self.gcode.register_command(
+            'EXCLUDE_OBJECT_END', self.cmd_EXCLUDE_OBJECT_END,
+            desc=self.cmd_EXCLUDE_OBJECT_END_help)
+        self.gcode.register_command(
+            'EXCLUDE_OBJECT', self.cmd_EXCLUDE_OBJECT,
+            desc=self.cmd_EXCLUDE_OBJECT_help)
+        self.gcode.register_command(
+            'EXCLUDE_OBJECT_DEFINE', self.cmd_EXCLUDE_OBJECT_DEFINE,
+            desc=self.cmd_EXCLUDE_OBJECT_DEFINE_help)
+
+    def _register_transform(self):
+        if self.next_transform is None:
+            tuning_tower = self.printer.lookup_object('tuning_tower')
+            if tuning_tower.is_active():
+                logging.info('The ExcludeObject move transform is not being '
+                    'loaded due to Tuning tower being Active')
+                return
+
+            self.next_transform = self.gcode_move.set_move_transform(self,
+                                                                     force=True)
+            self.extrusion_offsets = {}
+            self.max_position_extruded = 0
+            self.max_position_excluded = 0
+            self.extruder_adj = 0
+            self.initial_extrusion_moves = 5
+            self.last_position = [0., 0., 0., 0.]
+
+            self.get_position()
+            self.last_position_extruded[:] = self.last_position
+            self.last_position_excluded[:] = self.last_position
+
+    def _handle_connect(self):
+        self.toolhead = self.printer.lookup_object('toolhead')
+
+    def _unregister_transform(self):
+        if self.next_transform:
+            tuning_tower = self.printer.lookup_object('tuning_tower')
+            if tuning_tower.is_active():
+                logging.error('The Exclude Object move transform was not '
+                    'unregistered because it is not at the head of the '
+                    'transform chain.')
+                return
+
+            self.gcode_move.set_move_transform(self.next_transform, force=True)
+            self.next_transform = None
+            self.gcode_move.reset_last_position()
+
+    def _reset_state(self):
+        self.objects = []
+        self.excluded_objects = []
+        self.current_object = None
+        self.in_excluded_region = False
+
+    def _reset_file(self):
+        self._reset_state()
+        self._unregister_transform()
+
+    def _get_extrusion_offsets(self):
+        offset = self.extrusion_offsets.get(
+            self.toolhead.get_extruder().get_name())
+        if offset is None:
+            offset = [0., 0., 0., 0.]
+            self.extrusion_offsets[self.toolhead.get_extruder().get_name()] = \
+                offset
+        return offset
+
+    def get_position(self):
+        offset = self._get_extrusion_offsets()
+        pos = self.next_transform.get_position()
+        for i in range(4):
+            self.last_position[i] = pos[i] + offset[i]
+        return list(self.last_position)
+
+    def _normal_move(self, newpos, speed):
+        offset = self._get_extrusion_offsets()
+
+        if self.initial_extrusion_moves > 0 and \
+            self.last_position[3] != newpos[3]:
+            # Since the transform is not loaded until there is a request to
+            # exclude an object, the transform needs to track a few extrusions
+            # to get the state of the extruder
+            self.initial_extrusion_moves -= 1
+
+        self.last_position[:] = newpos
+        self.last_position_extruded[:] = self.last_position
+        self.max_position_extruded = max(self.max_position_extruded, newpos[3])
+
+        # These next few conditionals handle the moves immediately after leaving
+        # and excluded object.  The toolhead is at the end of the last printed
+        # object and the gcode is at the end of the last excluded object.
+        #
+        # Ideally, there will be Z and E moves right away to adjust any offsets
+        # before moving away from the last position.  Any remaining corrections
+        # will be made on the firs XY move.
+        if (offset[0] != 0 or offset[1] != 0) and \
+            (newpos[0] != self.last_position_excluded[0] or \
+            newpos[1] != self.last_position_excluded[1]):
+            offset[0] = 0
+            offset[1] = 0
+            offset[2] = 0
+            offset[3] += self.extruder_adj
+            self.extruder_adj = 0
+
+        if offset[2] != 0 and newpos[2] != self.last_position_excluded[2]:
+            offset[2] = 0
+
+        if self.extruder_adj != 0 and \
+            newpos[3] != self.last_position_excluded[3]:
+            offset[3] += self.extruder_adj
+            self.extruder_adj = 0
+
+        tx_pos = newpos[:]
+        for i in range(4):
+            tx_pos[i] = newpos[i] - offset[i]
+        self.next_transform.move(tx_pos, speed)
+
+    def _ignore_move(self, newpos, speed):
+        offset = self._get_extrusion_offsets()
+        for i in range(3):
+            offset[i] = newpos[i] - self.last_position_extruded[i]
+        offset[3] = offset[3] + newpos[3] - self.last_position[3]
+        self.last_position[:] = newpos
+        self.last_position_excluded[:] =self.last_position
+        self.max_position_excluded = max(self.max_position_excluded, newpos[3])
+
+    def _move_into_excluded_region(self, newpos, speed):
+        self.in_excluded_region = True
+        self._ignore_move(newpos, speed)
+
+    def _move_from_excluded_region(self, newpos, speed):
+        self.in_excluded_region = False
+
+        # This adjustment value is used to compensate for any retraction
+        # differences between the last object printed and excluded one.
+        self.extruder_adj = self.max_position_excluded \
+            - self.last_position_excluded[3] \
+            - (self.max_position_extruded - self.last_position_extruded[3])
+        self._normal_move(newpos, speed)
+
+    def _test_in_excluded_region(self):
+        # Inside cancelled object
+        return self.current_object in self.excluded_objects \
+            and self.initial_extrusion_moves == 0
+
+    def get_status(self, eventtime=None):
+        status = {
+            "objects": self.objects,
+            "excluded_objects": self.excluded_objects,
+            "current_object": self.current_object
+        }
+        return status
+
+    def move(self, newpos, speed):
+        move_in_excluded_region = self._test_in_excluded_region()
+        self.last_speed = speed
+
+        if move_in_excluded_region:
+            if self.in_excluded_region:
+                self._ignore_move(newpos, speed)
+            else:
+                self._move_into_excluded_region(newpos, speed)
+        else:
+            if self.in_excluded_region:
+                self._move_from_excluded_region(newpos, speed)
+            else:
+                self._normal_move(newpos, speed)
+
+    cmd_EXCLUDE_OBJECT_START_help = "Marks the beginning the current object" \
+                                    " as labeled"
+    def cmd_EXCLUDE_OBJECT_START(self, gcmd):
+        name = gcmd.get('NAME').upper()
+        if not any(obj["name"] == name for obj in self.objects):
+            self._add_object_definition({"name": name})
+        self.current_object = name
+        self.was_excluded_at_start = self._test_in_excluded_region()
+
+    cmd_EXCLUDE_OBJECT_END_help = "Marks the end the current object"
+    def cmd_EXCLUDE_OBJECT_END(self, gcmd):
+        if self.current_object == None and self.next_transform:
+            gcmd.respond_info("EXCLUDE_OBJECT_END called, but no object is"
+                              " currently active")
+            return
+        name = gcmd.get('NAME', default=None)
+        if name != None and name.upper() != self.current_object:
+            gcmd.respond_info("EXCLUDE_OBJECT_END NAME=%s does not match the"
+                              " current object NAME=%s" %
+                              (name.upper(), self.current_object))
+
+        self.current_object = None
+
+    cmd_EXCLUDE_OBJECT_help = "Cancel moves inside a specified objects"
+    def cmd_EXCLUDE_OBJECT(self, gcmd):
+        reset = gcmd.get('RESET', None)
+        current = gcmd.get('CURRENT', None)
+        name = gcmd.get('NAME', '').upper()
+
+        if reset:
+            if name:
+                self._unexclude_object(name)
+
+            else:
+                self.excluded_objects = []
+
+        elif name:
+            if name.upper() not in self.excluded_objects:
+                self._exclude_object(name.upper())
+
+        elif current:
+            if not self.current_object:
+                gcmd.respond_error('There is no current object to cancel')
+
+            else:
+                self._exclude_object(self.current_object)
+
+        else:
+            self._list_excluded_objects(gcmd)
+
+    cmd_EXCLUDE_OBJECT_DEFINE_help = "Provides a summary of an object"
+    def cmd_EXCLUDE_OBJECT_DEFINE(self, gcmd):
+        reset = gcmd.get('RESET', None)
+        name = gcmd.get('NAME', '').upper()
+
+        if reset:
+            self._reset_file()
+
+        elif name:
+            parameters = gcmd.get_command_parameters().copy()
+            parameters.pop('NAME')
+            center = parameters.pop('CENTER', None)
+            polygon = parameters.pop('POLYGON', None)
+
+            obj = {"name": name.upper()}
+            obj.update(parameters)
+
+            if center != None:
+                obj['center'] = json.loads('[%s]' % center)
+
+            if polygon != None:
+                obj['polygon'] = json.loads(polygon)
+
+            self._add_object_definition(obj)
+
+        else:
+            self._list_objects(gcmd)
+
+    def _add_object_definition(self, definition):
+        self.objects = sorted(self.objects + [definition],
+                              key=lambda o: o["name"])
+
+    def _exclude_object(self, name):
+        self._register_transform()
+        self.gcode.respond_info('Excluding object {}'.format(name.upper()))
+        if name not in self.excluded_objects:
+            self.excluded_objects = sorted(self.excluded_objects + [name])
+
+    def _unexclude_object(self, name):
+        self.gcode.respond_info('Unexcluding object {}'.format(name.upper()))
+        if name in self.excluded_objects:
+            excluded_objects = list(self.excluded_objects)
+            excluded_objects.remove(name)
+            self.excluded_objects = sorted(excluded_objects)
+
+    def _list_objects(self, gcmd):
+        if gcmd.get('JSON', None) is not None:
+            object_list = json.dumps(self.objects)
+        else:
+            object_list = " ".join(obj['name'] for obj in self.objects)
+        gcmd.respond_info('Known objects: {}'.format(object_list))
+
+    def _list_excluded_objects(self, gcmd):
+        object_list = " ".join(self.excluded_objects)
+        gcmd.respond_info('Excluded objects: {}'.format(object_list))
+
+def load_config(config):
+    return ExcludeObject(config)

klippy/extras/extruder_stepper.py

@@ -0,0 +1,20 @@
+# Code for supporting multiple steppers in single filament extruder.
+#
+# Copyright (C) 2019 Simo Apell <simo.apell@live.fi>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import logging
+from kinematics import extruder
+
+class PrinterExtruderStepper:
+    def __init__(self, config):
+        self.printer = config.get_printer()
+        self.extruder_stepper = extruder.ExtruderStepper(config)
+        self.extruder_name = config.get('extruder')
+        self.printer.register_event_handler("klippy:connect",
+                                            self.handle_connect)
+    def handle_connect(self):
+        self.extruder_stepper.sync_to_extruder(self.extruder_name)
+
+def load_config_prefix(config):
+    return PrinterExtruderStepper(config)

klippy/extras/fan.py

@@ -0,0 +1,110 @@
+# Printer cooling fan
+#
+# Copyright (C) 2016-2020  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+from . import pulse_counter
+
+FAN_MIN_TIME = 0.100
+
+class Fan:
+    def __init__(self, config, default_shutdown_speed=0.):
+        self.printer = config.get_printer()
+        self.last_fan_value = 0.
+        self.last_fan_time = 0.
+        # Read config
+        self.max_power = config.getfloat('max_power', 1., above=0., maxval=1.)
+        self.kick_start_time = config.getfloat('kick_start_time', 0.1,
+                                               minval=0.)
+        self.off_below = config.getfloat('off_below', default=0.,
+                                         minval=0., maxval=1.)
+        cycle_time = config.getfloat('cycle_time', 0.010, above=0.)
+        hardware_pwm = config.getboolean('hardware_pwm', False)
+        shutdown_speed = config.getfloat(
+            'shutdown_speed', default_shutdown_speed, minval=0., maxval=1.)
+        # Setup pwm object
+        ppins = self.printer.lookup_object('pins')
+        self.mcu_fan = ppins.setup_pin('pwm', config.get('pin'))
+        self.mcu_fan.setup_max_duration(0.)
+        self.mcu_fan.setup_cycle_time(cycle_time, hardware_pwm)
+        shutdown_power = max(0., min(self.max_power, shutdown_speed))
+        self.mcu_fan.setup_start_value(0., shutdown_power)
+
+        # Setup tachometer
+        self.tachometer = FanTachometer(config)
+
+        # Register callbacks
+        self.printer.register_event_handler("gcode:request_restart",
+                                            self._handle_request_restart)
+
+    def get_mcu(self):
+        return self.mcu_fan.get_mcu()
+    def set_speed(self, print_time, value):
+        if value < self.off_below:
+            value = 0.
+        value = max(0., min(self.max_power, value * self.max_power))
+        if value == self.last_fan_value:
+            return
+        print_time = max(self.last_fan_time + FAN_MIN_TIME, print_time)
+        if (value and value < self.max_power and self.kick_start_time
+            and (not self.last_fan_value or value - self.last_fan_value > .5)):
+            # Run fan at full speed for specified kick_start_time
+            self.mcu_fan.set_pwm(print_time, self.max_power)
+            print_time += self.kick_start_time
+        self.mcu_fan.set_pwm(print_time, value)
+        self.last_fan_time = print_time
+        self.last_fan_value = value
+    def set_speed_from_command(self, value):
+        toolhead = self.printer.lookup_object('toolhead')
+        toolhead.register_lookahead_callback((lambda pt:
+                                              self.set_speed(pt, value)))
+    def _handle_request_restart(self, print_time):
+        self.set_speed(print_time, 0.)
+
+    def get_status(self, eventtime):
+        tachometer_status = self.tachometer.get_status(eventtime)
+        return {
+            'speed': self.last_fan_value,
+            'rpm': tachometer_status['rpm'],
+        }
+
+class FanTachometer:
+    def __init__(self, config):
+        printer = config.get_printer()
+        self._freq_counter = None
+
+        pin = config.get('tachometer_pin', None)
+        if pin is not None:
+            self.ppr = config.getint('tachometer_ppr', 2, minval=1)
+            poll_time = config.getfloat('tachometer_poll_interval',
+                                        0.0015, above=0.)
+            sample_time = 1.
+            self._freq_counter = pulse_counter.FrequencyCounter(
+                printer, pin, sample_time, poll_time)
+
+    def get_status(self, eventtime):
+        if self._freq_counter is not None:
+            rpm = self._freq_counter.get_frequency() * 30. / self.ppr
+        else:
+            rpm = None
+        return {'rpm': rpm}
+
+class PrinterFan:
+    def __init__(self, config):
+        self.fan = Fan(config)
+        # Register commands
+        gcode = config.get_printer().lookup_object('gcode')
+        gcode.register_command("M106", self.cmd_M106)
+        gcode.register_command("M107", self.cmd_M107)
+    def get_status(self, eventtime):
+        return self.fan.get_status(eventtime)
+    def cmd_M106(self, gcmd):
+        # Set fan speed
+        value = gcmd.get_float('S', 255., minval=0.) / 255.
+        self.fan.set_speed_from_command(value)
+    def cmd_M107(self, gcmd):
+        # Turn fan off
+        self.fan.set_speed_from_command(0.)
+
+def load_config(config):
+    return PrinterFan(config)

klippy/extras/fan_generic.py

@@ -0,0 +1,28 @@
+# Support fans that are controlled by gcode
+#
+# Copyright (C) 2016-2020  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+from . import fan
+
+class PrinterFanGeneric:
+    cmd_SET_FAN_SPEED_help = "Sets the speed of a fan"
+    def __init__(self, config):
+        self.printer = config.get_printer()
+        self.fan = fan.Fan(config, default_shutdown_speed=0.)
+        self.fan_name = config.get_name().split()[-1]
+
+        gcode = self.printer.lookup_object("gcode")
+        gcode.register_mux_command("SET_FAN_SPEED", "FAN",
+                                   self.fan_name,
+                                   self.cmd_SET_FAN_SPEED,
+                                   desc=self.cmd_SET_FAN_SPEED_help)
+
+    def get_status(self, eventtime):
+        return self.fan.get_status(eventtime)
+    def cmd_SET_FAN_SPEED(self, gcmd):
+        speed = gcmd.get_float('SPEED', 0.)
+        self.fan.set_speed_from_command(speed)
+
+def load_config_prefix(config):
+    return PrinterFanGeneric(config)

klippy/extras/filament_motion_sensor.py

@@ -0,0 +1,77 @@
+# 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 EncoderSensor:
+    def __init__(self, config):
+        # Read config
+        self.printer = config.get_printer()
+        switch_pin = config.get('switch_pin')
+        self.extruder_name = config.get('extruder')
+        self.detection_length = config.getfloat(
+                'detection_length', 7., above=0.)
+        # Configure pins
+        buttons = self.printer.load_object(config, 'buttons')
+        buttons.register_buttons([switch_pin], self.encoder_event)
+        # Get printer objects
+        self.reactor = self.printer.get_reactor()
+        self.runout_helper = filament_switch_sensor.RunoutHelper(config)
+        self.get_status = self.runout_helper.get_status
+        self.extruder = None
+        self.estimated_print_time = 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 is 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.estimated_print_time = (
+                self.printer.lookup_object('mcu').estimated_print_time)
+        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 is None:
+            eventtime = self.reactor.monotonic()
+        print_time = self.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.extruder is not 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)
+
+def load_config_prefix(config):
+    return EncoderSensor(config)

klippy/extras/filament_switch_sensor.py

@@ -0,0 +1,118 @@
+# Generic Filament Sensor Module
+#
+# Copyright (C) 2019  Eric Callahan <arksine.code@gmail.com>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import logging
+
+class RunoutHelper:
+    def __init__(self, config):
+        self.name = config.get_name().split()[-1]
+        self.printer = config.get_printer()
+        self.reactor = self.printer.get_reactor()
+        self.gcode = self.printer.lookup_object('gcode')
+        # Read config
+        self.runout_pause = config.getboolean('pause_on_runout', True)
+        if self.runout_pause:
+            self.printer.load_object(config, 'pause_resume')
+        self.runout_gcode = self.insert_gcode = None
+        gcode_macro = self.printer.load_object(config, 'gcode_macro')
+        if self.runout_pause or config.get('runout_gcode', None) is not None:
+            self.runout_gcode = gcode_macro.load_template(
+                config, 'runout_gcode', '')
+        if config.get('insert_gcode', None) is not None:
+            self.insert_gcode = gcode_macro.load_template(
+                config, 'insert_gcode')
+        self.pause_delay = config.getfloat('pause_delay', .5, above=.0)
+        self.event_delay = config.getfloat('event_delay', 3., above=0.)
+        # Internal state
+        self.min_event_systime = self.reactor.NEVER
+        self.filament_present = False
+        self.sensor_enabled = True
+        # Register commands and event handlers
+        self.printer.register_event_handler("klippy:ready", self._handle_ready)
+        self.gcode.register_mux_command(
+            "QUERY_FILAMENT_SENSOR", "SENSOR", self.name,
+            self.cmd_QUERY_FILAMENT_SENSOR,
+            desc=self.cmd_QUERY_FILAMENT_SENSOR_help)
+        self.gcode.register_mux_command(
+            "SET_FILAMENT_SENSOR", "SENSOR", self.name,
+            self.cmd_SET_FILAMENT_SENSOR,
+            desc=self.cmd_SET_FILAMENT_SENSOR_help)
+    def _handle_ready(self):
+        self.min_event_systime = self.reactor.monotonic() + 2.
+    def _runout_event_handler(self, eventtime):
+        # Pausing from inside an event requires that the pause portion
+        # of pause_resume execute immediately.
+        pause_prefix = ""
+        if self.runout_pause:
+            pause_resume = self.printer.lookup_object('pause_resume')
+            pause_resume.send_pause_command()
+            pause_prefix = "PAUSE\n"
+            self.printer.get_reactor().pause(eventtime + self.pause_delay)
+        self._exec_gcode(pause_prefix, self.runout_gcode)
+    def _insert_event_handler(self, eventtime):
+        self._exec_gcode("", self.insert_gcode)
+    def _exec_gcode(self, prefix, template):
+        try:
+            self.gcode.run_script(prefix + template.render() + "\nM400")
+        except Exception:
+            logging.exception("Script running error")
+        self.min_event_systime = self.reactor.monotonic() + self.event_delay
+    def note_filament_present(self, is_filament_present):
+        if is_filament_present == self.filament_present:
+            return
+        self.filament_present = is_filament_present
+        eventtime = self.reactor.monotonic()
+        if eventtime < self.min_event_systime or not self.sensor_enabled:
+            # do not process during the initialization time, duplicates,
+            # during the event delay time, while an event is running, or
+            # when the sensor is disabled
+            return
+        # Determine "printing" status
+        idle_timeout = self.printer.lookup_object("idle_timeout")
+        is_printing = idle_timeout.get_status(eventtime)["state"] == "Printing"
+        # Perform filament action associated with status change (if any)
+        if is_filament_present:
+            if not is_printing and self.insert_gcode is not None:
+                # insert detected
+                self.min_event_systime = self.reactor.NEVER
+                logging.info(
+                    "Filament Sensor %s: insert event detected, Time %.2f" %
+                    (self.name, eventtime))
+                self.reactor.register_callback(self._insert_event_handler)
+        elif is_printing and self.runout_gcode is not None:
+            # runout detected
+            self.min_event_systime = self.reactor.NEVER
+            logging.info(
+                "Filament Sensor %s: runout event detected, Time %.2f" %
+                (self.name, eventtime))
+            self.reactor.register_callback(self._runout_event_handler)
+    def get_status(self, eventtime):
+        return {
+            "filament_detected": bool(self.filament_present),
+            "enabled": bool(self.sensor_enabled)}
+    cmd_QUERY_FILAMENT_SENSOR_help = "Query the status of the Filament Sensor"
+    def cmd_QUERY_FILAMENT_SENSOR(self, gcmd):
+        if self.filament_present:
+            msg = "Filament Sensor %s: filament detected" % (self.name)
+        else:
+            msg = "Filament Sensor %s: filament not detected" % (self.name)
+        gcmd.respond_info(msg)
+    cmd_SET_FILAMENT_SENSOR_help = "Sets the filament sensor on/off"
+    def cmd_SET_FILAMENT_SENSOR(self, gcmd):
+        self.sensor_enabled = gcmd.get_int("ENABLE", 1)
+
+class SwitchSensor:
+    def __init__(self, config):
+        printer = config.get_printer()
+        buttons = printer.load_object(config, 'buttons')
+        switch_pin = config.get('switch_pin')
+        buttons.register_buttons([switch_pin], self._button_handler)
+        self.runout_helper = RunoutHelper(config)
+        self.get_status = self.runout_helper.get_status
+    def _button_handler(self, eventtime, state):
+        self.runout_helper.note_filament_present(state)
+
+def load_config_prefix(config):
+    return SwitchSensor(config)