2016-05-25 18:37:40 +03:00
|
|
|
// Stepper pulse schedule compression
|
|
|
|
//
|
|
|
|
// Copyright (C) 2016 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 writtin 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
|
2016-12-13 18:44:26 +03:00
|
|
|
#include <stdio.h> // fprintf
|
2016-05-25 18:37:40 +03:00
|
|
|
#include <stdlib.h> // malloc
|
|
|
|
#include <string.h> // memset
|
2016-11-30 20:04:28 +03:00
|
|
|
#include "pyhelper.h" // errorf
|
2016-05-25 18:37:40 +03:00
|
|
|
#include "serialqueue.h" // struct queue_message
|
|
|
|
|
|
|
|
#define CHECK_LINES 1
|
|
|
|
#define QUEUE_START_SIZE 1024
|
|
|
|
|
|
|
|
struct stepcompress {
|
|
|
|
// Buffer management
|
2016-11-02 02:38:37 +03:00
|
|
|
uint64_t *queue, *queue_end, *queue_pos, *queue_next;
|
2016-05-25 18:37:40 +03:00
|
|
|
// Internal tracking
|
2016-11-02 02:38:37 +03:00
|
|
|
uint32_t max_error;
|
2016-05-25 18:37:40 +03:00
|
|
|
// Error checking
|
|
|
|
uint32_t errors;
|
|
|
|
// Message generation
|
2016-12-31 01:02:28 +03:00
|
|
|
uint64_t last_step_clock, homing_clock;
|
2016-05-25 18:37:40 +03:00
|
|
|
struct list_head msg_queue;
|
2016-11-10 20:44:04 +03:00
|
|
|
uint32_t queue_step_msgid, set_next_step_dir_msgid, oid;
|
|
|
|
int sdir, invert_sdir;
|
2016-05-25 18:37:40 +03:00
|
|
|
};
|
|
|
|
|
|
|
|
|
|
|
|
/****************************************************************
|
|
|
|
* Queue management
|
|
|
|
****************************************************************/
|
|
|
|
|
|
|
|
// Shuffle the internal queue to avoid having to allocate more ram
|
|
|
|
static void
|
|
|
|
clean_queue(struct stepcompress *sc)
|
|
|
|
{
|
2016-11-02 02:38:37 +03:00
|
|
|
int in_use = sc->queue_next - sc->queue_pos;
|
|
|
|
memmove(sc->queue, sc->queue_pos, in_use * sizeof(*sc->queue));
|
2016-05-25 18:37:40 +03:00
|
|
|
sc->queue_pos = sc->queue;
|
2016-11-02 02:38:37 +03:00
|
|
|
sc->queue_next = sc->queue + in_use;
|
2016-05-25 18:37:40 +03:00
|
|
|
}
|
|
|
|
|
|
|
|
// Expand the internal queue of step times
|
|
|
|
static void
|
|
|
|
expand_queue(struct stepcompress *sc, int count)
|
|
|
|
{
|
2016-07-17 04:25:30 +03:00
|
|
|
int alloc = sc->queue_end - sc->queue;
|
|
|
|
if (count + sc->queue_next - sc->queue_pos <= alloc) {
|
2016-05-25 18:37:40 +03:00
|
|
|
clean_queue(sc);
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
int pos = sc->queue_pos - sc->queue;
|
|
|
|
int next = sc->queue_next - sc->queue;
|
|
|
|
if (!alloc)
|
|
|
|
alloc = QUEUE_START_SIZE;
|
|
|
|
while (next + count > alloc)
|
|
|
|
alloc *= 2;
|
|
|
|
sc->queue = realloc(sc->queue, alloc * sizeof(*sc->queue));
|
|
|
|
sc->queue_end = sc->queue + alloc;
|
|
|
|
sc->queue_pos = sc->queue + pos;
|
|
|
|
sc->queue_next = sc->queue + next;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/****************************************************************
|
|
|
|
* Step compression
|
|
|
|
****************************************************************/
|
|
|
|
|
|
|
|
#define DIV_UP(n,d) (((n) + (d) - 1) / (d))
|
|
|
|
|
|
|
|
static inline int32_t
|
|
|
|
idiv_up(int32_t n, int32_t d)
|
|
|
|
{
|
|
|
|
return (n>=0) ? DIV_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
|
2016-12-13 18:44:26 +03:00
|
|
|
static inline struct points
|
2016-11-02 02:38:37 +03:00
|
|
|
minmax_point(struct stepcompress *sc, uint64_t *pos)
|
2016-05-25 18:37:40 +03:00
|
|
|
{
|
2016-11-02 02:38:37 +03:00
|
|
|
uint32_t prevpoint = pos > sc->queue_pos ? *(pos-1) - sc->last_step_clock : 0;
|
|
|
|
uint32_t point = *pos - sc->last_step_clock;
|
2016-05-25 18:37:40 +03:00
|
|
|
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
|
2016-12-14 18:59:15 +03:00
|
|
|
// using 11 works well in practice.
|
2016-05-25 18:37:40 +03:00
|
|
|
#define QUADRATIC_DEV 11
|
|
|
|
|
|
|
|
struct step_move {
|
|
|
|
uint32_t interval;
|
|
|
|
uint16_t count;
|
|
|
|
int16_t add;
|
|
|
|
};
|
|
|
|
|
|
|
|
// Find a 'step_move' that covers a series of step times
|
|
|
|
static struct step_move
|
|
|
|
compress_bisect_add(struct stepcompress *sc)
|
|
|
|
{
|
|
|
|
struct points point = minmax_point(sc, sc->queue_pos);
|
2016-09-22 23:40:00 +03:00
|
|
|
int32_t outer_mininterval = point.minp, outer_maxinterval = point.maxp;
|
2016-12-14 18:59:15 +03:00
|
|
|
int32_t add = 0, minadd = -0x8000, maxadd = 0x7fff;
|
2016-12-07 03:01:00 +03:00
|
|
|
int32_t bestinterval = 0, bestcount = 1, bestadd = 1, bestreach = INT32_MIN;
|
2016-12-19 05:56:30 +03:00
|
|
|
int32_t zerointerval = 0, zerocount = 0;
|
2016-05-25 18:37:40 +03:00
|
|
|
|
|
|
|
for (;;) {
|
|
|
|
// Find longest valid sequence with the given 'add'
|
2016-12-13 18:44:26 +03:00
|
|
|
struct points nextpoint;
|
|
|
|
int32_t nextmininterval = outer_mininterval;
|
|
|
|
int32_t nextmaxinterval = outer_maxinterval, interval = nextmaxinterval;
|
|
|
|
int32_t nextcount = 1;
|
2016-05-25 18:37:40 +03:00
|
|
|
for (;;) {
|
2016-12-13 18:44:26 +03:00
|
|
|
nextcount++;
|
|
|
|
if (nextcount > bestcount
|
|
|
|
&& (&sc->queue_pos[nextcount-1] >= sc->queue_next
|
|
|
|
|| sc->queue_pos[nextcount-1] >= sc->last_step_clock+(3<<28)
|
|
|
|
|| nextcount > 65535)) {
|
|
|
|
int32_t count = nextcount - 1;
|
|
|
|
return (struct step_move){ interval, count, add };
|
2016-11-02 06:08:16 +03:00
|
|
|
}
|
2016-12-13 18:44:26 +03:00
|
|
|
nextpoint = minmax_point(sc, sc->queue_pos + nextcount - 1);
|
2016-12-26 20:47:17 +03:00
|
|
|
int32_t nextaddfactor = nextcount*(nextcount-1)/2;
|
2016-12-13 18:44:26 +03:00
|
|
|
int32_t c = add*nextaddfactor;
|
|
|
|
if (nextmininterval*nextcount < nextpoint.minp - c)
|
|
|
|
nextmininterval = DIV_UP(nextpoint.minp - c, nextcount);
|
|
|
|
if (nextmaxinterval*nextcount > nextpoint.maxp - c)
|
|
|
|
nextmaxinterval = (nextpoint.maxp - c) / nextcount;
|
2016-05-25 18:37:40 +03:00
|
|
|
if (nextmininterval > nextmaxinterval)
|
|
|
|
break;
|
2016-12-13 18:44:26 +03:00
|
|
|
interval = nextmaxinterval;
|
2016-05-25 18:37:40 +03:00
|
|
|
}
|
2016-09-22 23:40:00 +03:00
|
|
|
|
|
|
|
// Check if this is the best sequence found so far
|
2016-12-26 20:47:17 +03:00
|
|
|
int32_t count = nextcount - 1, addfactor = count*(count-1)/2;
|
2016-12-13 18:44:26 +03:00
|
|
|
int32_t reach = add*addfactor + interval*count;
|
2016-12-19 06:11:20 +03:00
|
|
|
if (reach > bestreach
|
|
|
|
|| (reach == bestreach && interval > bestinterval)) {
|
2016-12-13 18:44:26 +03:00
|
|
|
bestinterval = interval;
|
2016-05-25 18:37:40 +03:00
|
|
|
bestcount = count;
|
|
|
|
bestadd = add;
|
2016-09-22 23:40:00 +03:00
|
|
|
bestreach = reach;
|
2016-12-19 05:56:30 +03:00
|
|
|
if (!add) {
|
|
|
|
zerointerval = interval;
|
|
|
|
zerocount = count;
|
|
|
|
}
|
2016-12-26 20:47:17 +03:00
|
|
|
if (count > 0x200)
|
|
|
|
// No 'add' will improve sequence; avoid integer overflow
|
|
|
|
break;
|
2016-05-25 18:37:40 +03:00
|
|
|
}
|
|
|
|
|
|
|
|
// Check if a greater or lesser add could extend the sequence
|
2016-12-26 20:47:17 +03:00
|
|
|
int32_t nextaddfactor = nextcount*(nextcount-1)/2;
|
2016-12-13 18:44:26 +03:00
|
|
|
int32_t nextreach = add*nextaddfactor + interval*nextcount;
|
|
|
|
if (nextreach < nextpoint.minp) {
|
2016-12-14 18:59:15 +03:00
|
|
|
minadd = add + 1;
|
2016-12-13 18:44:26 +03:00
|
|
|
outer_maxinterval = nextmaxinterval;
|
2016-07-06 20:19:21 +03:00
|
|
|
} else {
|
2016-12-14 18:59:15 +03:00
|
|
|
maxadd = add - 1;
|
2016-12-13 18:44:26 +03:00
|
|
|
outer_mininterval = nextmininterval;
|
2016-07-06 20:19:21 +03:00
|
|
|
}
|
2016-05-25 18:37:40 +03:00
|
|
|
|
|
|
|
// The maximum valid deviation between two quadratic sequences
|
|
|
|
// can be calculated and used to further limit the add range.
|
|
|
|
if (count > 1) {
|
2016-12-14 18:59:15 +03:00
|
|
|
int32_t errdelta = sc->max_error*QUADRATIC_DEV / (count*count);
|
2016-05-25 18:37:40 +03:00
|
|
|
if (minadd < add - errdelta)
|
|
|
|
minadd = add - errdelta;
|
|
|
|
if (maxadd > add + errdelta)
|
|
|
|
maxadd = add + errdelta;
|
|
|
|
}
|
|
|
|
|
2016-09-22 23:40:00 +03:00
|
|
|
// See if next point would further limit the add range
|
2016-12-13 18:44:26 +03:00
|
|
|
int32_t c = outer_maxinterval * nextcount;
|
2016-12-14 18:59:15 +03:00
|
|
|
if (minadd*nextaddfactor < nextpoint.minp - c)
|
|
|
|
minadd = idiv_up(nextpoint.minp - c, nextaddfactor);
|
2016-12-13 18:44:26 +03:00
|
|
|
c = outer_mininterval * nextcount;
|
2016-12-14 18:59:15 +03:00
|
|
|
if (maxadd*nextaddfactor > nextpoint.maxp - c)
|
|
|
|
maxadd = idiv_down(nextpoint.maxp - c, nextaddfactor);
|
2016-09-22 23:40:00 +03:00
|
|
|
|
2016-05-25 18:37:40 +03:00
|
|
|
// Bisect valid add range and try again with new 'add'
|
2016-12-14 18:59:15 +03:00
|
|
|
if (minadd > maxadd)
|
2016-05-25 18:37:40 +03:00
|
|
|
break;
|
2016-12-13 18:53:13 +03:00
|
|
|
add = maxadd - (maxadd - minadd) / 4;
|
2016-05-25 18:37:40 +03:00
|
|
|
}
|
2016-12-19 05:56:30 +03:00
|
|
|
if (zerocount + zerocount/16 >= bestcount)
|
|
|
|
// Prefer add=0 if it's similar to the best found sequence
|
|
|
|
return (struct step_move){ zerointerval, zerocount, 0 };
|
2016-05-25 18:37:40 +03:00
|
|
|
return (struct step_move){ bestinterval, bestcount, bestadd };
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/****************************************************************
|
|
|
|
* Step compress checking
|
|
|
|
****************************************************************/
|
|
|
|
|
|
|
|
// Verify that a given 'step_move' matches the actual step times
|
|
|
|
static void
|
|
|
|
check_line(struct stepcompress *sc, struct step_move move)
|
|
|
|
{
|
|
|
|
if (!CHECK_LINES)
|
|
|
|
return;
|
2016-11-02 06:08:16 +03:00
|
|
|
if (move.count == 1) {
|
|
|
|
if (move.interval != (uint32_t)(*sc->queue_pos - sc->last_step_clock)
|
|
|
|
|| *sc->queue_pos < sc->last_step_clock) {
|
2016-11-30 20:04:28 +03:00
|
|
|
errorf("Count 1 point out of range: %d %d %d"
|
|
|
|
, move.interval, move.count, move.add);
|
2016-11-02 06:08:16 +03:00
|
|
|
sc->errors++;
|
|
|
|
}
|
|
|
|
return;
|
|
|
|
}
|
2016-05-25 18:37:40 +03:00
|
|
|
int err = 0;
|
2016-12-13 18:46:46 +03:00
|
|
|
if (!move.count || (!move.interval && !move.add)
|
|
|
|
|| move.interval >= 0x80000000) {
|
2016-11-30 20:04:28 +03:00
|
|
|
errorf("Point out of range: %d %d %d"
|
|
|
|
, move.interval, move.count, move.add);
|
2016-05-25 18:37:40 +03:00
|
|
|
err++;
|
|
|
|
}
|
2016-11-02 06:08:16 +03:00
|
|
|
uint32_t interval = move.interval, p = 0;
|
2016-05-25 18:37:40 +03:00
|
|
|
uint16_t i;
|
|
|
|
for (i=0; i<move.count; i++) {
|
|
|
|
struct points point = minmax_point(sc, sc->queue_pos + i);
|
2016-11-02 06:08:16 +03:00
|
|
|
p += interval;
|
2016-05-25 18:37:40 +03:00
|
|
|
if (p < point.minp || p > point.maxp) {
|
2016-11-30 20:04:28 +03:00
|
|
|
errorf("Point %d of %d: %d not in %d:%d"
|
|
|
|
, i+1, move.count, p, point.minp, point.maxp);
|
2016-05-25 18:37:40 +03:00
|
|
|
err++;
|
|
|
|
}
|
2016-11-02 06:08:16 +03:00
|
|
|
if (interval >= 0x80000000) {
|
2016-11-30 20:04:28 +03:00
|
|
|
errorf("Point %d of %d: interval overflow %d"
|
|
|
|
, i+1, move.count, interval);
|
2016-11-02 06:08:16 +03:00
|
|
|
err++;
|
|
|
|
}
|
2016-05-25 18:37:40 +03:00
|
|
|
interval += move.add;
|
|
|
|
}
|
|
|
|
sc->errors += err;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/****************************************************************
|
|
|
|
* Step compress interface
|
|
|
|
****************************************************************/
|
|
|
|
|
2016-11-14 23:12:53 +03:00
|
|
|
#define likely(x) __builtin_expect(!!(x), 1)
|
|
|
|
|
2016-10-10 03:57:55 +03:00
|
|
|
// Wrapper around sqrt() to handle small negative numbers
|
2016-11-14 23:12:53 +03:00
|
|
|
static double
|
|
|
|
_safe_sqrt(double v)
|
2016-10-10 03:57:55 +03:00
|
|
|
{
|
2016-11-14 23:12:53 +03:00
|
|
|
if (v > -0.001)
|
2016-10-10 03:57:55 +03:00
|
|
|
// Due to floating point truncation, it's possible to get a
|
|
|
|
// small negative number - treat it as zero.
|
|
|
|
return 0.;
|
|
|
|
return sqrt(v);
|
|
|
|
}
|
2016-11-14 23:12:53 +03:00
|
|
|
static inline double safe_sqrt(double v) {
|
|
|
|
return likely(v >= 0.) ? sqrt(v) : _safe_sqrt(v);
|
|
|
|
}
|
2016-10-10 03:57:55 +03:00
|
|
|
|
2016-05-25 18:37:40 +03:00
|
|
|
// Allocate a new 'stepcompress' object
|
|
|
|
struct stepcompress *
|
2016-11-10 20:44:04 +03:00
|
|
|
stepcompress_alloc(uint32_t max_error, uint32_t queue_step_msgid
|
|
|
|
, uint32_t set_next_step_dir_msgid, uint32_t invert_sdir
|
|
|
|
, uint32_t oid)
|
2016-05-25 18:37:40 +03:00
|
|
|
{
|
|
|
|
struct stepcompress *sc = malloc(sizeof(*sc));
|
|
|
|
memset(sc, 0, sizeof(*sc));
|
|
|
|
sc->max_error = max_error;
|
|
|
|
list_init(&sc->msg_queue);
|
|
|
|
sc->queue_step_msgid = queue_step_msgid;
|
2016-11-10 20:44:04 +03:00
|
|
|
sc->set_next_step_dir_msgid = set_next_step_dir_msgid;
|
2016-05-25 18:37:40 +03:00
|
|
|
sc->oid = oid;
|
2016-11-10 20:44:04 +03:00
|
|
|
sc->sdir = -1;
|
|
|
|
sc->invert_sdir = !!invert_sdir;
|
2016-05-25 18:37:40 +03:00
|
|
|
return sc;
|
|
|
|
}
|
|
|
|
|
2016-11-30 09:58:45 +03:00
|
|
|
// Free memory associated with a 'stepcompress' object
|
|
|
|
void
|
|
|
|
stepcompress_free(struct stepcompress *sc)
|
|
|
|
{
|
|
|
|
if (!sc)
|
|
|
|
return;
|
|
|
|
free(sc->queue);
|
|
|
|
message_queue_free(&sc->msg_queue);
|
|
|
|
free(sc);
|
|
|
|
}
|
|
|
|
|
2016-11-13 06:55:27 +03:00
|
|
|
// Convert previously scheduled steps into commands for the mcu
|
|
|
|
static void
|
|
|
|
stepcompress_flush(struct stepcompress *sc, uint64_t move_clock)
|
|
|
|
{
|
|
|
|
if (sc->queue_pos >= sc->queue_next)
|
|
|
|
return;
|
|
|
|
while (move_clock > sc->last_step_clock) {
|
|
|
|
struct step_move move = compress_bisect_add(sc);
|
|
|
|
check_line(sc, move);
|
|
|
|
|
|
|
|
uint32_t msg[5] = {
|
|
|
|
sc->queue_step_msgid, 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 && sc->last_step_clock + (1<<27) < *sc->queue_pos) {
|
|
|
|
// Be careful with 32bit overflow
|
|
|
|
sc->last_step_clock = qm->req_clock = *sc->queue_pos;
|
|
|
|
} else {
|
2016-12-26 20:47:17 +03:00
|
|
|
int32_t addfactor = move.count*(move.count-1)/2;
|
2016-11-13 06:55:27 +03:00
|
|
|
uint32_t ticks = move.add*addfactor + move.interval*move.count;
|
|
|
|
sc->last_step_clock += ticks;
|
|
|
|
}
|
2016-12-31 01:02:28 +03:00
|
|
|
if (sc->homing_clock)
|
|
|
|
// When homing, all steps should be sent prior to homing_clock
|
|
|
|
qm->min_clock = qm->req_clock = sc->homing_clock;
|
2016-11-13 06:55:27 +03:00
|
|
|
list_add_tail(&qm->node, &sc->msg_queue);
|
|
|
|
|
|
|
|
if (sc->queue_pos + move.count >= sc->queue_next) {
|
|
|
|
sc->queue_pos = sc->queue_next = sc->queue;
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
sc->queue_pos += move.count;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2016-11-10 20:44:04 +03:00
|
|
|
// Send the set_next_step_dir command
|
|
|
|
static void
|
|
|
|
set_next_step_dir(struct stepcompress *sc, int sdir)
|
|
|
|
{
|
2016-12-31 21:21:53 +03:00
|
|
|
if (sc->sdir == sdir)
|
|
|
|
return;
|
2016-11-10 20:44:04 +03:00
|
|
|
sc->sdir = sdir;
|
|
|
|
stepcompress_flush(sc, UINT64_MAX);
|
|
|
|
uint32_t msg[3] = {
|
|
|
|
sc->set_next_step_dir_msgid, sc->oid, sdir ^ sc->invert_sdir
|
|
|
|
};
|
|
|
|
struct queue_message *qm = message_alloc_and_encode(msg, 3);
|
2016-12-31 01:02:28 +03:00
|
|
|
qm->req_clock = sc->homing_clock ?: sc->last_step_clock;
|
2016-11-10 20:44:04 +03:00
|
|
|
list_add_tail(&qm->node, &sc->msg_queue);
|
|
|
|
}
|
|
|
|
|
2016-11-13 06:55:27 +03:00
|
|
|
// Check if the internal queue needs to be expanded, and expand if so
|
2016-12-31 21:21:53 +03:00
|
|
|
static void
|
|
|
|
_check_expand(struct stepcompress *sc, uint64_t *qn)
|
|
|
|
{
|
|
|
|
sc->queue_next = qn;
|
|
|
|
if (qn - sc->queue_pos > 65535 + 2000)
|
|
|
|
// No point in keeping more than 64K steps in memory
|
|
|
|
stepcompress_flush(sc, *(qn - 65535));
|
|
|
|
expand_queue(sc, 1);
|
|
|
|
}
|
2016-11-13 06:55:27 +03:00
|
|
|
static inline void
|
2016-12-31 21:21:53 +03:00
|
|
|
check_expand(struct stepcompress *sc, uint64_t **pqn, uint64_t **pqend)
|
2016-11-13 06:55:27 +03:00
|
|
|
{
|
2016-12-31 21:21:53 +03:00
|
|
|
if (likely(*pqn < *pqend))
|
|
|
|
return;
|
|
|
|
_check_expand(sc, *pqn);
|
|
|
|
*pqn = sc->queue_next;
|
|
|
|
*pqend = sc->queue_end;
|
2016-11-13 06:55:27 +03:00
|
|
|
}
|
|
|
|
|
2016-05-25 18:37:40 +03:00
|
|
|
// Schedule a step event at the specified step_clock time
|
|
|
|
void
|
2016-11-10 20:44:04 +03:00
|
|
|
stepcompress_push(struct stepcompress *sc, double step_clock, int32_t sdir)
|
2016-05-25 18:37:40 +03:00
|
|
|
{
|
2016-12-31 21:21:53 +03:00
|
|
|
set_next_step_dir(sc, !!sdir);
|
2016-11-02 02:38:37 +03:00
|
|
|
step_clock += 0.5;
|
2016-12-31 21:21:53 +03:00
|
|
|
uint64_t *qn = sc->queue_next, *qend = sc->queue_end;
|
|
|
|
check_expand(sc, &qn, &qend);
|
|
|
|
*qn++ = step_clock;
|
|
|
|
sc->queue_next = qn;
|
2016-05-25 18:37:40 +03:00
|
|
|
}
|
|
|
|
|
|
|
|
// Schedule 'steps' number of steps with a constant time between steps
|
|
|
|
// using the formula: step_clock = clock_offset + step_num*factor
|
2016-11-10 20:44:04 +03:00
|
|
|
int32_t
|
2016-05-25 18:37:40 +03:00
|
|
|
stepcompress_push_factor(struct stepcompress *sc
|
|
|
|
, double steps, double step_offset
|
|
|
|
, double clock_offset, double factor)
|
|
|
|
{
|
|
|
|
// Calculate number of steps to take
|
2016-11-10 20:44:04 +03:00
|
|
|
int sdir = 1;
|
|
|
|
if (steps < 0) {
|
|
|
|
sdir = 0;
|
|
|
|
steps = -steps;
|
|
|
|
step_offset = -step_offset;
|
2016-07-13 21:47:03 +03:00
|
|
|
}
|
2016-11-10 20:44:04 +03:00
|
|
|
int count = steps + .5 - step_offset;
|
2016-12-24 07:13:35 +03:00
|
|
|
if (count <= 0 || count > 10000000) {
|
2016-11-10 20:44:04 +03:00
|
|
|
if (count && steps)
|
2016-11-30 20:04:28 +03:00
|
|
|
errorf("push_factor invalid count %d %f %f %f %f"
|
|
|
|
, sc->oid, steps, step_offset, clock_offset, factor);
|
2016-11-10 20:44:04 +03:00
|
|
|
return 0;
|
|
|
|
}
|
2016-12-31 21:21:53 +03:00
|
|
|
set_next_step_dir(sc, sdir);
|
|
|
|
int res = sdir ? count : -count;
|
2016-05-25 18:37:40 +03:00
|
|
|
|
|
|
|
// Calculate each step time
|
2016-11-02 02:38:37 +03:00
|
|
|
clock_offset += 0.5;
|
2016-11-10 20:44:04 +03:00
|
|
|
double pos = step_offset + .5;
|
2016-12-31 21:21:53 +03:00
|
|
|
uint64_t *qn = sc->queue_next, *qend = sc->queue_end;
|
|
|
|
while (count--) {
|
|
|
|
check_expand(sc, &qn, &qend);
|
2016-05-25 18:37:40 +03:00
|
|
|
*qn++ = clock_offset + pos*factor;
|
|
|
|
pos += 1.0;
|
|
|
|
}
|
|
|
|
sc->queue_next = qn;
|
2016-12-31 21:21:53 +03:00
|
|
|
return res;
|
2016-05-25 18:37:40 +03:00
|
|
|
}
|
|
|
|
|
|
|
|
// Schedule 'steps' number of steps using the formula:
|
|
|
|
// step_clock = clock_offset + sqrt(step_num*factor + sqrt_offset)
|
2016-11-10 20:44:04 +03:00
|
|
|
int32_t
|
2016-05-25 18:37:40 +03:00
|
|
|
stepcompress_push_sqrt(struct stepcompress *sc, double steps, double step_offset
|
|
|
|
, double clock_offset, double sqrt_offset, double factor)
|
|
|
|
{
|
|
|
|
// Calculate number of steps to take
|
2016-11-10 20:44:04 +03:00
|
|
|
int sdir = 1;
|
|
|
|
if (steps < 0) {
|
|
|
|
sdir = 0;
|
|
|
|
steps = -steps;
|
|
|
|
step_offset = -step_offset;
|
|
|
|
}
|
|
|
|
int count = steps + .5 - step_offset;
|
2016-12-24 07:13:35 +03:00
|
|
|
if (count <= 0 || count > 10000000) {
|
2016-11-10 20:44:04 +03:00
|
|
|
if (count && steps)
|
2016-11-30 20:04:28 +03:00
|
|
|
errorf("push_sqrt invalid count %d %f %f %f %f %f"
|
|
|
|
, sc->oid, steps, step_offset, clock_offset, sqrt_offset
|
|
|
|
, factor);
|
2016-11-10 20:44:04 +03:00
|
|
|
return 0;
|
2016-07-13 21:47:03 +03:00
|
|
|
}
|
2016-12-31 21:21:53 +03:00
|
|
|
set_next_step_dir(sc, sdir);
|
|
|
|
int res = sdir ? count : -count;
|
2016-05-25 18:37:40 +03:00
|
|
|
|
|
|
|
// Calculate each step time
|
2016-11-02 02:38:37 +03:00
|
|
|
clock_offset += 0.5;
|
2016-11-10 20:44:04 +03:00
|
|
|
double pos = step_offset + .5 + sqrt_offset/factor;
|
2016-12-31 21:21:53 +03:00
|
|
|
uint64_t *qn = sc->queue_next, *qend = sc->queue_end;
|
|
|
|
while (count--) {
|
|
|
|
check_expand(sc, &qn, &qend);
|
2016-11-14 23:36:11 +03:00
|
|
|
double v = safe_sqrt(pos*factor);
|
|
|
|
*qn++ = clock_offset + (factor >= 0. ? v : -v);
|
|
|
|
pos += 1.0;
|
|
|
|
}
|
2016-07-13 21:47:03 +03:00
|
|
|
sc->queue_next = qn;
|
2016-12-31 21:21:53 +03:00
|
|
|
return res;
|
2016-05-25 18:37:40 +03:00
|
|
|
}
|
|
|
|
|
2016-09-15 19:20:49 +03:00
|
|
|
// Schedule 'count' number of steps using the delta kinematic const speed
|
|
|
|
int32_t
|
|
|
|
stepcompress_push_delta_const(
|
2016-12-05 03:30:35 +03:00
|
|
|
struct stepcompress *sc, double clock_offset, double dist, double start_pos
|
|
|
|
, double inv_velocity, double step_dist
|
|
|
|
, double height, double closestxy_d, double closest_height2, double movez_r)
|
2016-09-15 19:20:49 +03:00
|
|
|
{
|
|
|
|
// Calculate number of steps to take
|
2016-12-05 03:30:35 +03:00
|
|
|
double movexy_r = movez_r ? sqrt(1. - movez_r*movez_r) : 1.;
|
|
|
|
double reldist = closestxy_d - movexy_r*dist;
|
|
|
|
double end_height = safe_sqrt(closest_height2 - reldist*reldist);
|
|
|
|
int count = (end_height - height + movez_r*dist) / step_dist + .5;
|
2016-12-24 07:13:35 +03:00
|
|
|
if (count <= 0 || count > 10000000) {
|
2016-09-15 19:20:49 +03:00
|
|
|
if (count)
|
2016-11-30 20:04:28 +03:00
|
|
|
errorf("push_delta_const invalid count %d %d %f %f %f %f %f %f %f %f"
|
|
|
|
, sc->oid, count, clock_offset, dist, step_dist, start_pos
|
|
|
|
, closest_height2, height, movez_r, inv_velocity);
|
2016-09-15 19:20:49 +03:00
|
|
|
return 0;
|
|
|
|
}
|
2016-12-31 21:21:53 +03:00
|
|
|
set_next_step_dir(sc, step_dist > 0.);
|
|
|
|
int res = step_dist > 0. ? count : -count;
|
2016-09-15 19:20:49 +03:00
|
|
|
|
|
|
|
// Calculate each step time
|
|
|
|
clock_offset += 0.5;
|
2016-12-05 03:30:35 +03:00
|
|
|
start_pos += movexy_r*closestxy_d;
|
2016-09-15 19:20:49 +03:00
|
|
|
height += .5 * step_dist;
|
2016-12-31 21:21:53 +03:00
|
|
|
uint64_t *qn = sc->queue_next, *qend = sc->queue_end;
|
2016-12-05 21:45:39 +03:00
|
|
|
if (!movez_r) {
|
|
|
|
// Optmized case for common XY only moves (no Z movement)
|
2016-12-31 21:21:53 +03:00
|
|
|
while (count--) {
|
|
|
|
check_expand(sc, &qn, &qend);
|
2016-12-05 21:45:39 +03:00
|
|
|
double v = safe_sqrt(closest_height2 - height*height);
|
|
|
|
double pos = start_pos + (step_dist > 0. ? -v : v);
|
|
|
|
*qn++ = clock_offset + pos * inv_velocity;
|
|
|
|
height += step_dist;
|
|
|
|
}
|
|
|
|
} else if (!movexy_r) {
|
|
|
|
// Optmized case for Z only moves
|
|
|
|
double v = (step_dist > 0. ? -end_height : end_height);
|
2016-12-31 21:21:53 +03:00
|
|
|
while (count--) {
|
|
|
|
check_expand(sc, &qn, &qend);
|
2016-12-05 21:45:39 +03:00
|
|
|
double pos = start_pos + movez_r*height + v;
|
|
|
|
*qn++ = clock_offset + pos * inv_velocity;
|
|
|
|
height += step_dist;
|
|
|
|
}
|
|
|
|
} else {
|
|
|
|
// General case (handles XY+Z moves)
|
2016-12-31 21:21:53 +03:00
|
|
|
while (count--) {
|
|
|
|
check_expand(sc, &qn, &qend);
|
2016-12-05 21:45:39 +03:00
|
|
|
double relheight = movexy_r*height - movez_r*closestxy_d;
|
|
|
|
double v = safe_sqrt(closest_height2 - relheight*relheight);
|
|
|
|
double pos = start_pos + movez_r*height + (step_dist > 0. ? -v : v);
|
|
|
|
*qn++ = clock_offset + pos * inv_velocity;
|
|
|
|
height += step_dist;
|
|
|
|
}
|
2016-09-15 19:20:49 +03:00
|
|
|
}
|
|
|
|
sc->queue_next = qn;
|
2016-12-31 21:21:53 +03:00
|
|
|
return res;
|
2016-09-15 19:20:49 +03:00
|
|
|
}
|
|
|
|
|
|
|
|
// Schedule 'count' number of steps using delta kinematic acceleration
|
|
|
|
int32_t
|
|
|
|
stepcompress_push_delta_accel(
|
2016-12-05 03:30:35 +03:00
|
|
|
struct stepcompress *sc, double clock_offset, double dist, double start_pos
|
|
|
|
, double accel_multiplier, double step_dist
|
|
|
|
, double height, double closestxy_d, double closest_height2, double movez_r)
|
2016-09-15 19:20:49 +03:00
|
|
|
{
|
|
|
|
// Calculate number of steps to take
|
2016-12-05 03:30:35 +03:00
|
|
|
double movexy_r = movez_r ? sqrt(1. - movez_r*movez_r) : 1.;
|
|
|
|
double reldist = closestxy_d - movexy_r*dist;
|
|
|
|
double end_height = safe_sqrt(closest_height2 - reldist*reldist);
|
|
|
|
int count = (end_height - height + movez_r*dist) / step_dist + .5;
|
2016-12-24 07:13:35 +03:00
|
|
|
if (count <= 0 || count > 10000000) {
|
2016-09-15 19:20:49 +03:00
|
|
|
if (count)
|
2016-11-30 20:04:28 +03:00
|
|
|
errorf("push_delta_accel invalid count %d %d %f %f %f %f %f %f %f %f"
|
|
|
|
, sc->oid, count, clock_offset, dist, step_dist, start_pos
|
|
|
|
, closest_height2, height, movez_r, accel_multiplier);
|
2016-09-15 19:20:49 +03:00
|
|
|
return 0;
|
|
|
|
}
|
2016-12-31 21:21:53 +03:00
|
|
|
set_next_step_dir(sc, step_dist > 0.);
|
|
|
|
int res = step_dist > 0. ? count : -count;
|
2016-09-15 19:20:49 +03:00
|
|
|
|
|
|
|
// Calculate each step time
|
|
|
|
clock_offset += 0.5;
|
2016-12-05 03:30:35 +03:00
|
|
|
start_pos += movexy_r*closestxy_d;
|
2016-09-15 19:20:49 +03:00
|
|
|
height += .5 * step_dist;
|
2016-12-31 21:21:53 +03:00
|
|
|
uint64_t *qn = sc->queue_next, *qend = sc->queue_end;
|
|
|
|
while (count--) {
|
|
|
|
check_expand(sc, &qn, &qend);
|
2016-12-05 03:30:35 +03:00
|
|
|
double relheight = movexy_r*height - movez_r*closestxy_d;
|
|
|
|
double v = safe_sqrt(closest_height2 - relheight*relheight);
|
|
|
|
double pos = start_pos + movez_r*height + (step_dist > 0. ? -v : v);
|
2016-09-15 19:20:49 +03:00
|
|
|
v = safe_sqrt(pos * accel_multiplier);
|
|
|
|
*qn++ = clock_offset + (accel_multiplier >= 0. ? v : -v);
|
|
|
|
height += step_dist;
|
|
|
|
}
|
|
|
|
sc->queue_next = qn;
|
2016-12-31 21:21:53 +03:00
|
|
|
return res;
|
2016-09-15 19:20:49 +03:00
|
|
|
}
|
|
|
|
|
2016-05-25 18:37:40 +03:00
|
|
|
// Reset the internal state of the stepcompress object
|
|
|
|
void
|
|
|
|
stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock)
|
|
|
|
{
|
|
|
|
stepcompress_flush(sc, UINT64_MAX);
|
|
|
|
sc->last_step_clock = last_step_clock;
|
2016-11-10 20:44:04 +03:00
|
|
|
sc->sdir = -1;
|
2016-05-25 18:37:40 +03:00
|
|
|
}
|
|
|
|
|
2016-12-31 01:02:28 +03:00
|
|
|
// Indicate the stepper is in homing mode (or done homing if zero)
|
|
|
|
void
|
|
|
|
stepcompress_set_homing(struct stepcompress *sc, uint64_t homing_clock)
|
|
|
|
{
|
|
|
|
stepcompress_flush(sc, UINT64_MAX);
|
|
|
|
sc->homing_clock = homing_clock;
|
|
|
|
}
|
|
|
|
|
2016-05-25 18:37:40 +03:00
|
|
|
// Queue an mcu command to go out in order with stepper commands
|
|
|
|
void
|
|
|
|
stepcompress_queue_msg(struct stepcompress *sc, uint32_t *data, int len)
|
|
|
|
{
|
|
|
|
stepcompress_flush(sc, UINT64_MAX);
|
|
|
|
|
|
|
|
struct queue_message *qm = message_alloc_and_encode(data, len);
|
2016-12-31 01:02:28 +03:00
|
|
|
qm->req_clock = sc->homing_clock ?: sc->last_step_clock;
|
2016-05-25 18:37:40 +03:00
|
|
|
list_add_tail(&qm->node, &sc->msg_queue);
|
|
|
|
}
|
|
|
|
|
|
|
|
// Return the count of internal errors found
|
|
|
|
uint32_t
|
|
|
|
stepcompress_get_errors(struct stepcompress *sc)
|
|
|
|
{
|
|
|
|
return sc->errors;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/****************************************************************
|
|
|
|
* 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;
|
|
|
|
};
|
|
|
|
|
2016-07-13 21:47:03 +03:00
|
|
|
// Allocate a new 'steppersync' object
|
2016-05-25 18:37:40 +03:00
|
|
|
struct steppersync *
|
|
|
|
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;
|
|
|
|
}
|
|
|
|
|
2016-11-30 09:58:45 +03:00
|
|
|
// Free memory associated with a 'steppersync' object
|
|
|
|
void
|
|
|
|
steppersync_free(struct steppersync *ss)
|
|
|
|
{
|
|
|
|
if (!ss)
|
|
|
|
return;
|
|
|
|
free(ss->sc_list);
|
|
|
|
free(ss->move_clocks);
|
|
|
|
serialqueue_free_commandqueue(ss->cq);
|
|
|
|
free(ss);
|
|
|
|
}
|
|
|
|
|
2016-05-25 18:37:40 +03:00
|
|
|
// 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'
|
|
|
|
void
|
|
|
|
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++)
|
|
|
|
stepcompress_flush(ss->sc_list[i], move_clock);
|
|
|
|
|
|
|
|
// 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;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
2016-11-02 06:11:27 +03:00
|
|
|
if (!qm || (qm->min_clock && req_clock > move_clock))
|
2016-05-25 18:37:40 +03:00
|
|
|
break;
|
|
|
|
|
2016-11-02 06:11:27 +03:00
|
|
|
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;
|
2016-05-25 18:37:40 +03:00
|
|
|
|
|
|
|
// 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);
|
|
|
|
}
|