stepcompress: Move stepcompress_push_* functions to their own section
This is only code movement; no code changes. Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
parent
bfad970e4d
commit
47f12f107d
|
@ -16,6 +16,10 @@ defs_stepcompress = """
|
||||||
, uint32_t queue_step_msgid, uint32_t set_next_step_dir_msgid
|
, uint32_t queue_step_msgid, uint32_t set_next_step_dir_msgid
|
||||||
, uint32_t invert_sdir, uint32_t oid);
|
, uint32_t invert_sdir, uint32_t oid);
|
||||||
void stepcompress_free(struct stepcompress *sc);
|
void stepcompress_free(struct stepcompress *sc);
|
||||||
|
int stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock);
|
||||||
|
int stepcompress_set_homing(struct stepcompress *sc, uint64_t homing_clock);
|
||||||
|
int stepcompress_queue_msg(struct stepcompress *sc, uint32_t *data, int len);
|
||||||
|
|
||||||
int stepcompress_push(struct stepcompress *sc, double step_clock
|
int stepcompress_push(struct stepcompress *sc, double step_clock
|
||||||
, int32_t sdir);
|
, int32_t sdir);
|
||||||
int32_t stepcompress_push_factor(struct stepcompress *sc
|
int32_t stepcompress_push_factor(struct stepcompress *sc
|
||||||
|
@ -32,9 +36,6 @@ defs_stepcompress = """
|
||||||
, double clock_offset, double dist, double start_pos
|
, double clock_offset, double dist, double start_pos
|
||||||
, double accel_multiplier, double step_dist, double height
|
, double accel_multiplier, double step_dist, double height
|
||||||
, double closestxy_d, double closest_height2, double movez_r);
|
, double closestxy_d, double closest_height2, double movez_r);
|
||||||
int stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock);
|
|
||||||
int stepcompress_set_homing(struct stepcompress *sc, uint64_t homing_clock);
|
|
||||||
int stepcompress_queue_msg(struct stepcompress *sc, uint32_t *data, int len);
|
|
||||||
|
|
||||||
struct steppersync *steppersync_alloc(struct serialqueue *sq
|
struct steppersync *steppersync_alloc(struct serialqueue *sq
|
||||||
, struct stepcompress **sc_list, int sc_num, int move_num);
|
, struct stepcompress **sc_list, int sc_num, int move_num);
|
||||||
|
|
|
@ -275,22 +275,6 @@ check_line(struct stepcompress *sc, struct step_move move)
|
||||||
* Step compress interface
|
* Step compress interface
|
||||||
****************************************************************/
|
****************************************************************/
|
||||||
|
|
||||||
#define likely(x) __builtin_expect(!!(x), 1)
|
|
||||||
|
|
||||||
// Wrapper around sqrt() to handle small negative numbers
|
|
||||||
static double
|
|
||||||
_safe_sqrt(double v)
|
|
||||||
{
|
|
||||||
// Due to floating point truncation, it's possible to get a small
|
|
||||||
// negative number - treat it as zero.
|
|
||||||
if (v < -0.001)
|
|
||||||
errorf("safe_sqrt of %.9f", v);
|
|
||||||
return 0.;
|
|
||||||
}
|
|
||||||
static inline double safe_sqrt(double v) {
|
|
||||||
return likely(v >= 0.) ? sqrt(v) : _safe_sqrt(v);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Allocate a new 'stepcompress' object
|
// Allocate a new 'stepcompress' object
|
||||||
struct stepcompress *
|
struct stepcompress *
|
||||||
stepcompress_alloc(uint32_t max_error, uint32_t queue_step_msgid
|
stepcompress_alloc(uint32_t max_error, uint32_t queue_step_msgid
|
||||||
|
@ -378,6 +362,8 @@ set_next_step_dir(struct stepcompress *sc, int sdir)
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#define likely(x) __builtin_expect(!!(x), 1)
|
||||||
|
|
||||||
// Check if the internal queue needs to be expanded, and expand if so
|
// Check if the internal queue needs to be expanded, and expand if so
|
||||||
static int
|
static int
|
||||||
_check_expand(struct stepcompress *sc, uint64_t *qn)
|
_check_expand(struct stepcompress *sc, uint64_t *qn)
|
||||||
|
@ -405,6 +391,62 @@ check_expand(struct stepcompress *sc, uint64_t **pqn, uint64_t **pqend)
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Reset the internal state of the stepcompress object
|
||||||
|
int
|
||||||
|
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;
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Indicate the stepper is in homing mode (or done homing if zero)
|
||||||
|
int
|
||||||
|
stepcompress_set_homing(struct stepcompress *sc, uint64_t homing_clock)
|
||||||
|
{
|
||||||
|
int ret = stepcompress_flush(sc, UINT64_MAX);
|
||||||
|
if (ret)
|
||||||
|
return ret;
|
||||||
|
sc->homing_clock = homing_clock;
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Queue an mcu command to go out in order with stepper commands
|
||||||
|
int
|
||||||
|
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->homing_clock ?: sc->last_step_clock;
|
||||||
|
list_add_tail(&qm->node, &sc->msg_queue);
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/****************************************************************
|
||||||
|
* Motion to step conversions
|
||||||
|
****************************************************************/
|
||||||
|
|
||||||
|
// Wrapper around sqrt() to handle small negative numbers
|
||||||
|
static double
|
||||||
|
_safe_sqrt(double v)
|
||||||
|
{
|
||||||
|
// Due to floating point truncation, it's possible to get a small
|
||||||
|
// negative number - treat it as zero.
|
||||||
|
if (v < -0.001)
|
||||||
|
errorf("safe_sqrt of %.9f", v);
|
||||||
|
return 0.;
|
||||||
|
}
|
||||||
|
static inline double safe_sqrt(double v) {
|
||||||
|
return likely(v >= 0.) ? sqrt(v) : _safe_sqrt(v);
|
||||||
|
}
|
||||||
|
|
||||||
// Schedule a step event at the specified step_clock time
|
// Schedule a step event at the specified step_clock time
|
||||||
int
|
int
|
||||||
stepcompress_push(struct stepcompress *sc, double step_clock, int32_t sdir)
|
stepcompress_push(struct stepcompress *sc, double step_clock, int32_t sdir)
|
||||||
|
@ -625,43 +667,6 @@ stepcompress_push_delta_accel(
|
||||||
return res;
|
return res;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Reset the internal state of the stepcompress object
|
|
||||||
int
|
|
||||||
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;
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Indicate the stepper is in homing mode (or done homing if zero)
|
|
||||||
int
|
|
||||||
stepcompress_set_homing(struct stepcompress *sc, uint64_t homing_clock)
|
|
||||||
{
|
|
||||||
int ret = stepcompress_flush(sc, UINT64_MAX);
|
|
||||||
if (ret)
|
|
||||||
return ret;
|
|
||||||
sc->homing_clock = homing_clock;
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Queue an mcu command to go out in order with stepper commands
|
|
||||||
int
|
|
||||||
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->homing_clock ?: sc->last_step_clock;
|
|
||||||
list_add_tail(&qm->node, &sc->msg_queue);
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
/****************************************************************
|
/****************************************************************
|
||||||
* Step compress synchronization
|
* Step compress synchronization
|
||||||
|
|
Loading…
Reference in New Issue