// Add a clock time to the queue (flushing the queue if needed)
public bool append(double step_clock)
{
double rel_sc = step_clock + clock_offset;
if (!(qnext >= qend || rel_sc >= (double)Stepcompress.CLOCK_DIFF_MAX))
{
*qnext++ = last_step_clock_32 + (uint)rel_sc;
return(false);
}
// Call queue_append_slow() to handle queue expansion and integer overflow
stepcompress sc = this.sc;
ulong old_last_step_clock = sc.last_step_clock;
sc.queue_next = qnext;
int ret = sc.queue_append_slow(rel_sc);
if (ret != 0)
{
return(ret > 0);
}
qnext = sc.queue_next;
qend = sc.queue_end;
last_step_clock_32 = (uint)sc.last_step_clock;
clock_offset -= sc.last_step_clock - old_last_step_clock;
return(false);
}
// Set the conversion rate of 'print_time' to mcu clock
public void set_time(double time_offset, double mcu_freq)
{
int i;
for (i = 0; i < sc_list.Count; i++)
{
stepcompress sc = sc_list[i];
sc.set_time(time_offset, mcu_freq);
}
}
public bool append_set_next_step_dir(bool sdir)
{
stepcompress sc = this.sc;
ulong old_last_step_clock = sc.last_step_clock;
sc.queue_next = qnext;
int ret = sc.set_next_step_dir(sdir ? 1 : 0);
if (ret != 0)
{
return(ret > 0);
}
qnext = sc.queue_next;
qend = sc.queue_end;
last_step_clock_32 = (uint)sc.last_step_clock;
clock_offset -= sc.last_step_clock - old_last_step_clock;
return(false);
}
// Find and transmit any scheduled steps prior to the given 'move_clock'
public int flush(ulong move_clock)
{
// Flush each stepcompress to the specified move_clock
int i;
for (i = 0; i < sc_list.Count; i++)
{
var sc = sc_list[i];
int ret = sc.flush(move_clock);
if (ret != 0)
{
return(ret);
}
}
// Order commands by the reqclock of each pending command
List <SerialQueue.RawMessage> msgs = new List <SerialQueue.RawMessage>();
while (true)
{
// Find message with lowest reqclock
ulong req_clock = SerialQueue.MAX_CLOCK;
var qm = new SerialQueue.RawMessage();
var scFound = default(stepcompress);
bool found = false;
for (i = 0; i < sc_list.Count; i++)
{
stepcompress sc = sc_list[i];
if (sc.msg_queue.Count != 0)
{
var m = sc.msg_queue.Peek();
if (m.req_clock < req_clock)
{
found = true;
scFound = sc;
qm = m;
req_clock = m.req_clock;
}
}
}
if (!found || (qm.min_clock != 0 && req_clock > move_clock))
{
break;
}
ulong next_avail = move_clocks[0];
if (qm.min_clock != 0)
{
// 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(qm.min_clock);
}
// Reset the min_clock to its normal meaning (minimum transmit time)
qm.min_clock = next_avail;
qm = scFound.msg_queue.Dequeue();
// Batch this command
msgs.Add(qm);
}
// Transmit commands
if (msgs.Count != 0)
{
sq.send_batch(cq, msgs.ToArray());
}
return(0);
}
// Generate step times for a stepper during a move
public bool itersolve_gen_steps(ref move m)
{
stepcompress sc = this.sc;
double half_step = 0.5 * step_dist;
double mcu_freq = sc.get_mcu_freq();
timepos last = new timepos(0.0, commanded_pos), low = last, high = last;
double seek_time_delta = 0.000100;
bool sdir = sc.get_step_dir();
queue_append qa = sc.queue_append_start(m.print_time, 0.5);
bool ret;
while (true)
{
// Determine if next step is in forward or reverse direction
double dist = high.position - last.position;
if (Math.Abs(dist) < half_step)
{
if (high.time >= m.move_t)
{
// At end of move
break;
}
CalcPosition(ref m, last, ref low, ref high, ref seek_time_delta);
continue;
}
bool next_sdir = dist > 0.0;
if (next_sdir != sdir)
{
// Direction change
if (Math.Abs(dist) < half_step + .000000001)
{
// Only change direction if going past midway point
if (high.time >= m.move_t)
{
// At end of move
break;
}
CalcPosition(ref m, last, ref low, ref high, ref seek_time_delta);
continue;
}
if (last.time >= low.time && high.time > last.time)
{
// Must seek new low range to avoid re-finding previous time
high.time = (last.time + high.time) * .5;
high.position = calc_position(ref m, high.time);
continue;
}
ret = qa.append_set_next_step_dir(next_sdir);
if (ret)
{
return(ret);
}
sdir = next_sdir;
}
// Find step
double target = last.position + (sdir ? half_step : -half_step);
timepos next = itersolve_find_step(ref m, ref low, ref high, target);
// Add step at given time
ret = qa.append(next.time * mcu_freq);
if (ret)
{
return(ret);
}
seek_time_delta = next.time - last.time;
if (seek_time_delta < 0.000000001)
{
seek_time_delta = 0.000000001;
}
last.position = target + (sdir ? half_step : -half_step);
last.time = next.time;
low = next;
if (last.time >= high.time)
{
// The high range is no longer valid - recalculate it
if (high.time >= m.move_t)
{
// At end of move
break;
}
CalcPosition(ref m, last, ref low, ref high, ref seek_time_delta);
continue;
}
}
qa.queue_append_finish();
commanded_pos = last.position;
return(false);
}
public void itersolve_set_stepcompress(ref stepcompress sc, double step_dist)
{
this.sc = sc;
this.step_dist = step_dist;
}
