3diy
/
klipper
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

msgblock: Add clock estimation helper functions 

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor 2021-02-15 19:18:51 -05:00
parent f938caa0d2
commit 620f77ddb7
7 changed files with 65 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
klippy/chelper/__init__.py
View File

@ -162,7 +162,7 @@ defs_serialqueue = """
void serialqueue_set_receive_window(struct serialqueue *sq
, int receive_window);
void serialqueue_set_clock_est(struct serialqueue *sq, double est_freq
, double last_clock_time, uint64_t last_clock);
, 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);

26
klippy/chelper/msgblock.c
View File

@ -181,3 +181,29 @@ message_queue_free(struct list_head *root)
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;
}

8
klippy/chelper/msgblock.h
View File

@ -34,6 +34,11 @@ struct queue_message {
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);
@ -42,5 +47,8 @@ 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

38
klippy/chelper/serialqueue.c
View File

@ -50,8 +50,7 @@ struct serialqueue {
// Baud / clock tracking
int receive_window;
double baud_adjust, idle_time;
double est_freq, last_clock_time;
uint64_t last_clock;
struct clock_estimate ce;
double last_receive_sent_time;
// Retransmit support
uint64_t send_seq, receive_seq;
@ -483,9 +482,7 @@ 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;
double timedelta = idletime - sq->last_clock_time;
uint64_t ack_clock = ((uint64_t)(timedelta * sq->est_freq)
+ sq->last_clock);
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) {
@ -508,11 +505,9 @@ 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 bgoffset = MIN_REQTIME_DELTA + MIN_BACKGROUND_DELTA;
if (req_clock == BACKGROUND_PRIORITY_CLOCK)
req_clock = (uint64_t)(
(sq->idle_time - sq->last_clock_time
+ MIN_REQTIME_DELTA + MIN_BACKGROUND_DELTA)
* sq->est_freq) + sq->last_clock;
req_clock = clock_from_time(&sq->ce, sq->idle_time + bgoffset);
if (req_clock < min_ready_clock)
min_ready_clock = req_clock;
}
@ -521,20 +516,20 @@ check_send_command(struct serialqueue *sq, double eventtime)
// Check for messages to send
if (sq->ready_bytes >= MESSAGE_PAYLOAD_MAX)
return PR_NOW;
if (! sq->est_freq) {
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->est_freq;
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->est_freq;
return idletime + (wantclock - ack_clock) / sq->ce.est_freq;
}
// Callback timer to send data to the serial port
@ -819,12 +814,23 @@ serialqueue_set_receive_window(struct serialqueue *sq, int receive_window)
// serial port
void __visible
serialqueue_set_clock_est(struct serialqueue *sq, double est_freq
, double last_clock_time, uint64_t last_clock)
, double conv_time, uint64_t conv_clock
, uint64_t last_clock)
{
pthread_mutex_lock(&sq->lock);
sq->est_freq = est_freq;
sq->last_clock_time = last_clock_time;
sq->last_clock = last_clock;
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);
}

5
klippy/chelper/serialqueue.h
View File

@ -30,7 +30,10 @@ 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 last_clock_time, uint64_t last_clock);
, 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);

4
klippy/clocksync.py
View File

@ -54,7 +54,7 @@ class ClockSync:
freq = 1000000000000.
if pace:
freq = self.mcu_freq
serial.set_clock_est(freq, self.reactor.monotonic(), 0)
serial.set_clock_est(freq, self.reactor.monotonic(), 0, 0)
# MCU clock querying (_handle_clock is invoked from background thread)
def _get_clock_event(self, eventtime):
self.serial.raw_send(self.get_clock_cmd, 0, 0, self.cmd_queue)
@ -116,7 +116,7 @@ class ClockSync:
new_freq = self.clock_covariance / self.time_variance
pred_stddev = math.sqrt(self.prediction_variance)
self.serial.set_clock_est(new_freq, self.time_avg + TRANSMIT_EXTRA,
int(self.clock_avg - 3. * pred_stddev))
int(self.clock_avg - 3. * pred_stddev), clock)
self.clock_est = (self.time_avg + self.min_half_rtt,
self.clock_avg, new_freq)
#logging.debug("regr %.3f: freq=%.3f d=%d(%.3f)",

4
klippy/serialhdl.py
View File

@ -200,9 +200,9 @@ class SerialReader:
self.serialqueue = self.ffi_main.gc(
self.ffi_lib.serialqueue_alloc(self.serial_dev.fileno(), 'f', 0),
self.ffi_lib.serialqueue_free)
def set_clock_est(self, freq, last_time, last_clock):
def set_clock_est(self, freq, conv_time, conv_clock, last_clock):
self.ffi_lib.serialqueue_set_clock_est(
self.serialqueue, freq, last_time, last_clock)
self.serialqueue, freq, conv_time, conv_clock, last_clock)
def disconnect(self):
if self.serialqueue is not None:
self.ffi_lib.serialqueue_exit(self.serialqueue)