TelemetryIO/TelemetryIO/Models/Telemetry.cs

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 = "";
        public float[] raw_imu = new float[6];
        public float[] quaternion = new float[4];
        public float[] battery = new float[4];
        public float[] motor_act = new float[4];
        public float[] motor_sp = new float[4];
        public float[] pid0 = new float[9];
        public float[] pid1 = new float[9];
        public float[] pid2 = new float[9];
        public float[] pid3 = new float[9];

        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("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));
        }

        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)
        {
            if (slot != 9999) Debug.WriteLine(slot);
            switch (slot)
            {
                case RAW_IMU_ADDRESS://RAW IMU
                    set_float(raw_imu, data, offset, len);
                    break;

                case Q_ADDRESS://quaternion
                    set_float(quaternion, 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 NAME_ADDRESS:
                    set_name(data);
                    break;
            }
        }


        private void set_float(float[] dest, byte[] data, int offset, int len)
        {
            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 void set_name(byte[] data)
        {
            name = Encoding.ASCII.GetString(data, 0, 16);
        }

        public const int RAW_IMU_ADDRESS = 0;
        public const int Q_ADDRESS = 1;

        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;
        }
    }
}