Talos_Drones_Tracking_and_Telemetry/tracking and telemetry/simpleexample.cs

using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.IO.Ports;
using System.Linq;
using System.Reflection;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;

namespace SimpleExample
{
    public partial class simpleexample : Form
    {
        MAVLink.MavlinkParse mavlink = new MAVLink.MavlinkParse();
        bool armed = false;
        // locking to prevent multiple reads on serial port
        object readlock = new object();
        // our target sysid
        byte sysid;
        // our target compid
        byte compid;

        // Giak stuff
        List<uint> listItemIds = new List<uint>();
        Dictionary<int, object> droneDataDictionary = new Dictionary<int, object>();

        public simpleexample()
        {
            InitializeComponent();
        }

        private void but_connect_Click(object sender, EventArgs e)
        {
            // if the port is open close it
            if (serialPort1.IsOpen)
            {
                serialPort1.Close();
                return;
            }

            // set the comport options
            serialPort1.PortName = CMB_comport.Text;
            serialPort1.BaudRate = int.Parse(cmb_baudrate.Text);

            // open the comport
            serialPort1.Open();

            // set timeout to 2 seconds
            serialPort1.ReadTimeout = 2000;

            BackgroundWorker bgw = new BackgroundWorker();

            bgw.DoWork += bgw_DoWork;

            bgw.RunWorkerAsync();
        }

        void bgw_DoWork(object sender, DoWorkEventArgs e)
        {
            while (serialPort1.IsOpen)
            {
                try
                {
                    MAVLink.MAVLinkMessage packet;
                    lock (readlock)
                    {
                        // read any valid packet from the port
                        packet = mavlink.ReadPacket(serialPort1.BaseStream);
                        
                        // check its valid
                        if (packet == null || packet.data == null)
                            continue;
                    }

                    // check to see if its a hb packet from the comport
                    if (packet.data.GetType() == typeof(MAVLink.mavlink_heartbeat_t))
                    {
                        
                        var hb = (MAVLink.mavlink_heartbeat_t)packet.data;
                        
                        // save the sysid and compid of the seen MAV
                        sysid = packet.sysid;
                        compid = packet.compid;

                        // request streams at 2 hz
                        var buffer = mavlink.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.REQUEST_DATA_STREAM,
                            new MAVLink.mavlink_request_data_stream_t()
                            {
                                req_message_rate = 2,
                                req_stream_id = (byte)MAVLink.MAV_DATA_STREAM.ALL,
                                start_stop = 1,
                                target_component = compid,
                                target_system = sysid
                            });

                        serialPort1.Write(buffer, 0, buffer.Length);

                        buffer = mavlink.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.HEARTBEAT, hb);

                        serialPort1.Write(buffer, 0, buffer.Length);
                    }

                    // from here we should check the the message is addressed to us
                    if (sysid != packet.sysid || compid != packet.compid)
                        continue;

                    Console.WriteLine(packet.msgtypename);

                    // Update the data in the dictionary.
                    //
                    droneDataDictionary[(int)packet.msgid] = packet.data;
                    
                    listBox1.Invoke(new Action(() =>
                    {

                        if (!listBox1.Items.Contains(packet.msgtypename))
                        {
                            listBox1.Items.Add(packet.msgtypename);
                            listItemIds.Add(packet.msgid);
                        }

                    }));

                    if (packet.msgid == (byte)MAVLink.MAVLINK_MSG_ID.ATTITUDE)
                    //or
                    //if (packet.data.GetType() == typeof(MAVLink.mavlink_attitude_t))
                    {
                        var att = (MAVLink.mavlink_attitude_t)packet.data;
                        //Console.WriteLine(packet.msgtypename + " => " + att.pitch*57.2958 + " " + att.roll*57.2958);
                    }

                    string d = GetData((int)packet.msgid, packet.data);
                    BeginInvoke(new Action(() => richTextBox1.Text = d));

                }
                catch(Exception ex)
                {
                    MessageBox.Show(ex.ToString());
                }

                System.Threading.Thread.Sleep(1);
            }
        }

        private string GetData(int id, object droneData)
        {
            string ret = "";
            if (id == (int)MAVLink.MAVLINK_MSG_ID.HEARTBEAT)
            {
                MAVLink.mavlink_heartbeat_t data = (MAVLink.mavlink_heartbeat_t)droneDataDictionary[id];

                foreach (var field in typeof(MAVLink.mavlink_heartbeat_t).GetFields(BindingFlags.Instance |
                                                                 BindingFlags.NonPublic |
                                                                 BindingFlags.Public))
                {
                    ret += String.Format("{0} = {1}\n", field.Name, field.GetValue(data));
                }
            }
            else if (id == (int)MAVLink.MAVLINK_MSG_ID.POWER_STATUS)
            {
                MAVLink.mavlink_power_status_t data = (MAVLink.mavlink_power_status_t)droneDataDictionary[id];

                foreach (var field in typeof(MAVLink.mavlink_power_status_t).GetFields(BindingFlags.Instance |
                                                                 BindingFlags.NonPublic |
                                                                 BindingFlags.Public))
                {
                    ret += String.Format("{0} = {1}\n", field.Name, field.GetValue(data));
                }
            }
            else if (id == (int)MAVLink.MAVLINK_MSG_ID.LOCAL_POSITION_NED)
            {
                MAVLink.mavlink_local_position_ned_t data = (MAVLink.mavlink_local_position_ned_t)droneDataDictionary[id];

                foreach (var field in typeof(MAVLink.mavlink_local_position_ned_t).GetFields(BindingFlags.Instance |
                                                                 BindingFlags.NonPublic |
                                                                 BindingFlags.Public))
                {
                    ret += String.Format("{0} = {1}\n", field.Name, field.GetValue(data));
                }
            }
            else if (id == (int)MAVLink.MAVLINK_MSG_ID.VIBRATION)
            {
                MAVLink.mavlink_vibration_t data = (MAVLink.mavlink_vibration_t)droneDataDictionary[id];

                foreach (var field in typeof(MAVLink.mavlink_vibration_t).GetFields(BindingFlags.Instance |
                                                                 BindingFlags.NonPublic |
                                                                 BindingFlags.Public))
                {
                    ret += String.Format("{0} = {1}\n", field.Name, field.GetValue(data));
                }
            }
            else if (id == (int)MAVLink.MAVLINK_MSG_ID.VFR_HUD)
            {
                MAVLink.mavlink_vfr_hud_t data = (MAVLink.mavlink_vfr_hud_t)droneDataDictionary[id];

                foreach (var field in typeof(MAVLink.mavlink_vfr_hud_t).GetFields(BindingFlags.Instance |
                                                                 BindingFlags.NonPublic |
                                                                 BindingFlags.Public))
                {
                    ret += String.Format("{0} = {1}\n", field.Name, field.GetValue(data));
                }
            }
            else if (id == (int)MAVLink.MAVLINK_MSG_ID.SIMSTATE)
            {
                MAVLink.mavlink_simstate_t data = (MAVLink.mavlink_simstate_t)droneDataDictionary[id];

                foreach (var field in typeof(MAVLink.mavlink_simstate_t).GetFields(BindingFlags.Instance |
                                                                 BindingFlags.NonPublic |
                                                                 BindingFlags.Public))
                {
                    ret += String.Format("{0} = {1}\n", field.Name, field.GetValue(data));
                }
            }
            else if (id == (int)MAVLink.MAVLINK_MSG_ID.AHRS2)
            {
                MAVLink.mavlink_ahrs2_t data = (MAVLink.mavlink_ahrs2_t)droneDataDictionary[id];

                foreach (var field in typeof(MAVLink.mavlink_ahrs2_t).GetFields(BindingFlags.Instance |
                                                                 BindingFlags.NonPublic |
                                                                 BindingFlags.Public))
                {
                    ret += String.Format("{0} = {1}\n", field.Name, field.GetValue(data));
                }
            }
            else if (id == (int)MAVLink.MAVLINK_MSG_ID.AHRS3)
            {
                MAVLink.mavlink_ahrs3_t data = (MAVLink.mavlink_ahrs3_t)droneDataDictionary[id];

                foreach (var field in typeof(MAVLink.mavlink_ahrs3_t).GetFields(BindingFlags.Instance |
                                                                 BindingFlags.NonPublic |
                                                                 BindingFlags.Public))
                {
                    ret += String.Format("{0} = {1}\n", field.Name, field.GetValue(data));
                }
            }
            else if (id == (int)MAVLink.MAVLINK_MSG_ID.ATTITUDE)
            {
                MAVLink.mavlink_attitude_t data = (MAVLink.mavlink_attitude_t)droneDataDictionary[id];

                foreach (var field in typeof(MAVLink.mavlink_attitude_t).GetFields(BindingFlags.Instance |
                                                                 BindingFlags.NonPublic |
                                                                 BindingFlags.Public))
                {
                    ret += String.Format("{0} = {1}\n", field.Name, field.GetValue(data));
                }
            }
            else if (id == (int)MAVLink.MAVLINK_MSG_ID.MISSION_CURRENT)
            {
                MAVLink.mavlink_mission_current_t data = (MAVLink.mavlink_mission_current_t)droneDataDictionary[id];

                foreach (var field in typeof(MAVLink.mavlink_mission_current_t).GetFields(BindingFlags.Instance |
                                                                 BindingFlags.NonPublic |
                                                                 BindingFlags.Public))
                {
                    ret += String.Format("{0} = {1}\n", field.Name, field.GetValue(data));
                }
            }
            else if (id == (int)MAVLink.MAVLINK_MSG_ID.SERVO_OUTPUT_RAW)
            {
                MAVLink.mavlink_servo_output_raw_t data = (MAVLink.mavlink_servo_output_raw_t)droneDataDictionary[id];

                foreach (var field in typeof(MAVLink.mavlink_servo_output_raw_t).GetFields(BindingFlags.Instance |
                                                                 BindingFlags.NonPublic |
                                                                 BindingFlags.Public))
                {
                    ret += String.Format("{0} = {1}\n", field.Name, field.GetValue(data));
                }
            }
            else if (id == (int)MAVLink.MAVLINK_MSG_ID.SERVO_OUTPUT_RAW)
            {
                MAVLink.mavlink_servo_output_raw_t data = (MAVLink.mavlink_servo_output_raw_t)droneDataDictionary[id];

                foreach (var field in typeof(MAVLink.mavlink_servo_output_raw_t).GetFields(BindingFlags.Instance |
                                                                 BindingFlags.NonPublic |
                                                                 BindingFlags.Public))
                {
                    ret += String.Format("{0} = {1}\n", field.Name, field.GetValue(data));
                }
            }

            return ret == "" ? "" : ret.Substring(0, ret.Length-1);

        }

        T readsomedata<T>(byte sysid,byte compid,int timeout = 2000)
        {
            DateTime deadline = DateTime.Now.AddMilliseconds(timeout);
           
            lock (readlock)
            {
                // read the current buffered bytes
                while (DateTime.Now < deadline)
                {
                    var packet = mavlink.ReadPacket(serialPort1.BaseStream);

                    // check its not null, and its addressed to us
                    if (packet == null || sysid != packet.sysid || compid != packet.compid)
                        continue;

                    //Console.WriteLine(packet);

                    if (packet.data.GetType() == typeof (T))
                    {
                        return (T) packet.data;
                    }
                }
            }

            throw new Exception("No packet match found");
        }

        private void but_armdisarm_Click(object sender, EventArgs e)
        {
            MAVLink.mavlink_command_long_t req = new MAVLink.mavlink_command_long_t();

            req.target_system = 1;
            req.target_component = 1;

            req.command = (ushort)MAVLink.MAV_CMD.COMPONENT_ARM_DISARM;

            req.param1 = armed ? 0 : 1;
            armed = !armed;
            /*
            req.param2 = p2;
            req.param3 = p3;
            req.param4 = p4;
            req.param5 = p5;
            req.param6 = p6;
            req.param7 = p7;
            */

            byte[] packet = mavlink.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, req);

            serialPort1.Write(packet, 0, packet.Length);

            try
            {
                var ack = readsomedata<MAVLink.mavlink_command_ack_t>(sysid, compid);
                if (ack.result == (byte)MAVLink.MAV_RESULT.ACCEPTED) 
                {

                }
            }
            catch 
            { 
            }
        }

        private void CMB_comport_Click(object sender, EventArgs e)
        {
            CMB_comport.DataSource = SerialPort.GetPortNames();
        }

        private void but_mission_Click(object sender, EventArgs e)
        {
            MAVLink.mavlink_mission_count_t req = new MAVLink.mavlink_mission_count_t();

            req.target_system = 1;
            req.target_component = 1;

            // set wp count
            req.count = 1;

            byte[] packet = mavlink.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.MISSION_COUNT, req);
            //Console.WriteLine("MISSION_COUNT send");
            serialPort1.Write(packet, 0, packet.Length);

            var ack = readsomedata<MAVLink.mavlink_mission_request_t>(sysid, compid);
            if (ack.seq == 0)
            {
                MAVLink.mavlink_mission_item_int_t req2 = new MAVLink.mavlink_mission_item_int_t();

                req2.target_system = sysid;
                req2.target_component = compid;

                req2.command = (byte)MAVLink.MAV_CMD.WAYPOINT;

                req2.current = 1;
                req2.autocontinue = 0;

                req2.frame = (byte)MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT;

                req2.y = (int) (115 * 1.0e7);
                req2.x = (int) (-35 * 1.0e7);

                req2.z = (float) (2.34);

                req2.param1 = 0;
                req2.param2 = 0;
                req2.param3 = 0;
                req2.param4 = 0;

                req2.seq = 0;

                packet = mavlink.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.MISSION_ITEM_INT, req2);
                //Console.WriteLine("MISSION_ITEM_INT send");
                lock (readlock)
                {
                    serialPort1.Write(packet, 0, packet.Length);

                    var ack2 = readsomedata<MAVLink.mavlink_mission_ack_t>(sysid, compid);
                    if ((MAVLink.MAV_MISSION_RESULT) ack2.type != MAVLink.MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED)
                    {

                    }
                }


                MAVLink.mavlink_mission_ack_t req3 = new MAVLink.mavlink_mission_ack_t();
                req3.target_system = 1;
                req3.target_component = 1;
                req3.type = 0;

                packet = mavlink.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.MISSION_ACK, req3);
                //Console.WriteLine("MISSION_ACK send");
                serialPort1.Write(packet, 0, packet.Length);
            }
        }

        private void simpleexample_Load(object sender, EventArgs e)
        {

        }

        private void listBox1_SelectedIndexChanged(object sender, EventArgs e)
        {

        }

        private void listBox1_DoubleClick(object sender, EventArgs e)
        {

        }
    }
}