forked from CPL/Simulator
479 lines
18 KiB
C#
479 lines
18 KiB
C#
using System;
|
||
using System.Collections.Concurrent;
|
||
using System.Collections.Generic;
|
||
using System.Diagnostics;
|
||
using System.Linq;
|
||
using System.Text;
|
||
using System.Threading;
|
||
using System.Threading.Tasks;
|
||
using TelemetryIO.Models;
|
||
|
||
namespace TelemetryIO
|
||
{
|
||
public interface iCommParams
|
||
{
|
||
|
||
}
|
||
|
||
internal class TCPCommParams : iCommParams
|
||
{
|
||
public string IP = "";
|
||
public int Port = 0;
|
||
public TCPCommParams(string addr, int port)
|
||
{
|
||
this.IP = addr;
|
||
this.Port = port;
|
||
}
|
||
}
|
||
|
||
public class SerialCommParams : iCommParams
|
||
{
|
||
public string PortName = "";
|
||
public int BaudRate = 9600;
|
||
public SerialCommParams(string portName, int baudRate)
|
||
{
|
||
PortName = portName;
|
||
BaudRate = baudRate;
|
||
}
|
||
}
|
||
|
||
public abstract class BaseCommHandler
|
||
{
|
||
public const int TELE_CMD_RD_ONCE = 1;
|
||
public const int TELE_CMD_RD_MON_ON = 2;
|
||
public const int TELE_CMD_RD_MON_OFF = 3;
|
||
public const int TELE_CMD_RD_MON_ADD = 4;
|
||
public const int TELE_CMD_RD_MON_REMOVE = 5;
|
||
public const int TELE_CMD_RD_MON_REMOVEALL = 6;
|
||
public const int TELE_CMD_WR = 10;
|
||
public const int TELE_CMD_MOTORS_CTRL = 100;
|
||
public const int TELE_CMD_ABORT = 999;
|
||
public const int TELE_CMD_HELLO = 9999;
|
||
public const byte CRC8_POLYNOMIAL = 0x07;
|
||
public const int CRC32_POLY = 0x04C11DB7;
|
||
public const byte ESCAPE_BEGIN = 0xBE;
|
||
public const byte ESCAPE_END = 0xED;
|
||
public const byte ESCAPE_CHAR = 0x0E;
|
||
|
||
|
||
private volatile bool _isReading = true;
|
||
private readonly ConcurrentQueue<byte> dataQueue = new ConcurrentQueue<byte>();
|
||
static private List<byte> rx_buf = new List<byte>();
|
||
private Task _readingTask;
|
||
private object lock_obj = new object();
|
||
private object lock_obj_put = new object();
|
||
private bool waitingForResponse = false;
|
||
private bool isTimeout = false;
|
||
private readonly int responseTimeout = 2000;
|
||
private System.Timers.Timer timeout = new System.Timers.Timer(3000);
|
||
private bool exitPending = false;
|
||
private float[] monitor = new float[32];
|
||
public string view_str = "";
|
||
|
||
protected string IP = "127.0.0.1";
|
||
protected int Port = 8888;
|
||
|
||
protected string PortName = "COM1";
|
||
protected int BaudRate = 9600;
|
||
|
||
private Queue<byte[]> req_buffer = new Queue<byte[]>();
|
||
|
||
//Generate event when answer is received
|
||
public delegate void AnswerEventHandler<SerialEventArgs>(object sender, SerialEventArgs e);
|
||
public event AnswerEventHandler<FeedbackEventArgs> AnswerReceived;//событие получены данные в ответ на запрос
|
||
|
||
//Generate event when handshake is occurred
|
||
public delegate void HandShakeHandler(object sender, EventArgs e);
|
||
public event HandShakeHandler HandShakeOccurred;//событие полетник ответил на запрос HELLO
|
||
|
||
//Generate event when monitoring telegram is received
|
||
public delegate void MonitoringEventHandler<MonitoringEventArgs>(object sender, MonitoringEventArgs e);
|
||
public event MonitoringEventHandler<MonitoringEventArgs> MonitoringItemsReceived;//событие получены данные мониторинга
|
||
|
||
Models.Telemetry telemetry = Models.Telemetry.Instance;
|
||
abstract public Task Open();
|
||
abstract public void Close();
|
||
abstract protected void sendData(byte[] data);
|
||
abstract public bool IsOpen();
|
||
abstract public void CloseConnection();
|
||
abstract public Task StartReadingAsync(object? client = null);
|
||
|
||
abstract public void setCommParams(iCommParams commParams);
|
||
|
||
abstract protected void ProcessCommand(int cmd, int slot, byte[] data, int offset, int len);
|
||
|
||
/// <summary>
|
||
/// Обрабатывает входящий поток данных, при обнаружении ECSAPE_END байта декодирует данные, проверяет контрольную сумму и запускает обработку команды
|
||
/// </summary>
|
||
protected void data_extract()
|
||
{
|
||
byte b = new byte();
|
||
while (dataQueue.TryDequeue(out b))
|
||
{
|
||
if (b == ESCAPE_END)
|
||
{//END BYTE IS RECEIVED
|
||
isTimeout = false;
|
||
byte[] unscape = EscapeSeqToBytes(rx_buf.ToArray());
|
||
uint checksum = crc32(unscape, unscape.Length - 4);
|
||
uint re_checksum = BitConverter.ToUInt32(unscape, unscape.Length - 4);
|
||
if (re_checksum == checksum)
|
||
{
|
||
//Parse telegram
|
||
int cmd = BitConverter.ToInt32(unscape, 0);
|
||
int slot = BitConverter.ToInt32(unscape, 4);
|
||
int len = BitConverter.ToInt32(unscape, 8);
|
||
int offset = BitConverter.ToInt32(unscape, 12);
|
||
byte[] data = new byte[len];
|
||
Debug.WriteLine($"cmd = {cmd} *** slot = {slot} *** offset = {offset} *** len = {len}");
|
||
if(cmd == TELE_CMD_WR)Array.Copy(unscape, 16, data, 0, len);
|
||
ProcessCommand(cmd, slot, data, offset, len);
|
||
|
||
waitingForResponse = false;
|
||
timeout.Stop();
|
||
sendNextRequest();
|
||
}
|
||
}
|
||
else if (b == ESCAPE_BEGIN)
|
||
{//START BYTE IS RECEIVED
|
||
rx_buf.Clear();
|
||
}
|
||
else
|
||
{//FILLING BUFFER
|
||
rx_buf.Add(b);
|
||
}
|
||
}
|
||
|
||
}
|
||
//**********************************************************************************************
|
||
//*************************************** REQUESTS BLOCK ***************************************
|
||
//**********************************************************************************************
|
||
|
||
public void getPIDs()
|
||
{
|
||
//отправить наборы ПИДов
|
||
putRequest(prepareTelegram(TELE_CMD_RD_ONCE, 1000, new byte[0], 0, 4 * 9));
|
||
putRequest(prepareTelegram(TELE_CMD_RD_ONCE, 1001, new byte[0], 0, 4 * 9));
|
||
putRequest(prepareTelegram(TELE_CMD_RD_ONCE, 1002, new byte[0], 0, 4 * 9));
|
||
putRequest(prepareTelegram(TELE_CMD_RD_ONCE, 1003, new byte[0], 0, 4 * 9));
|
||
}
|
||
|
||
public void stopMonitoring()
|
||
{
|
||
//остановить мониторинг
|
||
putRequest(prepareTelegram(TELE_CMD_RD_MON_OFF, 0, new byte[0], 0, 0));
|
||
}
|
||
|
||
public void startMonitoring()
|
||
{
|
||
//начать мониторинг
|
||
putRequest(prepareTelegram(TELE_CMD_RD_MON_ON, 0, new byte[0], 0, 0));
|
||
}
|
||
|
||
public void AddMonitoringItem(int slot, int offset)
|
||
{
|
||
//добавить элемент из массива мониторинга по адресу
|
||
putRequest(prepareTelegram(TELE_CMD_RD_MON_ADD, slot, new byte[0], offset));
|
||
}
|
||
|
||
public void AddMonitoringItem(string name)
|
||
{
|
||
//добавить элемент из массива мониторинга по имени
|
||
putRequest(prepareTelegram(TELE_CMD_RD_MON_ADD, new byte[0], name));
|
||
}
|
||
|
||
public void RemoveMonitoringItem(int id)
|
||
{
|
||
//удалить элемент из массива мониторинга (len == id)
|
||
putRequest(prepareTelegram(TELE_CMD_RD_MON_REMOVE, 0, new byte[0], id));
|
||
}
|
||
|
||
public void RemoveMonitoringItems()
|
||
{
|
||
//удалить все элементы из массива мониторинга
|
||
putRequest(prepareTelegram(TELE_CMD_RD_MON_REMOVEALL, 0, new byte[0], 0));
|
||
}
|
||
|
||
public void sendFloats(int slot, float[] sp)
|
||
{
|
||
//Записать массив чисел с плавающей точкой
|
||
putRequest(prepareTelegram(TELE_CMD_WR, slot, sp, 0, sp.Length * 4));
|
||
}
|
||
|
||
public void sendMotorsControl()
|
||
{
|
||
//Отправляет задание на моторы в полетник. Тот в ответ посылает актуальные скорости на моторах
|
||
//В offset передается количество моторов
|
||
putRequest(prepareTelegram(TELE_CMD_MOTORS_CTRL, Telemetry.MOTORS_SP_ADDRESS, telemetry.motor_sp, 8));
|
||
}
|
||
|
||
/// <summary>
|
||
/// Считает контрольную сумму CRC32
|
||
/// </summary>
|
||
public uint crc32(byte[] data, int length)
|
||
{
|
||
uint crc = 0xFFFFFFFF; // Начальное значение CRC
|
||
|
||
for (int i = 0; i < length; i++)
|
||
{
|
||
crc ^= (uint)data[i] << 24; // XOR с текущим байтом
|
||
|
||
for (int j = 0; j < 8; j++)
|
||
{
|
||
if ((uint)(crc & 0x80000000) != 0)
|
||
{
|
||
crc = (crc << 1) ^ CRC32_POLY;
|
||
}
|
||
else
|
||
{
|
||
crc <<= 1;
|
||
}
|
||
}
|
||
}
|
||
|
||
return crc;
|
||
}
|
||
public byte[] prepareTelegram<T>(int cmd, T[] load, string var_name)
|
||
{
|
||
VarAddress va = telemetry.getVarAdress(var_name);
|
||
return prepareTelegram(cmd, va.slot, load, va.offset, va.length);
|
||
}
|
||
|
||
/// <summary>
|
||
/// Подготавливает данные для отправки, кодируя их в ESCAPE-последовательность
|
||
/// </summary>
|
||
public byte[] prepareTelegram<T>(int cmd, int slot, T[] load, int offset, int len = 0)
|
||
{
|
||
byte[] byteload = DataArrayToBytes(load);
|
||
int total_len = 20 + byteload.Length;//cmd[4 bytes] + slot[4 bytes] + len[4 bytes] + offset[4 bytes] + load[len bytes]
|
||
byte[] data = new byte[total_len];
|
||
|
||
//Construct telegram
|
||
data[0] = (byte)(0xFF & cmd);
|
||
data[1] = (byte)(0xFF & (cmd >> 8));
|
||
data[2] = (byte)(0xFF & (cmd >> 16));
|
||
data[3] = (byte)(0xFF & (cmd >> 24));
|
||
|
||
data[4] = (byte)(0xFF & slot);
|
||
data[5] = (byte)(0xFF & (slot >> 8));
|
||
data[6] = (byte)(0xFF & (slot >> 16));
|
||
data[7] = (byte)(0xFF & (slot >> 24));
|
||
|
||
int l = 0;
|
||
if (cmd == TELE_CMD_WR) l = byteload.Length;
|
||
else l = len;
|
||
|
||
data[8] = (byte)(0xFF & l);
|
||
data[9] = (byte)(0xFF & (l >> 8));
|
||
data[10] = (byte)(0xFF & (l >> 16));
|
||
data[11] = (byte)(0xFF & (l >> 24));
|
||
|
||
data[12] = (byte)(0xFF & offset);
|
||
data[13] = (byte)(0xFF & (offset >> 8));
|
||
data[14] = (byte)(0xFF & (offset >> 16));
|
||
data[15] = (byte)(0xFF & (offset >> 24));
|
||
|
||
if (byteload.Length > 0)
|
||
{
|
||
//Copy data
|
||
Array.Copy(byteload, 0, data, 16, byteload.Length);
|
||
}
|
||
//CRC32
|
||
uint checksum = crc32(data, total_len - 4);
|
||
data[total_len - 4] = (byte)(0xFF & checksum);
|
||
data[total_len - 3] = (byte)(0xFF & (checksum >> 8));
|
||
data[total_len - 2] = (byte)(0xFF & (checksum >> 16));
|
||
data[total_len - 1] = (byte)(0xFF & (checksum >> 24));
|
||
|
||
byte[] escape = BytesToEscapeSeq(data);
|
||
byte[] ret = new byte[escape.Length + 2];
|
||
Array.Copy(escape, 0, ret, 1, escape.Length);
|
||
ret[0] = ESCAPE_BEGIN;
|
||
ret[ret.Length - 1] = ESCAPE_END;
|
||
//Array.Copy(ret, saving_request, ret.Length);
|
||
return ret;
|
||
}
|
||
|
||
/// <summary>
|
||
/// Конвертирует массив bool/byte/int/float в массив байт. Создает новый массив.
|
||
/// </summary>
|
||
byte[] DataArrayToBytes<T>(T[] data)
|
||
{
|
||
if (data == null) return new byte[0];
|
||
List<byte> ret = new List<byte>();
|
||
for (int i = 0; i < data.Length; i++)
|
||
{
|
||
if (typeof(T) == typeof(float))
|
||
{
|
||
ret.AddRange(BitConverter.GetBytes((float)(object)data[i]));
|
||
}
|
||
else if (typeof(T) == typeof(int))
|
||
{
|
||
ret.AddRange(BitConverter.GetBytes((int)(object)data[i]));
|
||
}
|
||
else if (typeof(T) == typeof(byte))
|
||
{
|
||
ret.Add((byte)(object)data[i]);
|
||
}
|
||
else if (typeof(T) == typeof(bool))
|
||
{
|
||
bool t = (bool)(object)data[i];
|
||
if (t) ret.Add(1);
|
||
else ret.Add(0);
|
||
}
|
||
|
||
}
|
||
return ret.ToArray();
|
||
}
|
||
|
||
/// <summary>
|
||
/// Конвертирует массив байт в ESCAPE-последовательность.
|
||
/// </summary>
|
||
public byte[] BytesToEscapeSeq(byte[] data)
|
||
{
|
||
List<byte> ret = new List<byte>();
|
||
for (int i = 0; i < data.Length; i++)
|
||
{
|
||
if ((data[i] == ESCAPE_BEGIN) || (data[i] == ESCAPE_CHAR) || (data[i] == ESCAPE_END))
|
||
{
|
||
ret.Add(ESCAPE_CHAR);
|
||
ret.Add((byte)(data[i] - 0x0E));
|
||
}
|
||
else
|
||
{
|
||
ret.Add(data[i]);
|
||
}
|
||
}
|
||
return ret.ToArray();
|
||
}
|
||
|
||
/// <summary>
|
||
/// Конвертирует ESCAPE-последовательность в массив байт.
|
||
/// </summary>
|
||
public byte[] EscapeSeqToBytes(byte[] EscSeq)
|
||
{
|
||
List<byte> ret = new List<byte>();
|
||
for (int i = 0; i < EscSeq.Length; i++)
|
||
{
|
||
//if ((EscSeq[i] == ESCAPE_BEGIN) || (EscSeq[i] == ESCAPE_CHAR) || (EscSeq[i] == ESCAPE_END))
|
||
if (EscSeq[i] == ESCAPE_CHAR)
|
||
{
|
||
i++;
|
||
ret.Add((byte)(EscSeq[i] + 0x0E));
|
||
}
|
||
else
|
||
{
|
||
ret.Add(EscSeq[i]);
|
||
}
|
||
}
|
||
return ret.ToArray();
|
||
}
|
||
|
||
public void requestExit()
|
||
{
|
||
//запрос на закрытие приложения, отправляем команду на очистку массива мониторинга и ждем ответ, либо таймаут,
|
||
//после чего приложение закрывается
|
||
RemoveMonitoringItems();
|
||
exitPending = true;
|
||
}
|
||
|
||
/// <summary>
|
||
/// Добавляет данные в очередь ожидания отправки.
|
||
/// </summary>
|
||
public void putRequest(byte[] request)
|
||
{
|
||
//добавить в очередь или отправить(если очередь пуста) пакет в порт
|
||
lock (lock_obj_put)
|
||
{
|
||
if (waitingForResponse)
|
||
{
|
||
req_buffer.Enqueue(request);
|
||
return;
|
||
}
|
||
|
||
sendRequest(request);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// Толкает данные непосредственно в очередь отправки.
|
||
/// </summary>
|
||
protected void sendRequest(byte[] request)
|
||
{
|
||
//отправка пакета
|
||
//waitingForResponse = true;
|
||
if (IsOpen())
|
||
{
|
||
sendData(request);
|
||
}
|
||
timeout.Stop();
|
||
timeout.Start();
|
||
}
|
||
|
||
/// <summary>
|
||
/// Вытягивает данные из очереди ожидания непосредственно в очередь отправки.
|
||
/// </summary>
|
||
protected void sendNextRequest()
|
||
{
|
||
//вытащить из очереди пакет и отправить в порт
|
||
waitingForResponse = false;
|
||
if (req_buffer.Count > 0)
|
||
{
|
||
sendRequest(req_buffer.Dequeue());
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// Копирует входящие данные в очередь.
|
||
/// </summary>
|
||
protected void EnqueueData(byte[] data, int len)
|
||
{
|
||
for (int i = 0; i < len; i++)
|
||
{
|
||
dataQueue.Enqueue(data[i]);
|
||
}
|
||
}
|
||
|
||
public void ClearReqQueue()
|
||
{
|
||
req_buffer.Clear();
|
||
}
|
||
|
||
protected virtual void OnAnswerReceived(string answer, int id)
|
||
{
|
||
AnswerReceived?.Invoke(this, new FeedbackEventArgs(answer, id));
|
||
}
|
||
|
||
protected virtual void OnHandShakeOccurred()
|
||
{
|
||
HandShakeOccurred?.Invoke(this, new EventArgs());
|
||
}
|
||
|
||
protected virtual void OnMonitoringItemsReceived(float[] data)
|
||
{
|
||
MonitoringItemsReceived?.Invoke(this, new MonitoringEventArgs(data));
|
||
}
|
||
}
|
||
|
||
public class FeedbackEventArgs : EventArgs
|
||
{
|
||
public string Answer { get; set; }
|
||
public int Id { get; set; }
|
||
|
||
public FeedbackEventArgs(string answer, int id)
|
||
{
|
||
Answer = answer;
|
||
Id = id;
|
||
}
|
||
}
|
||
|
||
public class MonitoringEventArgs : EventArgs
|
||
{
|
||
public float[] Data { get; set; }
|
||
|
||
public MonitoringEventArgs(float[] data)
|
||
{
|
||
Data = data;
|
||
}
|
||
}
|
||
}
|