public steppersync(SerialQueue sq, List <stepcompress> sc_list, int move_num)
{
this.sq = sq;
cq = new command_queue();
this.sc_list = sc_list;
this.sc_num = sc_list.Count;
move_clocks = new ulong[move_num];
num_move_clocks = move_num;
}
static void build_and_send_command(double eventtime)
{
queue_message output = new queue_message();
output.len = MESSAGE_HEADER_SIZE;
while (ready_bytes != 0)
{
// Find highest priority message (message with lowest req_clock)
ulong min_clock = MAX_CLOCK;
command_queue cq = null;
queue_message qm = new queue_message();
foreach (var q in pending_queues)
{
queue_message m;
if (!q.ready_queue.TryPeek(out m) && m.req_clock < min_clock)
{
min_clock = m.req_clock;
cq = q;
qm = m;
}
}
// Append message to outgoing command
if (output.len + qm.len > MESSAGE_MAX - MESSAGE_TRAILER_SIZE)
{
break;
}
cq.ready_queue.TryDequeue(out qm);
if (cq.ready_queue.Count == 0 && cq.stalled_queue.Count == 0)
{
pending_queues.Remove(cq);
}
Buffer.MemoryCopy(qm.msg, &output.msg[output.len], MESSAGE_MAX, qm.len);
//memcpy(output.msg[output.len], qm.msg, qm.len);
output.len += qm.len;
ready_bytes -= qm.len;
}
// Fill header / trailer
output.len += MESSAGE_TRAILER_SIZE;
output.msg[MESSAGE_POS_LEN] = output.len;
output.msg[MESSAGE_POS_SEQ] = (byte)(MESSAGE_DEST | (send_seq & MESSAGE_SEQ_MASK));
int crc = Crc16_ccitt(new ReadOnlySpan <byte>(output.msg, output.len - MESSAGE_TRAILER_SIZE));
output.msg[output.len - MESSAGE_TRAILER_CRC] = (byte)(crc >> 8);
output.msg[output.len - MESSAGE_TRAILER_CRC + 1] = (byte)(crc & 0xff);
output.msg[output.len - MESSAGE_TRAILER_SYNC] = MESSAGE_SYNC;
// Send message
fixed(byte *psendBuffer = sendBuffer)
{
*((queue_message *)psendBuffer) = output;
}
serialPort.Write(sendBuffer, 0, output.len);
//int ret = write(sq->serial_fd, output->msg, output->len);
//if (ret < 0)
// report_errno("write", ret);
bytes_write += output.len;
if (eventtime > idle_time)
{
idle_time = eventtime;
}
idle_time += output.len * baud_adjust;
output.sent_time = eventtime;
output.receive_time = idle_time;
if (sent_queue.Count == 0)
{
// pollreactor_update_timer(&sq->pr, SQPT_RETRANSMIT, sq->idle_time + sq->rto);
retransmitTimer = idle_time + rto;
}
if (rtt_sample_seq != 0)
{
rtt_sample_seq = send_seq;
}
send_seq++;
need_ack_bytes += output.len;
sent_queue.Enqueue(output);
}
public SerialCommand lookup_command(string msgformat, command_queue cq = null)
{
return(this._serial.lookup_command(msgformat, cq));
}
public SerialCommand(SerialReader serial, command_queue cmd_queue, BaseFormat cmd)
{
this.serial = serial;
this.cmd_queue = cmd_queue;
this.cmd = cmd;
}
public void TestConnect()
{
var serialPort = new SerialPort("COM5", 250000);
serialPort.ReadTimeout = SerialPort.InfiniteTimeout;
serialPort.WriteTimeout = SerialPort.InfiniteTimeout;
serialPort.Encoding = Encoding.ASCII;
serialPort.Open();
var ser = new SerialQueue(serialPort);
ser.Start();
MemoryStream buffer = new MemoryStream();
int res = 0;
var cq = new command_queue();
MemoryStream msg = new MemoryStream(16);
while (true)
{
QueueMessage qm;
ser.pull(out qm);
Console.WriteLine($"MSG {qm.sent_time}/{qm.receive_time} {(int)((qm.receive_time - qm.sent_time) * 1000)}");
msg.Position = 0;
msg.SetLength(0);
for (var i = SerialQueue.MESSAGE_HEADER_SIZE;
i < qm.len - SerialQueue.MESSAGE_TRAILER_SIZE; i++)
{
msg.WriteByte(qm.msg[i]);
}
msg.Position = 0;
var code = msg.ReadByte();
if (code == 0)
{
Console.WriteLine("identify_response");
var offset = msg.parse(false);
var len = msg.ReadByte();
if (len > 0)
{
var buf = new byte[len];
var read = msg.Read(buf, 0, len);
if (read != len)
{
Console.WriteLine("Error");
}
buffer.Write(buf, 0, read);
res += read;
Console.WriteLine($"Read bytes {buffer.Length}; {read}/{len}");
}
if (len < 50)
{
Console.WriteLine("download done");
res = -1;
//var final = ZlibDecompress(buffer.ToArray());
//var jsonText = Encoding.ASCII.GetString(final);
//var xml = XDocument.Load(JsonReaderWriterFactory.CreateJsonReader(Encoding.ASCII.GetBytes(jsonText), new XmlDictionaryReaderQuotas()));
//xml.Save("identify.xml");
Assert.AreEqual(PROGMEM, buffer.ToArray());
return;
}
}
else
{
Console.WriteLine($"{qm.receive_time * 1000} code:{code}");
}
if (res != -1)
{
Console.WriteLine($"identify get data {res}");
//{ 1, "identify offset=%u count=%c" }, uint32, byte
msg.Position = 0;
msg.SetLength(0);
msg.WriteByte(1);
msg.encode(res);
msg.encode((byte)50);
ser.send(cq, msg.ToArray());
}
}
}
