zr-v4/Tools/ArdupilotMegaPlanner/SerialOutput2.cs

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

namespace ArdupilotMega
{
    public partial class SerialOutput2 : Form
    {
        System.Threading.Thread t12;
        static bool threadrun = false;
        static internal SerialPort comPort = new SerialPort();
        static internal PointLatLngAlt HomeLoc = new PointLatLngAlt(0,0,0,"Home");

        public SerialOutput2()
        {
            InitializeComponent();

            CMB_serialport.DataSource = SerialPort.GetPortNames();

            if (threadrun)
            {
                BUT_connect.Text = "Stop";
            }
        }

        private void BUT_connect_Click(object sender, EventArgs e)
        {
            if (comPort.IsOpen)
            {
                threadrun = false;
                comPort.Close();
                BUT_connect.Text = "Connect";
            }
            else
            {
                try
                {
                    comPort.PortName = CMB_serialport.Text;
                }
                catch { CustomMessageBox.Show("Invalid PortName"); return; }
                try {
                comPort.BaudRate = int.Parse(CMB_baudrate.Text);
                } catch {CustomMessageBox.Show("Invalid BaudRate"); return;}
                try {
                comPort.Open();
                } catch {CustomMessageBox.Show("Error Connecting\nif using com0com please rename the ports to COM??"); return;}

                t12 = new System.Threading.Thread(new System.Threading.ThreadStart(mainloop))
                {
                    IsBackground = true,
                    Name = "Nmea output"
                };
                t12.Start();
            }
        }

        const float rad2deg = (float)(180 / Math.PI);
        const float deg2rad = (float)(1.0 / rad2deg);

        void mainloop()
        {
            threadrun = true;
            int counter = 0;
            while (threadrun)
            {
                try
                {
                    //http://www.microdrones.com/wiki/index.php/Downlink_Protocol

                    /*
...
#1,27,48,1,1,1,1,0,25343,8192,85
#2,1,0,0,0,-100,-100,0,-100,-100,50,0,0,0,100,167
#3,0,0,0,0,13
#4,9,111731750,1595,0,197
#5,398843292,55871844,492974923,2.240,9,5
#6,0.15,-0.06,0.21,1.0,197
#7,-0.042,-0.022,2.474,182
#8,0.000,-418.438,2158,159
#9,-139,-139,431,199
#10,1.496,-1.628,0.842,161
#11,0,0,0,58
#12,,33
#1,27,48,1,1,1,1,0,25343,8192,85
#2,1,0,0,0,-100,-100,0,-100,-100,50,0,0,0,100,167
#3,0,0,0,0,13
#4,9,111732000,1595,0,208
#5,398843288,55871840,492974925,2.240,9,2
#6,0.07,-0.02,0.16,0.9,188
#7,-0.042,-0.019,2.475,175
#8,0.000,-418.233,2158,166
#9,-139,-139,431,199
#10,1.354,-1.549,0.972,162
#11,0,0,0,58
#12,,33
...

                     */

                    writeline("#1,28,07,2,1,1,1,2,16000,0,2,");

                 //   writeline("#2,100,99,98,97,96,95,94,93,92,");

                   // writeline("#3,100,120,140,160,");

                    int week, seconds;
                    GetGPSTime(out week, out seconds);

                    writeline(string.Format("#4,{0},{1},{2},{3},", counter / 10, seconds, week, 25));

                    //    writeline("#4,counter/2,111732000,1595,0,");

                    double x,y,z;

                    GetGeo(out x, out y, out z, MainV2.cs.lat, MainV2.cs.lng, MainV2.cs.alt);

                    writeline(string.Format("#5,{0},{1},{2},{3},{4},", x * 100, y * 100, z*100, MainV2.cs.gpshdop + 0.01, MainV2.cs.satcount));

                    writeline(string.Format("#6,{0},{1},{2},{3},", MainV2.cs.groundspeed * Math.Sin(MainV2.cs.groundcourse * deg2rad),
                        MainV2.cs.groundspeed * Math.Cos(MainV2.cs.groundcourse * deg2rad),
                        MainV2.cs.verticalspeed,2));

                    writeline(string.Format("#7,{0},{1},{2},", MainV2.cs.roll * deg2rad, MainV2.cs.pitch * deg2rad, MainV2.cs.yaw * deg2rad));

                    writeline(string.Format("#8,{0},{1},{2},", MainV2.cs.alt , MainV2.cs.alt , MainV2.cs.press_temp));

                    writeline(string.Format("#9,{0},{1},{2},", MainV2.cs.mx, MainV2.cs.my, MainV2.cs.mz));

                  //  writeline(string.Format("#10,{0},{1},{2},", 1.354,-1.549,0.972));

                    System.Threading.Thread.Sleep(100);
                    counter++;
                }
                catch { }
            }
        }

        DateTime GetFromGps(int weeknumber, int seconds)
        {
            DateTime datum = new DateTime(1980, 1, 6, 0, 0, 0, DateTimeKind.Utc);
            DateTime week = datum.AddDays(weeknumber * 7);
            DateTime time = week.AddSeconds(seconds);
            return time;
        }

        void GetGPSTime(out int weeknumber, out int seconds)
        {
            DateTime datum = new DateTime(1980, 1, 6, 0, 0, 0,DateTimeKind.Utc);

            TimeSpan ts = DateTime.Now - datum;

            weeknumber = ((int)ts.TotalDays / 7);

            ts = DateTime.Now - GetFromGps(weeknumber, 0);

            seconds =  (int)ts.TotalSeconds;
        }


        void GetDegrees(double x, double y, double z, out double latitude, out double longitude)
        {
            double deg = 0.01745329252;
            double r = Math.Sqrt(x * x + y * y);
            longitude = Math.Asin(y / r) / deg;
            if (longitude > 180d)
                longitude = 180d - longitude;

            latitude = Math.Atan(z / r) / deg;
        }

        void GetGeo(out double x, out double y, out double z, double latitude, double longitude, double alt)
        {
            double wgs84a = 6378137;
            double wgs84f = 1.0 / 298.257223563;
            double wgs84b = wgs84a * (1.0 - wgs84f);

            double clat = Math.Cos(deg2rad * latitude);
            double slat = Math.Sin(deg2rad * latitude);
            double clon = Math.Cos(deg2rad * longitude);
            double slon = Math.Sin(deg2rad * longitude);

            var ecc = Math.Sqrt(2 * wgs84f - Math.Pow(wgs84f, 2));
            var esq = ecc * ecc;

            alt = alt * 0.0001;

                        // var rrnrm  = radcur (flat);
            var rn = wgs84a;/// (Math.Sqrt(1-esq)*(slat*slat));// rrnrm[1];
            var re = wgs84a;// rrnrm[0];

            x = (rn + alt) * clat * clon;
            y = (rn + alt) * clat * slon;
            z = ((1 - esq) * rn + (alt)) * slat;

        }

        void writeline(string line)
        {
            comPort.Write(line + checksum(line) + "\r\n");
        }

        byte checksum(string line)
        {
            byte ans = 0;
            foreach (char ch in line.ToCharArray())
            {
                ans += (byte)ch;
            }
            ans = (byte)(ans ^ 0xff);

            return ans;
        }

        private void SerialOutput_FormClosing(object sender, FormClosingEventArgs e)
        {
        }

   

    }
}