QIDITECH/klipper
1
0
Fork 0
mirror of https://github.com/QIDITECH/klipper.git synced 2026-01-30 23:48:43 +03:00
Code Issues Packages Projects Releases Wiki Activity

klipper update

Browse Source
This commit is contained in:
Rainboooom
2023-06-15 11:41:08 +08:00
parent 845d13acb1
commit dffff1ae35
1921 changed files with 1625400 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

304
klippy/chelper/__init__.py Normal file
View File

@@ -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()

BIN
klippy/chelper/__init__.pyc Normal file
View File

Binary file not shown.

BIN
klippy/chelper/c_helper.so Normal file
View File

Binary file not shown.

46
klippy/chelper/compiler.h Normal file
View File

@@ -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

280
klippy/chelper/itersolve.c Normal file
View File

@@ -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;
}

41
klippy/chelper/itersolve.h Normal file
View File

@@ -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

90
klippy/chelper/kin_cartesian.c Normal file
View File

@@ -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;
}

40
klippy/chelper/kin_corexy.c Normal file
View File

@@ -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;
}

40
klippy/chelper/kin_corexz.c Normal file
View File

@@ -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;
}

41
klippy/chelper/kin_delta.c Normal file
View File

@@ -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;
}

145
klippy/chelper/kin_extruder.c Normal file
View File

@@ -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;
}

59
klippy/chelper/kin_polar.c Normal file
View File

@@ -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;
}

73
klippy/chelper/kin_rotary_delta.c Normal file
View File

@@ -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;
}

232
klippy/chelper/kin_shaper.c Normal file
View File

@@ -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;
}

42
klippy/chelper/kin_winch.c Normal file
View File

@@ -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;
}

126
klippy/chelper/list.h Normal file
View File

@@ -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

209
klippy/chelper/msgblock.c Normal file
View File

@@ -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;
}

54
klippy/chelper/msgblock.h Normal file
View File

@@ -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

179
klippy/chelper/pollreactor.c Normal file
View File

@@ -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;
}

20
klippy/chelper/pollreactor.h Normal file
View File

@@ -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

94
klippy/chelper/pyhelper.c Normal file
View File

@@ -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;
}

11
klippy/chelper/pyhelper.h Normal file
View File

@@ -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

947
klippy/chelper/serialqueue.c Normal file
View File

@@ -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;
}

56
klippy/chelper/serialqueue.h Normal file
View File

@@ -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

795
klippy/chelper/stepcompress.c Normal file
View File

@@ -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;
}

45
klippy/chelper/stepcompress.h Normal file
View File

@@ -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

258
klippy/chelper/trapq.c Normal file
View File

@@ -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;
}

52
klippy/chelper/trapq.h Normal file
View File

@@ -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

226
klippy/chelper/trdispatch.c Normal file
View File

@@ -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);
}