150billionsteps/Simulator
1
0
Fork 0
forked from CPL/Simulator
Code Pull Requests Activity

Добавлен сервер для отправки телеметрии

Browse Source
This commit is contained in:
dr-i-boleet
2025-07-05 08:27:19 +11:00
parent 15d4fa5011
commit e5d8a9c507
13 changed files with 2347 additions and 736 deletions
Download Patch File Download Diff File Expand all files Collapse all files

478
DroneClient/BaseCommHandler.cs Normal file
View File

@ -0,0 +1,478 @@
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;
}
}
}