// Convert previously scheduled steps into commands for the mcu
public int flush(ulong move_clock)
{
if (queue_pos >= queue_next)
{
return(0);
}
uint *msg = stackalloc uint[5];
var span = new ReadOnlySpan <uint>(msg, 5);
while (last_step_clock < move_clock)
{
step_move move = compress_bisect_add();
int ret = check_line(ref move);
if (ret != 0)
{
return(ret);
}
msg[0] = queue_step_msgid;
msg[1] = oid;
msg[2] = move.interval;
msg[3] = move.count;
msg[4] = (uint)move.add;
var qm = SerialQueue.RawMessage.CreateAndEncode(span);
qm.min_clock = qm.req_clock = last_step_clock;
int addfactor = move.count * (move.count - 1) / 2;
ulong ticks = (ulong)(move.add * addfactor + move.interval * move.count);
last_step_clock += ticks;
if (homing_clock != 0)
{
// When homing, all steps should be sent prior to homing_clock
qm.min_clock = qm.req_clock = homing_clock;
}
msg_queue.Enqueue(qm);
if (queue_pos + move.count >= queue_next)
{
queue_pos = queue_next = queue;
break;
}
queue_pos += move.count;
}
return(0);
}
// Verify that a given 'step_move' matches the actual step times
int check_line(ref step_move move)
{
if (!Stepcompress.CHECK_LINES)
{
return(0);
}
if (move.count == 0 || (move.interval == 0 && move.add == 0 && move.count > 1) || move.interval >= 0x80000000)
{
//errorf("stepcompress o=%d i=%d c=%d a=%d: Invalid sequence"
// , sc->oid, move.interval, move.count, move.add);
return(Stepcompress.ERROR_RET);
}
uint interval = move.interval, p = 0;
ushort i;
for (i = 0; i < move.count; i++)
{
points point = minmax_point(queue_pos + i);
p += interval;
if (p < point.minp || p > point.maxp)
{
//errorf("stepcompress o=%d i=%d c=%d a=%d: Point %d: %d not in %d:%d"
// , sc.oid, move.interval, move.count, move.add
// , i + 1, p, point.minp, point.maxp);
return(Stepcompress.ERROR_RET);
}
if (interval >= 0x80000000)
{
//errorf("stepcompress o=%d i=%d c=%d a=%d:"+
// " Point %d: interval overflow %d"
// , sc.oid, move.interval, move.count, move.add
// , i + 1, interval);
return(Stepcompress.ERROR_RET);
}
interval += (ushort)move.add;
}
return(0);
}
