forked from CPL/Simulator
341 lines
12 KiB
C#
341 lines
12 KiB
C#
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;
|
|
}
|
|
}
|
|
}
|