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

plus4的klipper版本

Browse Source
This commit is contained in:
whb0514
2024-09-02 13:37:34 +08:00
parent 653d7a8f6e
commit b90736975b
1006 changed files with 1195894 additions and 11114 deletions
Download Patch File Download Diff File Expand all files Collapse all files

42
klippy/chelper/__init__.py
View File

@@ -20,8 +20,8 @@ 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',
'kin_deltesian.c', 'kin_polar.c', 'kin_rotary_delta.c', 'kin_winch.c',
'kin_extruder.c', 'kin_shaper.c', 'kin_idex.c',
]
DEST_LIB = "c_helper.so"
OTHER_FILES = [
@@ -49,6 +49,8 @@ defs_stepcompress = """
, uint64_t clock);
int stepcompress_queue_msg(struct stepcompress *sc
, uint32_t *data, int len);
int stepcompress_queue_mq_msg(struct stepcompress *sc, uint64_t req_clock
, 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);
@@ -58,7 +60,8 @@ defs_stepcompress = """
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);
int steppersync_flush(struct steppersync *ss, uint64_t move_clock
, uint64_t clear_history_clock);
"""
defs_itersolve = """
@@ -85,14 +88,15 @@ defs_trapq = """
double x_r, y_r, z_r;
};
struct trapq *trapq_alloc(void);
void trapq_free(struct trapq *tq);
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_finalize_moves(struct trapq *tq, double print_time
, double clear_history_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
@@ -101,7 +105,6 @@ defs_trapq = """
defs_kin_cartesian = """
struct stepper_kinematics *cartesian_stepper_alloc(char axis);
struct stepper_kinematics *cartesian_reverse_stepper_alloc(char axis);
"""
defs_kin_corexy = """
@@ -117,6 +120,11 @@ defs_kin_delta = """
, double tower_x, double tower_y);
"""
defs_kin_deltesian = """
struct stepper_kinematics *deltesian_stepper_alloc(double arm2
, double arm_x);
"""
defs_kin_polar = """
struct stepper_kinematics *polar_stepper_alloc(char type);
"""
@@ -139,8 +147,8 @@ defs_kin_extruder = """
"""
defs_kin_shaper = """
double input_shaper_get_step_generation_window(int n, double a[]
, double t[]);
double input_shaper_get_step_generation_window(
struct stepper_kinematics *sk);
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
@@ -148,6 +156,14 @@ defs_kin_shaper = """
struct stepper_kinematics * input_shaper_alloc(void);
"""
defs_kin_idex = """
void dual_carriage_set_sk(struct stepper_kinematics *sk
, struct stepper_kinematics *orig_sk);
int dual_carriage_set_transform(struct stepper_kinematics *sk
, char axis, double scale, double offs);
struct stepper_kinematics * dual_carriage_alloc(void);
"""
defs_serialqueue = """
#define MESSAGE_MAX 64
struct pull_queue_message {
@@ -168,8 +184,8 @@ defs_serialqueue = """
, 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_wire_frequency(struct serialqueue *sq
, double frequency);
void serialqueue_set_receive_window(struct serialqueue *sq
, int receive_window);
void serialqueue_set_clock_est(struct serialqueue *sq, double est_freq
@@ -205,8 +221,8 @@ 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,
defs_kin_deltesian, defs_kin_polar, defs_kin_rotary_delta, defs_kin_winch,
defs_kin_extruder, defs_kin_shaper, defs_kin_idex,
]
# Update filenames to an absolute path

BIN
klippy/chelper/__init__.pyc
View File

Binary file not shown.

BIN
klippy/chelper/c_helper.so
View File

Binary file not shown.

39
klippy/chelper/kin_cartesian.c
View File

@@ -49,42 +49,3 @@ cartesian_stepper_alloc(char axis)
}
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;
}

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

@@ -0,0 +1,41 @@
// Deltesian kinematics stepper pulse time generation
//
// Copyright (C) 2022 Fabrice Gallet <tircown@gmail.com>
//
// 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 deltesian_stepper {
struct stepper_kinematics sk;
double arm2, arm_x;
};
static double
deltesian_stepper_calc_position(struct stepper_kinematics *sk, struct move *m
, double move_time)
{
struct deltesian_stepper *ds = container_of(
sk, struct deltesian_stepper, sk);
struct coord c = move_get_coord(m, move_time);
double dx = c.x - ds->arm_x;
return sqrt(ds->arm2 - dx*dx) + c.z;
}
struct stepper_kinematics * __visible
deltesian_stepper_alloc(double arm2, double arm_x)
{
struct deltesian_stepper *ds = malloc(sizeof(*ds));
memset(ds, 0, sizeof(*ds));
ds->arm2 = arm2;
ds->arm_x = arm_x;
ds->sk.calc_position_cb = deltesian_stepper_calc_position;
ds->sk.active_flags = AF_X | AF_Z;
return &ds->sk;
}

81
klippy/chelper/kin_idex.c Normal file
View File

@@ -0,0 +1,81 @@
// Idex dual carriage kinematics
//
// Copyright (C) 2023 Dmitry Butyugin <dmbutyugin@google.com>
//
// 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 "trapq.h" // struct move
#define DUMMY_T 500.0
struct dual_carriage_stepper {
struct stepper_kinematics sk;
struct stepper_kinematics *orig_sk;
struct move m;
double x_scale, x_offs, y_scale, y_offs;
};
double
dual_carriage_calc_position(struct stepper_kinematics *sk, struct move *m
, double move_time)
{
struct dual_carriage_stepper *dc = container_of(
sk, struct dual_carriage_stepper, sk);
struct coord pos = move_get_coord(m, move_time);
dc->m.start_pos.x = pos.x * dc->x_scale + dc->x_offs;
dc->m.start_pos.y = pos.y * dc->y_scale + dc->y_offs;
dc->m.start_pos.z = pos.z;
return dc->orig_sk->calc_position_cb(dc->orig_sk, &dc->m, DUMMY_T);
}
void __visible
dual_carriage_set_sk(struct stepper_kinematics *sk
, struct stepper_kinematics *orig_sk)
{
struct dual_carriage_stepper *dc = container_of(
sk, struct dual_carriage_stepper, sk);
dc->sk.calc_position_cb = dual_carriage_calc_position;
dc->sk.active_flags = orig_sk->active_flags;
dc->orig_sk = orig_sk;
}
int __visible
dual_carriage_set_transform(struct stepper_kinematics *sk, char axis
, double scale, double offs)
{
struct dual_carriage_stepper *dc = container_of(
sk, struct dual_carriage_stepper, sk);
if (axis == 'x') {
dc->x_scale = scale;
dc->x_offs = offs;
if (!scale)
dc->sk.active_flags &= ~AF_X;
else if (scale && dc->orig_sk->active_flags & AF_X)
dc->sk.active_flags |= AF_X;
return 0;
}
if (axis == 'y') {
dc->y_scale = scale;
dc->y_offs = offs;
if (!scale)
dc->sk.active_flags &= ~AF_Y;
else if (scale && dc->orig_sk->active_flags & AF_Y)
dc->sk.active_flags |= AF_Y;
return 0;
}
return -1;
}
struct stepper_kinematics * __visible
dual_carriage_alloc(void)
{
struct dual_carriage_stepper *dc = malloc(sizeof(*dc));
memset(dc, 0, sizeof(*dc));
dc->m.move_t = 2. * DUMMY_T;
return &dc->sk;
}

24
klippy/chelper/kin_shaper.c
View File

@@ -171,6 +171,9 @@ input_shaper_set_sk(struct stepper_kinematics *sk
return -1;
is->sk.active_flags = orig_sk->active_flags;
is->orig_sk = orig_sk;
is->sk.commanded_pos = orig_sk->commanded_pos;
is->sk.last_flush_time = orig_sk->last_flush_time;
is->sk.last_move_time = orig_sk->last_move_time;
return 0;
}
@@ -201,25 +204,20 @@ input_shaper_set_shaper_params(struct stepper_kinematics *sk, char axis
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))
// Ignore input shaper update if the axis is not active
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);
shaper_note_generation_time(is);
}
return status;
}
double __visible
input_shaper_get_step_generation_window(int n, double a[], double t[])
input_shaper_get_step_generation_window(struct stepper_kinematics *sk)
{
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 input_shaper *is = container_of(sk, struct input_shaper, sk);
return is->sk.gen_steps_pre_active > is->sk.gen_steps_post_active
? is->sk.gen_steps_pre_active : is->sk.gen_steps_post_active;
}
struct stepper_kinematics * __visible

112
klippy/chelper/serialqueue.c
View File

@@ -30,7 +30,7 @@
#include "serialqueue.h" // struct queue_message
struct command_queue {
struct list_head stalled_queue, ready_queue;
struct list_head upcoming_queue, ready_queue;
struct list_node node;
};
@@ -49,7 +49,7 @@ struct serialqueue {
int receive_waiting;
// Baud / clock tracking
int receive_window;
double baud_adjust, idle_time;
double bittime_adjust, idle_time;
struct clock_estimate ce;
double last_receive_sent_time;
// Retransmit support
@@ -59,9 +59,10 @@ struct serialqueue {
double srtt, rttvar, rto;
// Pending transmission message queues
struct list_head pending_queues;
int ready_bytes, stalled_bytes, need_ack_bytes, last_ack_bytes;
int ready_bytes, upcoming_bytes, need_ack_bytes, last_ack_bytes;
uint64_t need_kick_clock;
struct list_head notify_queue;
double last_write_fail_time;
// Received messages
struct list_head receive_queue;
// Fastreader support
@@ -136,6 +137,23 @@ kick_bg_thread(struct serialqueue *sq)
report_errno("pipe write", ret);
}
// Minimum number of bits in a canbus message
#define CANBUS_PACKET_BITS ((1 + 11 + 3 + 4) + (16 + 2 + 7 + 3))
#define CANBUS_IFS_BITS 4
// Determine minimum time needed to transmit a given number of bytes
static double
calculate_bittime(struct serialqueue *sq, uint32_t bytes)
{
if (sq->serial_fd_type == SQT_CAN) {
uint32_t pkts = DIV_ROUND_UP(bytes, 8);
uint32_t bits = bytes * 8 + pkts * CANBUS_PACKET_BITS - CANBUS_IFS_BITS;
return sq->bittime_adjust * bits;
} else {
return sq->bittime_adjust * bytes;
}
}
// Update internal state when the receive sequence increases
static void
update_receive_seq(struct serialqueue *sq, double eventtime, uint64_t rseq)
@@ -192,7 +210,7 @@ update_receive_seq(struct serialqueue *sq, double eventtime, uint64_t rseq)
} 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;
double nr = eventtime + sq->rto + calculate_bittime(sq, sent->len);
pollreactor_update_timer(sq->pr, SQPT_RETRANSMIT, nr);
}
}
@@ -204,12 +222,11 @@ 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));
uint32_t rseq_delta = ((sq->input_buf[MESSAGE_POS_SEQ] - sq->receive_seq)
& MESSAGE_SEQ_MASK);
uint64_t rseq = sq->receive_seq + rseq_delta;
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;
@@ -251,7 +268,7 @@ handle_message(struct serialqueue *sq, double eventtime, int 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;
qm->receive_time -= calculate_bittime(sq, len);
list_add_tail(&qm->node, &sq->receive_queue);
must_wake = 1;
}
@@ -339,6 +356,7 @@ kick_event(struct serialqueue *sq, double eventtime)
pollreactor_update_timer(sq->pr, SQPT_COMMAND, PR_NOW);
}
// OS write of data to be sent to the mcu
static void
do_write(struct serialqueue *sq, void *buf, int buflen)
{
@@ -358,8 +376,16 @@ do_write(struct serialqueue *sq, void *buf, int buflen)
int ret = write(sq->serial_fd, &cf, sizeof(cf));
if (ret < 0) {
report_errno("can write", ret);
double curtime = get_monotonic();
if (!sq->last_write_fail_time) {
sq->last_write_fail_time = curtime;
} else if (curtime > sq->last_write_fail_time + 10.0) {
errorf("Halting reads due to CAN write errors.");
pollreactor_do_exit(sq->pr);
}
return;
}
sq->last_write_fail_time = 0.0;
buf += size;
buflen -= size;
}
@@ -407,8 +433,8 @@ retransmit_event(struct serialqueue *sq, double eventtime)
}
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;
sq->idle_time = eventtime + calculate_bittime(sq, buflen);
double waketime = eventtime + sq->rto + calculate_bittime(sq, first_buflen);
pthread_mutex_unlock(&sq->lock);
return waketime;
@@ -416,7 +442,8 @@ retransmit_event(struct serialqueue *sq, double eventtime)
// 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)
build_and_send_command(struct serialqueue *sq, uint8_t *buf, int pending
, double eventtime)
{
int len = MESSAGE_HEADER_SIZE;
while (sq->ready_bytes) {
@@ -439,7 +466,7 @@ build_and_send_command(struct serialqueue *sq, uint8_t *buf, double eventtime)
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))
if (list_empty(&cq->ready_queue) && list_empty(&cq->upcoming_queue))
list_del(&cq->node);
memcpy(&buf[len], qm->msg, qm->len);
len += qm->len;
@@ -463,17 +490,15 @@ build_and_send_command(struct serialqueue *sq, uint8_t *buf, double eventtime)
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;
double idletime = eventtime > sq->idle_time ? eventtime : sq->idle_time;
idletime += calculate_bittime(sq, pending + len);
struct queue_message *out = message_alloc();
memcpy(out->msg, buf, len);
out->len = len;
out->sent_time = eventtime;
out->receive_time = sq->idle_time;
out->receive_time = idletime;
if (list_empty(&sq->sent_queue))
pollreactor_update_timer(sq->pr, SQPT_RETRANSMIT
, sq->idle_time + sq->rto);
pollreactor_update_timer(sq->pr, SQPT_RETRANSMIT, idletime + sq->rto);
if (!sq->rtt_sample_seq)
sq->rtt_sample_seq = sq->send_seq;
sq->send_seq++;
@@ -484,7 +509,7 @@ build_and_send_command(struct serialqueue *sq, uint8_t *buf, double eventtime)
// Determine the time the next serial data should be sent
static double
check_send_command(struct serialqueue *sq, double eventtime)
check_send_command(struct serialqueue *sq, int pending, double eventtime)
{
if (sq->send_seq - sq->receive_seq >= MAX_PENDING_BLOCKS
&& sq->receive_seq != (uint64_t)-1)
@@ -501,15 +526,15 @@ check_send_command(struct serialqueue *sq, double eventtime)
// Check for stalled messages now ready
double idletime = eventtime > sq->idle_time ? eventtime : sq->idle_time;
idletime += MESSAGE_MIN * sq->baud_adjust;
idletime += calculate_bittime(sq, pending + MESSAGE_MIN);
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)) {
// Move messages from the upcoming_queue to the ready_queue
while (!list_empty(&cq->upcoming_queue)) {
struct queue_message *qm = list_first_entry(
&cq->stalled_queue, struct queue_message, node);
&cq->upcoming_queue, struct queue_message, node);
if (ack_clock < qm->min_clock) {
if (qm->min_clock < min_stalled_clock)
min_stalled_clock = qm->min_clock;
@@ -517,7 +542,7 @@ check_send_command(struct serialqueue *sq, double eventtime)
}
list_del(&qm->node);
list_add_tail(&qm->node, &cq->ready_queue);
sq->stalled_bytes -= qm->len;
sq->upcoming_bytes -= qm->len;
sq->ready_bytes += qm->len;
}
// Update min_ready_clock
@@ -525,9 +550,10 @@ check_send_command(struct serialqueue *sq, double eventtime)
struct queue_message *qm = list_first_entry(
&cq->ready_queue, struct queue_message, node);
uint64_t req_clock = qm->req_clock;
double bgtime = pending ? idletime : sq->idle_time;
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);
req_clock = clock_from_time(&sq->ce, bgtime + bgoffset);
if (req_clock < min_ready_clock)
min_ready_clock = req_clock;
}
@@ -561,18 +587,21 @@ command_event(struct serialqueue *sq, double eventtime)
int buflen = 0;
double waketime;
for (;;) {
waketime = check_send_command(sq, eventtime);
waketime = check_send_command(sq, buflen, eventtime);
if (waketime != PR_NOW || buflen + MESSAGE_MAX > sizeof(buf)) {
if (buflen) {
// Write message blocks
do_write(sq, buf, buflen);
sq->bytes_write += buflen;
double idletime = (eventtime > sq->idle_time
? eventtime : sq->idle_time);
sq->idle_time = idletime + calculate_bittime(sq, buflen);
buflen = 0;
}
if (waketime != PR_NOW)
break;
}
buflen += build_and_send_command(sq, &buf[buflen], eventtime);
buflen += build_and_send_command(sq, &buf[buflen], buflen, eventtime);
}
pthread_mutex_unlock(&sq->lock);
return waketime;
@@ -693,7 +722,7 @@ serialqueue_free(struct serialqueue *sq)
&sq->pending_queues, struct command_queue, node);
list_del(&cq->node);
message_queue_free(&cq->ready_queue);
message_queue_free(&cq->stalled_queue);
message_queue_free(&cq->upcoming_queue);
}
pthread_mutex_unlock(&sq->lock);
pollreactor_free(sq->pr);
@@ -707,7 +736,7 @@ 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);
list_init(&cq->upcoming_queue);
return cq;
}
@@ -717,7 +746,7 @@ serialqueue_free_commandqueue(struct command_queue *cq)
{
if (!cq)
return;
if (!list_empty(&cq->ready_queue) || !list_empty(&cq->stalled_queue)) {
if (!list_empty(&cq->ready_queue) || !list_empty(&cq->upcoming_queue)) {
errorf("Memory leak! Can't free non-empty commandqueue");
return;
}
@@ -763,12 +792,12 @@ serialqueue_send_batch(struct serialqueue *sq, struct command_queue *cq
return;
qm = list_first_entry(msgs, struct queue_message, node);
// Add list to cq->stalled_queue
// Add list to cq->upcoming_queue
pthread_mutex_lock(&sq->lock);
if (list_empty(&cq->ready_queue) && list_empty(&cq->stalled_queue))
if (list_empty(&cq->ready_queue) && list_empty(&cq->upcoming_queue))
list_add_tail(&cq->node, &sq->pending_queues);
list_join_tail(msgs, &cq->stalled_queue);
sq->stalled_bytes += len;
list_join_tail(msgs, &cq->upcoming_queue);
sq->upcoming_bytes += len;
int mustwake = 0;
if (qm->min_clock < sq->need_kick_clock) {
sq->need_kick_clock = 0;
@@ -847,10 +876,15 @@ exit:
}
void __visible
serialqueue_set_baud_adjust(struct serialqueue *sq, double baud_adjust)
serialqueue_set_wire_frequency(struct serialqueue *sq, double frequency)
{
pthread_mutex_lock(&sq->lock);
sq->baud_adjust = baud_adjust;
if (sq->serial_fd_type == SQT_CAN) {
sq->bittime_adjust = 1. / frequency;
} else {
// An 8N1 serial line is 10 bits per byte (1 start, 8 data, 1 stop)
sq->bittime_adjust = 10. / frequency;
}
pthread_mutex_unlock(&sq->lock);
}
@@ -899,13 +933,13 @@ serialqueue_get_stats(struct serialqueue *sq, char *buf, int len)
" 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"
" ready_bytes=%u upcoming_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);
, stats.ready_bytes, stats.upcoming_bytes);
}
// Extract old messages stored in the debug queues

2
klippy/chelper/serialqueue.h
View File

@@ -42,7 +42,7 @@ 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_wire_frequency(struct serialqueue *sq, double frequency);
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

44
klippy/chelper/stepcompress.c
View File

@@ -54,8 +54,6 @@ struct step_move {
int16_t add;
};
#define HISTORY_EXPIRE (30.0)
struct history_steps {
struct list_node node;
uint64_t first_clock, last_clock;
@@ -292,6 +290,13 @@ free_history(struct stepcompress *sc, uint64_t end_clock)
}
}
// Expire the stepcompress history older than the given clock
static void
stepcompress_history_expire(struct stepcompress *sc, uint64_t end_clock)
{
free_history(sc, end_clock);
}
// Free memory associated with a 'stepcompress' object
void __visible
stepcompress_free(struct stepcompress *sc)
@@ -322,9 +327,6 @@ 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
@@ -623,6 +625,21 @@ stepcompress_queue_msg(struct stepcompress *sc, uint32_t *data, int len)
return 0;
}
// Queue an mcu command that will consume space in the mcu move queue
int __visible
stepcompress_queue_mq_msg(struct stepcompress *sc, uint64_t req_clock
, 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->min_clock = qm->req_clock = req_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
@@ -716,6 +733,18 @@ steppersync_set_time(struct steppersync *ss, double time_offset
}
}
// Expire the stepcompress history before the given clock time
static void
steppersync_history_expire(struct steppersync *ss, uint64_t end_clock)
{
int i;
for (i = 0; i < ss->sc_num; i++)
{
struct stepcompress *sc = ss->sc_list[i];
stepcompress_history_expire(sc, end_clock);
}
}
// Implement a binary heap algorithm to track when the next available
// 'struct move' in the mcu will be available
static void
@@ -743,7 +772,8 @@ heap_replace(struct steppersync *ss, uint64_t req_clock)
// Find and transmit any scheduled steps prior to the given 'move_clock'
int __visible
steppersync_flush(struct steppersync *ss, uint64_t move_clock)
steppersync_flush(struct steppersync *ss, uint64_t move_clock
, uint64_t clear_history_clock)
{
// Flush each stepcompress to the specified move_clock
int i;
@@ -791,5 +821,7 @@ steppersync_flush(struct steppersync *ss, uint64_t move_clock)
// Transmit commands
if (!list_empty(&msgs))
serialqueue_send_batch(ss->sq, ss->cq, &msgs);
steppersync_history_expire(ss, clear_history_clock);
return 0;
}

5
klippy/chelper/stepcompress.h
View File

@@ -29,6 +29,8 @@ int stepcompress_set_last_position(struct stepcompress *sc, uint64_t clock
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_queue_mq_msg(struct stepcompress *sc, uint64_t req_clock
, 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);
@@ -40,6 +42,7 @@ struct steppersync *steppersync_alloc(
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);
int steppersync_flush(struct steppersync *ss, uint64_t move_clock
, uint64_t clear_history_clock);
#endif // stepcompress.h

104
klippy/chelper/trapq.c
View File

@@ -20,54 +20,6 @@ move_alloc(void)
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)
@@ -163,11 +115,58 @@ trapq_add_move(struct trapq *tq, struct move *m)
tail_sentinel->print_time = 0.;
}
#define HISTORY_EXPIRE (30.0)
// 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);
}
}
// Expire any moves older than `print_time` from the trapezoid velocity queue
void __visible
trapq_finalize_moves(struct trapq *tq, double print_time)
trapq_finalize_moves(struct trapq *tq, double print_time
, double clear_history_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);
@@ -190,10 +189,9 @@ trapq_finalize_moves(struct trapq *tq, double print_time)
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)
if (m == latest || m->print_time + m->move_t > clear_history_time)
break;
list_del(&m->node);
free(m);
@@ -206,7 +204,7 @@ 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);
trapq_finalize_moves(tq, NEVER_TIME, 0);
// Prune any moves in the trapq history that were interrupted
while (!list_empty(&tq->history)) {

13
klippy/chelper/trapq.h
View File

@@ -32,18 +32,19 @@ struct pull_move {
};
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_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);
void trapq_finalize_moves(struct trapq *tq, double print_time
, double clear_history_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