forked from CPL/Simulator
Добавлен сервер для отправки телеметрии
This commit is contained in:
126
DroneClient/Models/MonitorContainer.cs
Normal file
126
DroneClient/Models/MonitorContainer.cs
Normal file
@ -0,0 +1,126 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using System.Text;
|
||||
using System.Threading.Tasks;
|
||||
using TelemetryIO;
|
||||
|
||||
namespace DroneClient.Models
|
||||
{
|
||||
public class monitorItem
|
||||
{
|
||||
public int slotNo = 0;
|
||||
public int offset = 0;
|
||||
public int len = 0;
|
||||
public int id = 0;
|
||||
|
||||
public monitorItem()
|
||||
{
|
||||
|
||||
}
|
||||
public monitorItem(int slotNo, int offset, int len, int id)
|
||||
{
|
||||
this.slotNo = slotNo;
|
||||
this.offset = offset;
|
||||
this.len = len;
|
||||
this.id = id;
|
||||
}
|
||||
}
|
||||
|
||||
public class MonitorContainer
|
||||
{
|
||||
public const int monitorMaxFill = 32;
|
||||
public int monitorFillLevel = 0;
|
||||
public monitorItem[] monitorItems = new monitorItem[monitorMaxFill];
|
||||
public int isMonitor = 0;
|
||||
private static volatile MonitorContainer instance;
|
||||
private static readonly object _lock = new object();
|
||||
|
||||
public static MonitorContainer Instance
|
||||
{
|
||||
get
|
||||
{
|
||||
if (instance == null)
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
if (instance == null)
|
||||
{
|
||||
instance = new MonitorContainer();
|
||||
}
|
||||
}
|
||||
}
|
||||
return instance;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
private MonitorContainer()
|
||||
{
|
||||
for (int i = 0; i < monitorMaxFill; i++)
|
||||
{
|
||||
monitorItems[i] = new monitorItem();
|
||||
}
|
||||
}
|
||||
|
||||
public void resetMonitorContainer()
|
||||
{
|
||||
for (int i = 0; i < monitorMaxFill; i++)
|
||||
{
|
||||
monitorItems[i] = new monitorItem();
|
||||
}
|
||||
}
|
||||
|
||||
public int monitorPut(int slot, int offset, int len)
|
||||
{
|
||||
int ret_id = 0;
|
||||
if (monitorFillLevel < monitorMaxFill)
|
||||
{
|
||||
monitorItems[monitorFillLevel].slotNo = slot;
|
||||
monitorItems[monitorFillLevel].offset = offset;
|
||||
monitorItems[monitorFillLevel].len = len;
|
||||
monitorItems[monitorFillLevel].id = (slot << 8) + offset;
|
||||
ret_id = monitorItems[monitorFillLevel].id;
|
||||
monitorFillLevel++;
|
||||
isMonitor = 1;
|
||||
}
|
||||
|
||||
return ret_id;
|
||||
}
|
||||
|
||||
public int monitorRemove(int id)
|
||||
{
|
||||
int match = 0;
|
||||
int ret_id = 0;
|
||||
for (int i = 0; i < monitorMaxFill; i++)
|
||||
{
|
||||
if (monitorItems[i].id == id)
|
||||
{
|
||||
match = 1;
|
||||
if (monitorFillLevel > 0) monitorFillLevel--;
|
||||
ret_id = id;
|
||||
}
|
||||
|
||||
if (match > 0)
|
||||
{
|
||||
if (monitorItems[i].len == 0) return ret_id;
|
||||
if (i < 31)
|
||||
{
|
||||
monitorItems[i].id = monitorItems[i + 1].id;
|
||||
monitorItems[i].offset = monitorItems[i + 1].offset;
|
||||
monitorItems[i].slotNo = monitorItems[i + 1].slotNo;
|
||||
monitorItems[i].len = monitorItems[i + 1].len;
|
||||
}
|
||||
else
|
||||
{
|
||||
monitorItems[i].id = 0;
|
||||
monitorItems[i].offset = 0;
|
||||
monitorItems[i].slotNo = 0;
|
||||
monitorItems[i].len = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
return ret_id;
|
||||
}
|
||||
}
|
||||
}
|
||||
340
DroneClient/Models/Telemetry.cs
Normal file
340
DroneClient/Models/Telemetry.cs
Normal file
@ -0,0 +1,340 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Diagnostics;
|
||||
using System.Drawing;
|
||||
using System.Linq;
|
||||
using System.Text;
|
||||
using System.Threading.Tasks;
|
||||
|
||||
namespace TelemetryIO.Models
|
||||
{
|
||||
public sealed class Telemetry
|
||||
{
|
||||
//Класс синглтон для хранения данных телеметрии
|
||||
private static volatile Telemetry instance;
|
||||
private static readonly object _lock = new object();
|
||||
private Dictionary<string, VarAddress> dictMonitor = new Dictionary<string, VarAddress>();//Словарь адресов, для целей мониторинга
|
||||
public string name = "_150";
|
||||
public float[] raw_imu = new float[6];
|
||||
public float[] imu = new float[6];
|
||||
public float[] filtered_imu = new float[6];
|
||||
public float[] g_integration = new float[3];
|
||||
public float[] euler = new float[3];
|
||||
public float[] quaternion = new float[4];
|
||||
public float[] battery = new float[4];
|
||||
public float[] motor_act = new float[8];
|
||||
public float[] motor_sp = new float[8];
|
||||
public float[] pid0 = new float[9];
|
||||
public float[] pid1 = new float[9];
|
||||
public float[] pid2 = new float[9];
|
||||
public float[] pid3 = new float[9];
|
||||
public float[] pos = new float[3];
|
||||
public float[] of = new float[2];
|
||||
|
||||
private object _lockRW = new object();
|
||||
|
||||
private Telemetry() {
|
||||
dictMonitor.Add("rawAx", new VarAddress(RAW_IMU_ADDRESS, 0));
|
||||
dictMonitor.Add("rawAy", new VarAddress(RAW_IMU_ADDRESS, 1));
|
||||
dictMonitor.Add("rawAz", new VarAddress(RAW_IMU_ADDRESS, 2));
|
||||
dictMonitor.Add("rawGx", new VarAddress(RAW_IMU_ADDRESS, 3));
|
||||
dictMonitor.Add("rawGy", new VarAddress(RAW_IMU_ADDRESS, 4));
|
||||
dictMonitor.Add("rawGz", new VarAddress(RAW_IMU_ADDRESS, 5));
|
||||
|
||||
dictMonitor.Add("Ax", new VarAddress(IMU_ADDRESS, 0));
|
||||
dictMonitor.Add("Ay", new VarAddress(IMU_ADDRESS, 1));
|
||||
dictMonitor.Add("Az", new VarAddress(IMU_ADDRESS, 2));
|
||||
dictMonitor.Add("Gx", new VarAddress(IMU_ADDRESS, 3));
|
||||
dictMonitor.Add("Gy", new VarAddress(IMU_ADDRESS, 4));
|
||||
dictMonitor.Add("Gz", new VarAddress(IMU_ADDRESS, 5));
|
||||
|
||||
dictMonitor.Add("filtered_Ax", new VarAddress(FILTERED_IMU_ADDRESS, 0));
|
||||
dictMonitor.Add("filtered_Ay", new VarAddress(FILTERED_IMU_ADDRESS, 1));
|
||||
dictMonitor.Add("filtered_Az", new VarAddress(FILTERED_IMU_ADDRESS, 2));
|
||||
dictMonitor.Add("filtered_Gx", new VarAddress(FILTERED_IMU_ADDRESS, 3));
|
||||
dictMonitor.Add("filtered_Gy", new VarAddress(FILTERED_IMU_ADDRESS, 4));
|
||||
dictMonitor.Add("filtered_Gz", new VarAddress(FILTERED_IMU_ADDRESS, 5));
|
||||
|
||||
dictMonitor.Add("Gx_int", new VarAddress(G_INTEGRATION_ADDRESS, 0));
|
||||
dictMonitor.Add("Gy_int", new VarAddress(G_INTEGRATION_ADDRESS, 1));
|
||||
dictMonitor.Add("Gz_int", new VarAddress(G_INTEGRATION_ADDRESS, 2));
|
||||
|
||||
dictMonitor.Add("Euler.Phi", new VarAddress(EULER_ADDRESS, 0));
|
||||
dictMonitor.Add("Euler.Theta", new VarAddress(EULER_ADDRESS, 1));
|
||||
dictMonitor.Add("Euler.Psi", new VarAddress(EULER_ADDRESS, 2));
|
||||
|
||||
dictMonitor.Add("Qw", new VarAddress(Q_ADDRESS, 0));
|
||||
dictMonitor.Add("Qx", new VarAddress(Q_ADDRESS, 1));
|
||||
dictMonitor.Add("Qy", new VarAddress(Q_ADDRESS, 2));
|
||||
dictMonitor.Add("Qz", new VarAddress(Q_ADDRESS, 3));
|
||||
|
||||
dictMonitor.Add("Battery[V]", new VarAddress(BAT_ADDRESS, 0));
|
||||
|
||||
dictMonitor.Add("motor0_Act", new VarAddress(MOTORS_ACT_ADDRESS, 0));
|
||||
dictMonitor.Add("motor1_Act", new VarAddress(MOTORS_ACT_ADDRESS, 1));
|
||||
dictMonitor.Add("motor2_Act", new VarAddress(MOTORS_ACT_ADDRESS, 2));
|
||||
dictMonitor.Add("motor3_Act", new VarAddress(MOTORS_ACT_ADDRESS, 3));
|
||||
dictMonitor.Add("motor4_Act", new VarAddress(MOTORS_ACT_ADDRESS, 4));
|
||||
dictMonitor.Add("motor5_Act", new VarAddress(MOTORS_ACT_ADDRESS, 5));
|
||||
dictMonitor.Add("motor6_Act", new VarAddress(MOTORS_ACT_ADDRESS, 6));
|
||||
dictMonitor.Add("motor7_Act", new VarAddress(MOTORS_ACT_ADDRESS, 7));
|
||||
|
||||
dictMonitor.Add("Pos_X", new VarAddress(POS_ADDRESS, 0));
|
||||
dictMonitor.Add("Pos_Y", new VarAddress(POS_ADDRESS, 1));
|
||||
dictMonitor.Add("Pos_L", new VarAddress(POS_ADDRESS, 2));
|
||||
|
||||
dictMonitor.Add("OF_X", new VarAddress(OF_ADDRESS, 0));
|
||||
dictMonitor.Add("OF_Y", new VarAddress(OF_ADDRESS, 1));
|
||||
}
|
||||
|
||||
public int getId(string name)
|
||||
{
|
||||
VarAddress var = dictMonitor[name];
|
||||
if (var == null) return -1;
|
||||
return (var.slot << 8) | var.offset;
|
||||
}
|
||||
|
||||
public string getName(int id)
|
||||
{
|
||||
int slot = id >> 8;
|
||||
int offset = id & 0xFF;
|
||||
foreach(var element in dictMonitor)
|
||||
{
|
||||
if ((element.Value.slot == slot) && (element.Value.offset == offset)) return element.Key;
|
||||
}
|
||||
return null;
|
||||
}
|
||||
|
||||
public VarAddress getVarAdress(string name)
|
||||
{
|
||||
VarAddress ret;
|
||||
dictMonitor.TryGetValue(name, out ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
public string[] getKeys()
|
||||
{
|
||||
return dictMonitor.Keys.ToArray();
|
||||
}
|
||||
|
||||
public static Telemetry Instance
|
||||
{
|
||||
get
|
||||
{
|
||||
if(instance == null)
|
||||
{
|
||||
lock(_lock)
|
||||
{
|
||||
if (instance == null)
|
||||
{
|
||||
instance = new Telemetry();
|
||||
}
|
||||
}
|
||||
}
|
||||
return instance;
|
||||
}
|
||||
}
|
||||
|
||||
public void setSlot(int slot, byte[] data, int offset, int len)
|
||||
{
|
||||
switch (slot)
|
||||
{
|
||||
case RAW_IMU_ADDRESS://RAW IMU
|
||||
set_float(raw_imu, data, offset, len);
|
||||
break;
|
||||
|
||||
case IMU_ADDRESS://IMU
|
||||
set_float(imu, data, offset, len);
|
||||
break;
|
||||
|
||||
case Q_ADDRESS://quaternion
|
||||
set_float(quaternion, data, offset, len);
|
||||
break;
|
||||
|
||||
case G_INTEGRATION_ADDRESS://quaternion
|
||||
set_float(g_integration, data, offset, len);
|
||||
break;
|
||||
|
||||
case EULER_ADDRESS://quaternion
|
||||
set_float(euler, data, offset, len);
|
||||
break;
|
||||
|
||||
case FILTERED_IMU_ADDRESS://IMU
|
||||
set_float(filtered_imu, data, offset, len);
|
||||
break;
|
||||
|
||||
case BAT_ADDRESS://battery
|
||||
set_float(battery, data, offset, len);
|
||||
break;
|
||||
|
||||
case MOTORS_ACT_ADDRESS://motors act
|
||||
set_float(motor_act, data, offset, len);
|
||||
break;
|
||||
|
||||
case MOTORS_SP_ADDRESS://motors act
|
||||
set_float(motor_sp, data, offset, len);
|
||||
break;
|
||||
|
||||
case PID0_ADDRESS:
|
||||
set_float(pid0, data, offset, len);
|
||||
break;
|
||||
|
||||
case PID1_ADDRESS:
|
||||
set_float(pid1, data, offset, len);
|
||||
break;
|
||||
|
||||
case PID2_ADDRESS:
|
||||
set_float(pid2, data, offset, len);
|
||||
break;
|
||||
|
||||
case PID3_ADDRESS:
|
||||
set_float(pid3, data, offset, len);
|
||||
break;
|
||||
|
||||
case POS_ADDRESS:
|
||||
set_float(pos, data, offset, len);
|
||||
break;
|
||||
|
||||
case OF_ADDRESS:
|
||||
set_float(of, data, offset, len);
|
||||
break;
|
||||
|
||||
case NAME_ADDRESS:
|
||||
set_name(data);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
public byte[] getSlot(int slot, int offset, int len)
|
||||
{
|
||||
switch (slot)
|
||||
{
|
||||
case RAW_IMU_ADDRESS://RAW IMU
|
||||
return get_float(raw_imu, offset, len);
|
||||
|
||||
case IMU_ADDRESS://IMU
|
||||
return get_float(imu, offset, len);
|
||||
|
||||
case Q_ADDRESS://quaternion
|
||||
return get_float(quaternion, offset, len);
|
||||
|
||||
case G_INTEGRATION_ADDRESS://quaternion
|
||||
return get_float(g_integration, offset, len);
|
||||
|
||||
case EULER_ADDRESS://quaternion
|
||||
return get_float(euler, offset, len);
|
||||
|
||||
case FILTERED_IMU_ADDRESS://IMU
|
||||
return get_float(filtered_imu, offset, len);
|
||||
|
||||
case BAT_ADDRESS://battery
|
||||
return get_float(battery, offset, len);
|
||||
|
||||
case MOTORS_ACT_ADDRESS://motors act
|
||||
return get_float(motor_act, offset, len);
|
||||
|
||||
case MOTORS_SP_ADDRESS://motors act
|
||||
return get_float(motor_sp, offset, len);
|
||||
|
||||
case PID0_ADDRESS:
|
||||
return get_float(pid0, offset, len);
|
||||
|
||||
case PID1_ADDRESS:
|
||||
return get_float(pid1, offset, len);
|
||||
|
||||
case PID2_ADDRESS:
|
||||
return get_float(pid2, offset, len);
|
||||
|
||||
case PID3_ADDRESS:
|
||||
return get_float(pid3, offset, len);
|
||||
|
||||
case POS_ADDRESS:
|
||||
return get_float(pos, offset, len);
|
||||
break;
|
||||
|
||||
case OF_ADDRESS:
|
||||
return get_float(of, offset, len);
|
||||
|
||||
case NAME_ADDRESS:
|
||||
return Encoding.ASCII.GetBytes(name);
|
||||
|
||||
default:
|
||||
byte[] data = new byte[0];
|
||||
return data;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
private void set_float(float[] dest, byte[] data, int offset, int len)
|
||||
{
|
||||
lock (_lockRW)
|
||||
{
|
||||
for (int i = 0; i < (int)(len / 4); i++)
|
||||
{
|
||||
byte[] bs = new byte[4];
|
||||
Array.Copy(data, i * 4, bs, 0, 4);
|
||||
float f = BitConverter.ToSingle(bs, 0);
|
||||
dest[offset + i] = f;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private byte[] get_float(float[] source, int offset, int len)
|
||||
{
|
||||
lock (_lockRW)
|
||||
{
|
||||
byte[] bs = new byte[len];
|
||||
Buffer.BlockCopy(source, offset * sizeof(float), bs, 0, len);
|
||||
return bs;
|
||||
}
|
||||
}
|
||||
|
||||
private void set_name(byte[] data)
|
||||
{
|
||||
int len = data.Length;
|
||||
if (len > 16) len = 16;
|
||||
name = Encoding.ASCII.GetString(data, 0, len);
|
||||
}
|
||||
|
||||
public const int RAW_IMU_ADDRESS = 0;
|
||||
public const int IMU_ADDRESS = 1;
|
||||
public const int Q_ADDRESS = 2;
|
||||
public const int G_INTEGRATION_ADDRESS = 3;
|
||||
public const int EULER_ADDRESS = 4;
|
||||
public const int FILTERED_IMU_ADDRESS = 5;
|
||||
public const int POS_ADDRESS = 6;
|
||||
public const int OF_ADDRESS = 7;
|
||||
|
||||
public const int BAT_ADDRESS = 50;
|
||||
|
||||
public const int MOTORS_ACT_ADDRESS = 100;
|
||||
public const int MOTORS_SP_ADDRESS = 101;
|
||||
|
||||
public const int PID0_ADDRESS = 1000;
|
||||
public const int PID1_ADDRESS = 1001;
|
||||
public const int PID2_ADDRESS = 1002;
|
||||
public const int PID3_ADDRESS = 1003;
|
||||
|
||||
public const int NAME_ADDRESS = 9999;
|
||||
}
|
||||
|
||||
public class VarAddress
|
||||
{
|
||||
public int slot = 0;
|
||||
public int offset = 0;
|
||||
public int length = 0;
|
||||
|
||||
public VarAddress()
|
||||
{
|
||||
slot = offset = 0;
|
||||
length = 4;
|
||||
}
|
||||
|
||||
public VarAddress(int slot, int offset, int length = 4)
|
||||
{
|
||||
this.slot = slot;
|
||||
this.offset = offset;
|
||||
this.length = length;
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user