Mirror
/
klipper-dgus
mirror of https://github.com/Desuuuu/klipper.git
1
0
Fork 0
Code Issues Projects Releases Wiki Activity

serialqueue: Improve canbus timing 

Adjust timing based on the minimum transmission time of canbus
messages.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor 2022-07-25 20:50:19 -04:00
parent 2293e1506f
commit db6346e7e5
5 changed files with 58 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
klippy/chelper/__init__.py
View File

@ -168,8 +168,8 @@ defs_serialqueue = """
, uint64_t notify_id); , uint64_t notify_id);
void serialqueue_pull(struct serialqueue *sq void serialqueue_pull(struct serialqueue *sq
, struct pull_queue_message *pqm); , struct pull_queue_message *pqm);
void serialqueue_set_baud_adjust(struct serialqueue *sq void serialqueue_set_wire_frequency(struct serialqueue *sq
, double baud_adjust); , double frequency);
void serialqueue_set_receive_window(struct serialqueue *sq void serialqueue_set_receive_window(struct serialqueue *sq
, int receive_window); , int receive_window);
void serialqueue_set_clock_est(struct serialqueue *sq, double est_freq void serialqueue_set_clock_est(struct serialqueue *sq, double est_freq

63
klippy/chelper/serialqueue.c
View File

@ -49,7 +49,7 @@ struct serialqueue {
int receive_waiting; int receive_waiting;
// Baud / clock tracking // Baud / clock tracking
int receive_window; int receive_window;
double baud_adjust, idle_time; double bittime_adjust, idle_time;
struct clock_estimate ce; struct clock_estimate ce;
double last_receive_sent_time; double last_receive_sent_time;
// Retransmit support // Retransmit support
@ -136,6 +136,23 @@ kick_bg_thread(struct serialqueue *sq)
report_errno("pipe write", ret); 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 // Update internal state when the receive sequence increases
static void static void
update_receive_seq(struct serialqueue *sq, double eventtime, uint64_t rseq) update_receive_seq(struct serialqueue *sq, double eventtime, uint64_t rseq)
@ -192,7 +209,7 @@ update_receive_seq(struct serialqueue *sq, double eventtime, uint64_t rseq)
} else { } else {
struct queue_message *sent = list_first_entry( struct queue_message *sent = list_first_entry(
&sq->sent_queue, struct queue_message, node); &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); pollreactor_update_timer(sq->pr, SQPT_RETRANSMIT, nr);
} }
} }
@ -251,7 +268,7 @@ handle_message(struct serialqueue *sq, double eventtime, int len)
qm->sent_time = (rseq > sq->retransmit_seq qm->sent_time = (rseq > sq->retransmit_seq
? sq->last_receive_sent_time : 0.); ? sq->last_receive_sent_time : 0.);
qm->receive_time = get_monotonic(); // must be time post read() 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); list_add_tail(&qm->node, &sq->receive_queue);
must_wake = 1; must_wake = 1;
} }
@ -407,8 +424,8 @@ retransmit_event(struct serialqueue *sq, double eventtime)
} }
sq->retransmit_seq = sq->send_seq; sq->retransmit_seq = sq->send_seq;
sq->rtt_sample_seq = 0; sq->rtt_sample_seq = 0;
sq->idle_time = eventtime + buflen * sq->baud_adjust; sq->idle_time = eventtime + calculate_bittime(sq, buflen);
double waketime = eventtime + first_buflen * sq->baud_adjust + sq->rto; double waketime = eventtime + sq->rto + calculate_bittime(sq, first_buflen);
pthread_mutex_unlock(&sq->lock); pthread_mutex_unlock(&sq->lock);
return waketime; return waketime;
@ -416,7 +433,8 @@ retransmit_event(struct serialqueue *sq, double eventtime)
// Construct a block of data to be sent to the serial port // Construct a block of data to be sent to the serial port
static int 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; int len = MESSAGE_HEADER_SIZE;
while (sq->ready_bytes) { while (sq->ready_bytes) {
@ -463,17 +481,15 @@ build_and_send_command(struct serialqueue *sq, uint8_t *buf, double eventtime)
buf[len - MESSAGE_TRAILER_SYNC] = MESSAGE_SYNC; buf[len - MESSAGE_TRAILER_SYNC] = MESSAGE_SYNC;
// Store message block // Store message block
if (eventtime > sq->idle_time) double idletime = eventtime > sq->idle_time ? eventtime : sq->idle_time;
sq->idle_time = eventtime; idletime += calculate_bittime(sq, pending + len);
sq->idle_time += len * sq->baud_adjust;
struct queue_message *out = message_alloc(); struct queue_message *out = message_alloc();
memcpy(out->msg, buf, len); memcpy(out->msg, buf, len);
out->len = len; out->len = len;
out->sent_time = eventtime; out->sent_time = eventtime;
out->receive_time = sq->idle_time; out->receive_time = idletime;
if (list_empty(&sq->sent_queue)) if (list_empty(&sq->sent_queue))
pollreactor_update_timer(sq->pr, SQPT_RETRANSMIT pollreactor_update_timer(sq->pr, SQPT_RETRANSMIT, idletime + sq->rto);
, sq->idle_time + sq->rto);
if (!sq->rtt_sample_seq) if (!sq->rtt_sample_seq)
sq->rtt_sample_seq = sq->send_seq; sq->rtt_sample_seq = sq->send_seq;
sq->send_seq++; sq->send_seq++;
@ -484,7 +500,7 @@ build_and_send_command(struct serialqueue *sq, uint8_t *buf, double eventtime)
// Determine the time the next serial data should be sent // Determine the time the next serial data should be sent
static double 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 if (sq->send_seq - sq->receive_seq >= MAX_PENDING_BLOCKS
&& sq->receive_seq != (uint64_t)-1) && sq->receive_seq != (uint64_t)-1)
@ -501,7 +517,7 @@ check_send_command(struct serialqueue *sq, double eventtime)
// Check for stalled messages now ready // Check for stalled messages now ready
double idletime = eventtime > sq->idle_time ? eventtime : sq->idle_time; 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 ack_clock = clock_from_time(&sq->ce, idletime);
uint64_t min_stalled_clock = MAX_CLOCK, min_ready_clock = MAX_CLOCK; uint64_t min_stalled_clock = MAX_CLOCK, min_ready_clock = MAX_CLOCK;
struct command_queue *cq; struct command_queue *cq;
@ -525,9 +541,10 @@ check_send_command(struct serialqueue *sq, double eventtime)
struct queue_message *qm = list_first_entry( struct queue_message *qm = list_first_entry(
&cq->ready_queue, struct queue_message, node); &cq->ready_queue, struct queue_message, node);
uint64_t req_clock = qm->req_clock; uint64_t req_clock = qm->req_clock;
double bgtime = pending ? idletime : sq->idle_time;
double bgoffset = MIN_REQTIME_DELTA + MIN_BACKGROUND_DELTA; double bgoffset = MIN_REQTIME_DELTA + MIN_BACKGROUND_DELTA;
if (req_clock == BACKGROUND_PRIORITY_CLOCK) 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) if (req_clock < min_ready_clock)
min_ready_clock = req_clock; min_ready_clock = req_clock;
} }
@ -561,18 +578,21 @@ command_event(struct serialqueue *sq, double eventtime)
int buflen = 0; int buflen = 0;
double waketime; double waketime;
for (;;) { for (;;) {
waketime = check_send_command(sq, eventtime); waketime = check_send_command(sq, buflen, eventtime);
if (waketime != PR_NOW || buflen + MESSAGE_MAX > sizeof(buf)) { if (waketime != PR_NOW || buflen + MESSAGE_MAX > sizeof(buf)) {
if (buflen) { if (buflen) {
// Write message blocks // Write message blocks
do_write(sq, buf, buflen); do_write(sq, buf, buflen);
sq->bytes_write += 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; buflen = 0;
} }
if (waketime != PR_NOW) if (waketime != PR_NOW)
break; 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); pthread_mutex_unlock(&sq->lock);
return waketime; return waketime;
@ -847,10 +867,15 @@ exit:
} }
void __visible 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); 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); pthread_mutex_unlock(&sq->lock);
} }

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 , uint8_t *msg, int len, uint64_t min_clock
, uint64_t req_clock, uint64_t notify_id); , uint64_t req_clock, uint64_t notify_id);
void serialqueue_pull(struct serialqueue *sq, struct pull_queue_message *pqm); 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_receive_window(struct serialqueue *sq, int receive_window);
void serialqueue_set_clock_est(struct serialqueue *sq, double est_freq void serialqueue_set_clock_est(struct serialqueue *sq, double est_freq
, double conv_time, uint64_t conv_clock , double conv_time, uint64_t conv_clock

13
klippy/serialhdl.py
View File

@ -12,7 +12,6 @@ class error(Exception):
pass pass
class SerialReader: class SerialReader:
BITS_PER_BYTE = 10.
def __init__(self, reactor, warn_prefix=""): def __init__(self, reactor, warn_prefix=""):
self.reactor = reactor self.reactor = reactor
self.warn_prefix = warn_prefix self.warn_prefix = warn_prefix
@ -97,11 +96,13 @@ class SerialReader:
self.msgparser = msgparser self.msgparser = msgparser
self.register_response(self.handle_unknown, '#unknown') self.register_response(self.handle_unknown, '#unknown')
# Setup baud adjust # Setup baud adjust
mcu_baud = msgparser.get_constant_float('SERIAL_BAUD', None) if serial_fd_type == b'c':
if mcu_baud is not None: wire_freq = msgparser.get_constant_float('CANBUS_FREQUENCY', None)
baud_adjust = self.BITS_PER_BYTE / mcu_baud else:
self.ffi_lib.serialqueue_set_baud_adjust( wire_freq = msgparser.get_constant_float('SERIAL_BAUD', None)
self.serialqueue, baud_adjust) if wire_freq is not None:
self.ffi_lib.serialqueue_set_wire_frequency(self.serialqueue,
wire_freq)
receive_window = msgparser.get_constant_int('RECEIVE_WINDOW', None) receive_window = msgparser.get_constant_int('RECEIVE_WINDOW', None)
if receive_window is not None: if receive_window is not None:
self.ffi_lib.serialqueue_set_receive_window( self.ffi_lib.serialqueue_set_receive_window(

4
src/generic/canbus.c
View File

@ -4,8 +4,12 @@
// //
// This file may be distributed under the terms of the GNU GPLv3 license. // This file may be distributed under the terms of the GNU GPLv3 license.
#include "autoconf.h" // CONFIG_CANBUS_FREQUENCY
#include "canbus.h" // canbus_send #include "canbus.h" // canbus_send
#include "canserial.h" // canserial_send #include "canserial.h" // canserial_send
#include "command.h" // DECL_CONSTANT
DECL_CONSTANT("CANBUS_FREQUENCY", CONFIG_CANBUS_FREQUENCY);
int int
canserial_send(struct canbus_msg *msg) canserial_send(struct canbus_msg *msg)