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()); } } }