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 dictMonitor = new Dictionary();//Словарь адресов, для целей мониторинга 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; } } }