Esempio n. 1
0
        public void GoToTargetPoint(PointLatLngAlt target)
        {
            Locationwp gotohere = new Locationwp();

            gotohere.id  = (byte)MAVLink.MAV_CMD.WAYPOINT;
            gotohere.alt = target.Alt; // back to m
            gotohere.lat = target.Lat;
            gotohere.lng = target.Lng;
            MAVLink.MAV_MISSION_RESULT ans = this.setWP(gotohere, 0, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT, (byte)2);
            this.ListMessage("正在发送直达航点...");
        }
 private void updateLocationLabel(Locationwp plla)
 {
     if (!Instance.IsDisposed)
     {
         Instance.BeginInvoke(
             (MethodInvoker)
             delegate
         {
             Instance.LBL_location.Text = gotolocation.Lat + " " + gotolocation.Lng + " " +
                                          gotolocation.Alt + " " + gotolocation.Tag;
         }
             );
     }
 }
Esempio n. 3
0
        private static bool IsValidWaypoint(Locationwp location)
        {
            // Command that can contain latitude and longitude
            var cmd = (MAVLink.MAV_CMD)location.id;

            if (cmd != MAVLink.MAV_CMD.WAYPOINT &&
                cmd != MAVLink.MAV_CMD.LOITER_UNLIM &&
                cmd != MAVLink.MAV_CMD.LOITER_TURNS &&
                cmd != MAVLink.MAV_CMD.LOITER_TIME &&
                cmd != MAVLink.MAV_CMD.LAND &&
                cmd != MAVLink.MAV_CMD.TAKEOFF &&
                cmd != MAVLink.MAV_CMD.FOLLOW &&
                cmd != MAVLink.MAV_CMD.LOITER_TO_ALT &&
                cmd != MAVLink.MAV_CMD.PATHPLANNING &&
                cmd != MAVLink.MAV_CMD.SPLINE_WAYPOINT &&
                cmd != MAVLink.MAV_CMD.VTOL_TAKEOFF &&
                cmd != MAVLink.MAV_CMD.VTOL_LAND &&
                cmd != MAVLink.MAV_CMD.PAYLOAD_PLACE &&
                cmd != MAVLink.MAV_CMD.DO_SET_HOME &&
                cmd != MAVLink.MAV_CMD.DO_LAND_START &&
                cmd != MAVLink.MAV_CMD.DO_REPOSITION &&
                cmd != MAVLink.MAV_CMD.DO_SET_ROI_LOCATION &&
                cmd != MAVLink.MAV_CMD.DO_MOUNT_CONTROL &&
                cmd != MAVLink.MAV_CMD.OVERRIDE_GOTO &&
                cmd != MAVLink.MAV_CMD.SET_GUIDED_SUBMODE_CIRCLE &&
                cmd != MAVLink.MAV_CMD.FENCE_RETURN_POINT &&
                cmd != MAVLink.MAV_CMD.FENCE_POLYGON_VERTEX_INCLUSION &&
                cmd != MAVLink.MAV_CMD.FENCE_POLYGON_VERTEX_EXCLUSION &&
                cmd != MAVLink.MAV_CMD.FENCE_CIRCLE_INCLUSION &&
                cmd != MAVLink.MAV_CMD.FENCE_CIRCLE_EXCLUSION &&
                cmd != MAVLink.MAV_CMD.RALLY_POINT &&
                cmd != MAVLink.MAV_CMD.PAYLOAD_PREPARE_DEPLOY &&
                cmd != MAVLink.MAV_CMD.WAYPOINT_USER_1 &&
                cmd != MAVLink.MAV_CMD.WAYPOINT_USER_2 &&
                cmd != MAVLink.MAV_CMD.WAYPOINT_USER_3 &&
                cmd != MAVLink.MAV_CMD.WAYPOINT_USER_4 &&
                cmd != MAVLink.MAV_CMD.WAYPOINT_USER_5 &&
                cmd != MAVLink.MAV_CMD.SPATIAL_USER_1 &&
                cmd != MAVLink.MAV_CMD.SPATIAL_USER_2 &&
                cmd != MAVLink.MAV_CMD.SPATIAL_USER_3 &&
                cmd != MAVLink.MAV_CMD.SPATIAL_USER_4 &&
                cmd != MAVLink.MAV_CMD.SPATIAL_USER_5)
            {
                return(false);
            }

            // Exclude "empty" latitude and longitude
            return(location.lat != 0 && location.lng != 0);
        }
Esempio n. 4
0
 private MAVLink.MAV_MISSION_RESULT setWP(Locationwp loc, ushort index, MAVLink.MAV_FRAME frame, byte current = 0, byte autocontinue = 1)
 {
     MAVLink.mavlink_mission_item_t req = new MAVLink.mavlink_mission_item_t();
     req.target_system    = TARGET_SYSTEM_ID;
     req.target_component = TARGET_SYS_COMPID; // MSG_NAMES.MISSION_ITEM
     req.command          = loc.id;
     req.current          = current;
     req.autocontinue     = autocontinue;
     req.frame            = (byte)frame;
     req.y      = (float)(loc.lng);
     req.x      = (float)(loc.lat);
     req.z      = (float)(loc.alt);
     req.param1 = loc.p1;
     req.param2 = loc.p2;
     req.param3 = loc.p3;
     req.param4 = loc.p4;
     req.seq    = index;
     return(setWP(req));
 }
        private void LoiterAroundPoint(PointLatLngAlt gimbalPoint)
        {
            Locationwp gotohere = new Locationwp
            {
                id  = (ushort)MAVLink.MAV_CMD.WAYPOINT,
                alt = MainV2.comPort.MAV.GuidedMode.z > 0 ? MainV2.comPort.MAV.GuidedMode.z : 100,
                lat = gimbalPoint.Lat,
                lng = gimbalPoint.Lng
            };

            try
            {
                MainV2.comPort.setGuidedModeWP(gotohere);
            }
            catch (Exception ex)
            {
                MainV2.comPort.giveComport = false;
                CustomMessageBox.Show(Strings.CommandFailed + ex.Message, Strings.ERROR);
            }
        }
Esempio n. 6
0
        public void DoAcceptTcpClientCallback(IAsyncResult ar)
        {
            // Get the listener that handles the client request.
            TcpListener listener = (TcpListener)ar.AsyncState;

            // End the operation and display the received data on
            // the console.
            using (
            TcpClient client = listener.EndAcceptTcpClient(ar))
            {

                // Signal the calling thread to continue.
                tcpClientConnected.Set();

                try
                {

                    // Get a stream object for reading and writing
                    log.Info("Accepted Client " + client.Client.RemoteEndPoint.ToString());
                    //client.SendBufferSize = 100 * 1024; // 100kb
                    //client.LingerState.Enabled = true;
                    //client.NoDelay = true;

                    // makesure we have valid image
                    GCSViews.FlightData.mymap.streamjpgenable = true;
                    GCSViews.FlightData.myhud.streamjpgenable = true;

                    NetworkStream stream = client.GetStream();

                    // 3 seconds
                    stream.ReadTimeout = 3000;

                again:

                    var asciiEncoding = new ASCIIEncoding();

                    var request = new byte[1024];

                    int len = stream.Read(request, 0, request.Length);
                    string head = System.Text.Encoding.ASCII.GetString(request, 0, len);
                    log.Info(head);

                    int index = head.IndexOf('\n');

                    string url = head.Substring(0, index - 1);
                    //url = url.Replace("\r", "");
                    //url = url.Replace("GET ","");
                    //url = url.Replace(" HTTP/1.0", "");
                    //url = url.Replace(" HTTP/1.1", "");

                    if (url.Contains("websocket"))
                    {
                        using (var writer = new StreamWriter(stream, Encoding.Default))
                        {
                            writer.WriteLine("HTTP/1.1 101 WebSocket Protocol Handshake");
                            writer.WriteLine("Upgrade: WebSocket");
                            writer.WriteLine("Connection: Upgrade");
                            writer.WriteLine("WebSocket-Location: ws://localhost:56781/websocket/server");

                            int start = head.IndexOf("Sec-WebSocket-Key:") + 19;
                            int end = head.IndexOf('\r', start);
                            if (end == -1)
                                end = head.IndexOf('\n', start);
                            string accept = ComputeWebSocketHandshakeSecurityHash09(head.Substring(start, end - start));

                            writer.WriteLine("Sec-WebSocket-Accept: " + accept);

                            writer.WriteLine("Server: APM Planner");

                            writer.WriteLine("");

                            writer.Flush();

                            while (client.Connected)
                            {
                                Thread.Sleep(200);
                                log.Debug(stream.DataAvailable + " " + client.Available);

                                while (client.Available > 0)
                                {
                                    Console.Write(stream.ReadByte());
                                }

                                byte[] packet = new byte[256];

                                string sendme = MainV2.comPort.MAV.cs.roll + "," + MainV2.comPort.MAV.cs.pitch + "," + MainV2.comPort.MAV.cs.yaw;

                                packet[0] = 0x81; // fin - binary
                                packet[1] = (byte)sendme.Length;

                                int i = 2;
                                foreach (char ch in sendme)
                                {
                                    packet[i++] = (byte)ch;
                                }

                                stream.Write(packet, 0, i);

                                //break;
                            }
                        }
                    }
                    else if (url.Contains("georefnetwork.kml"))
                    {
                        string header = "HTTP/1.1 200 OK\r\nContent-Type: application/vnd.google-earth.kml+xml\r\nContent-Length: " + georefkml.Length + "\r\n\r\n";
                        byte[] temp = asciiEncoding.GetBytes(header);
                        stream.Write(temp, 0, temp.Length);

                        byte[] buffer = Encoding.ASCII.GetBytes(georefkml);

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

                        goto again;

                        //stream.Close();
                    }
                    else if (url.Contains("network.kml"))
                    {

                        SharpKml.Dom.Document kml = new SharpKml.Dom.Document();

                        SharpKml.Dom.Placemark pmplane = new SharpKml.Dom.Placemark();
                        pmplane.Name = "P/Q ";

                        pmplane.Visibility = true;

                        SharpKml.Dom.Location loc = new SharpKml.Dom.Location();
                        loc.Latitude = MainV2.comPort.MAV.cs.lat;
                        loc.Longitude = MainV2.comPort.MAV.cs.lng;
                        loc.Altitude = MainV2.comPort.MAV.cs.alt;

                        if (loc.Altitude < 0)
                            loc.Altitude = 0.01;

                        SharpKml.Dom.Orientation ori = new SharpKml.Dom.Orientation();
                        ori.Heading = MainV2.comPort.MAV.cs.yaw;
                        ori.Roll = -MainV2.comPort.MAV.cs.roll;
                        ori.Tilt = -MainV2.comPort.MAV.cs.pitch;

                        SharpKml.Dom.Scale sca = new SharpKml.Dom.Scale();

                        sca.X = 2;
                        sca.Y = 2;
                        sca.Z = 2;

                        SharpKml.Dom.Model model = new SharpKml.Dom.Model();
                        model.Location = loc;
                        model.Orientation = ori;
                        model.AltitudeMode = SharpKml.Dom.AltitudeMode.Absolute;
                        model.Scale = sca;

                        SharpKml.Dom.Link link = new SharpKml.Dom.Link();
                        link.Href = new Uri("block_plane_0.dae", UriKind.Relative);

                        model.Link = link;

                        pmplane.Geometry = model;

                        SharpKml.Dom.LookAt la = new SharpKml.Dom.LookAt()
                        {
                            Altitude = loc.Altitude.Value,
                            Latitude = loc.Latitude.Value,
                            Longitude = loc.Longitude.Value,
                            Tilt = 80,
                            Heading = MainV2.comPort.MAV.cs.yaw,
                            AltitudeMode = SharpKml.Dom.AltitudeMode.Absolute,
                            Range = 50
                        };

                        kml.Viewpoint = la;

                        kml.AddFeature(pmplane);

                        SharpKml.Dom.CoordinateCollection coords = new SharpKml.Dom.CoordinateCollection();

                        foreach (var point in MainV2.comPort.MAV.wps.Values)
                        {
                            coords.Add(new SharpKml.Base.Vector(point.x, point.y, point.z));
                        }

                        SharpKml.Dom.LineString ls = new SharpKml.Dom.LineString();
                        ls.AltitudeMode = SharpKml.Dom.AltitudeMode.RelativeToGround;
                        ls.Coordinates = coords;

                        SharpKml.Dom.Placemark pm = new SharpKml.Dom.Placemark() { Geometry = ls };

                        kml.AddFeature(pm);

                        SharpKml.Base.Serializer serializer = new SharpKml.Base.Serializer();
                        serializer.Serialize(kml);

                        byte[] buffer = Encoding.ASCII.GetBytes(serializer.Xml);

                        string header = "HTTP/1.1 200 OK\r\nContent-Type: application/vnd.google-earth.kml+xml\r\nContent-Length: " + buffer.Length + "\r\n\r\n";
                        byte[] temp = asciiEncoding.GetBytes(header);
                        stream.Write(temp, 0, temp.Length);

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

                        goto again;

                        //stream.Close();
                    }
                    else if (url.Contains("block_plane_0.dae"))
                    {
                        string header = "HTTP/1.1 200 OK\r\nContent-Type: text/plain\r\n\r\n";
                        byte[] temp = asciiEncoding.GetBytes(header);
                        stream.Write(temp, 0, temp.Length);

                        BinaryReader file = new BinaryReader(File.Open("block_plane_0.dae", FileMode.Open, FileAccess.Read, FileShare.Read));
                        byte[] buffer = new byte[1024];
                        while (file.PeekChar() != -1)
                        {

                            int leng = file.Read(buffer, 0, buffer.Length);

                            stream.Write(buffer, 0, leng);
                        }
                        file.Close();
                        stream.Close();
                    }
                    else if (url.Contains("hud.html"))
                    {
                        string header = "HTTP/1.1 200 OK\r\nContent-Type: text/html\r\n\r\n";
                        byte[] temp = asciiEncoding.GetBytes(header);
                        stream.Write(temp, 0, temp.Length);

                        BinaryReader file = new BinaryReader(File.Open("hud.html", FileMode.Open, FileAccess.Read, FileShare.Read));
                        byte[] buffer = new byte[1024];
                        while (file.PeekChar() != -1)
                        {

                            int leng = file.Read(buffer, 0, buffer.Length);

                            stream.Write(buffer, 0, leng);
                        }
                        file.Close();
                        stream.Close();
                    }
                    else if (url.ToLower().Contains("hud.jpg") || url.ToLower().Contains("map.jpg") || url.ToLower().Contains("both.jpg"))
                    {
                        refreshmap();

                        string header = "HTTP/1.1 200 OK\r\nContent-Type: multipart/x-mixed-replace;boundary=APMPLANNER\r\n\r\n--APMPLANNER\r\n";
                        byte[] temp = asciiEncoding.GetBytes(header);
                        stream.Write(temp, 0, temp.Length);

                        while (client.Connected)
                        {
                            System.Threading.Thread.Sleep(200); // 5hz
                            byte[] data = null;

                            if (url.ToLower().Contains("hud"))
                            {
                                GCSViews.FlightData.myhud.streamjpgenable = true;
                                data = GCSViews.FlightData.myhud.streamjpg.ToArray();
                            }
                            else if (url.ToLower().Contains("map"))
                            {
                                GCSViews.FlightData.mymap.streamjpgenable = true;
                                data = GCSViews.FlightData.mymap.streamjpg.ToArray();
                            }
                            else
                            {
                                GCSViews.FlightData.mymap.streamjpgenable = true;
                                GCSViews.FlightData.myhud.streamjpgenable = true;
                                Image img1 = Image.FromStream(GCSViews.FlightData.myhud.streamjpg);
                                Image img2 = Image.FromStream(GCSViews.FlightData.mymap.streamjpg);
                                int bigger = img1.Height > img2.Height ? img1.Height : img2.Height;
                                Image imgout = new Bitmap(img1.Width + img2.Width, bigger);

                                Graphics grap = Graphics.FromImage(imgout);

                                grap.DrawImageUnscaled(img1, 0, 0);
                                grap.DrawImageUnscaled(img2, img1.Width, 0);

                                MemoryStream streamjpg = new MemoryStream();
                                imgout.Save(streamjpg, System.Drawing.Imaging.ImageFormat.Jpeg);
                                data = streamjpg.ToArray();

                            }

                            header = "Content-Type: image/jpeg\r\nContent-Length: " + data.Length + "\r\n\r\n";
                            temp = asciiEncoding.GetBytes(header);
                            stream.Write(temp, 0, temp.Length);

                            stream.Write(data, 0, data.Length);

                            header = "\r\n--APMPLANNER\r\n";
                            temp = asciiEncoding.GetBytes(header);
                            stream.Write(temp, 0, temp.Length);

                        }
                        GCSViews.FlightData.mymap.streamjpgenable = false;
                        GCSViews.FlightData.myhud.streamjpgenable = false;
                        stream.Close();

                    }
                    else if (url.Contains("/guided?"))
                    {
                        //http://127.0.0.1:56781/guided?lat=-34&lng=117.8&alt=30

                        Regex rex = new Regex(@"lat=([\-\.0-9]+)&lng=([\-\.0-9]+)&alt=([\.0-9]+)", RegexOptions.IgnoreCase);

                        Match match = rex.Match(url);

                        if (match.Success)
                        {
                            Locationwp gwp = new Locationwp()
                            {
                                lat = double.Parse(match.Groups[1].Value),
                                lng = double.Parse(match.Groups[2].Value),
                                alt = float.Parse(match.Groups[3].Value)
                            };
                            try
                            {
                                MainV2.comPort.setGuidedModeWP(gwp);
                            }
                            catch { }

                            string header = "HTTP/1.1 200 OK\r\n\r\nSent Guide Mode Wp";
                            byte[] temp = asciiEncoding.GetBytes(header);
                            stream.Write(temp, 0, temp.Length);
                        }
                        else
                        {
                            string header = "HTTP/1.1 200 OK\r\n\r\nFailed Guide Mode Wp";
                            byte[] temp = asciiEncoding.GetBytes(header);
                            stream.Write(temp, 0, temp.Length);
                        }
                        stream.Close();
                    }
                    else if (url.ToLower().Contains(".jpg"))
                    {
                        Regex rex = new Regex(@"([^\s]+)\s(.+)\sHTTP/1", RegexOptions.IgnoreCase);

                        Match match = rex.Match(url);

                        if (match.Success)
                        {
                            string fileurl = match.Groups[2].Value;

                            using (Image orig = Image.FromFile(georefimagepath + fileurl))
                            using (Image resi = ResizeImage(orig, new Size(640, 480)))
                            using (MemoryStream memstream = new MemoryStream())
                            {
                                resi.Save(memstream, System.Drawing.Imaging.ImageFormat.Jpeg);

                                memstream.Position = 0;

                                string header = "HTTP/1.1 200 OK\r\nContent-Type: image/jpg\r\nContent-Length: " + memstream.Length + "\r\n\r\n";
                                byte[] temp = asciiEncoding.GetBytes(header);
                                stream.Write(temp, 0, temp.Length);

                                BinaryReader file = new BinaryReader(memstream);
                                byte[] buffer = new byte[1024];
                                while (file.BaseStream.Position < file.BaseStream.Length)
                                {

                                    int leng = file.Read(buffer, 0, buffer.Length);

                                    stream.Write(buffer, 0, leng);
                                }
                                file.Close();
                                resi.Dispose();
                                orig.Dispose();
                            }

                            goto again;

                            //stream.Close();
                        }
                        else
                        {
                            string header = "HTTP/1.1 404 not found\r\nContent-Type: image/jpg\r\n\r\n";
                            byte[] temp = asciiEncoding.GetBytes(header);
                            stream.Write(temp, 0, temp.Length);
                        }
                        stream.Close();
                    }
                    else if (url.ToLower().Contains("post /guide"))
                    {
                        Regex rex = new Regex(@"lat"":([\-\.0-9]+),""lon"":([\-\.0-9]+),""alt"":([\.0-9]+)", RegexOptions.IgnoreCase);

                        Match match = rex.Match(head);

                        if (match.Success)
                        {
                            Locationwp gwp = new Locationwp()
                            {
                                lat = double.Parse(match.Groups[1].Value),
                                lng = double.Parse(match.Groups[2].Value),
                                alt = float.Parse(match.Groups[3].Value)
                            };
                            try
                            {
                                MainV2.comPort.setGuidedModeWP(gwp);
                            }
                            catch { }

                            string header = "HTTP/1.1 200 OK\r\n\r\nSent Guide Mode Wp";
                            byte[] temp = asciiEncoding.GetBytes(header);
                            stream.Write(temp, 0, temp.Length);
                        }
                        else
                        {
                            string header = "HTTP/1.1 200 OK\r\n\r\nFailed Guide Mode Wp";
                            byte[] temp = asciiEncoding.GetBytes(header);
                            stream.Write(temp, 0, temp.Length);
                        }
                        stream.Close();
                    }
                    else if (url.ToLower().Contains("/command_long"))
                    {
                        string header = "HTTP/1.1 404 not found\r\nContent-Type: image/jpg\r\n\r\n";
                        byte[] temp = asciiEncoding.GetBytes(header);
                        stream.Write(temp, 0, temp.Length);

                        stream.Close();
                    }
                    else if (url.ToLower().Contains("/rcoverride"))
                    {
                        string header = "HTTP/1.1 404 not found\r\nContent-Type: image/jpg\r\n\r\n";
                        byte[] temp = asciiEncoding.GetBytes(header);
                        stream.Write(temp, 0, temp.Length);

                        stream.Close();
                    }
                    else if (url.ToLower().Contains("/get_mission"))
                    {
                        string header = "HTTP/1.1 404 not found\r\nContent-Type: image/jpg\r\n\r\n";
                        byte[] temp = asciiEncoding.GetBytes(header);
                        stream.Write(temp, 0, temp.Length);

                        stream.Close();
                    }
                    else if (url.ToLower().Contains("/mavlink/"))
                    {
                        /*
            GET /mavlink/ATTITUDE+VFR_HUD+NAV_CONTROLLER_OUTPUT+META_WAYPOINT+GPS_RAW_INT+HEARTBEAT+META_LINKQUALITY+GPS_STATUS+STATUSTEXT+SYS_STATUS?_=1355828718540 HTTP/1.1
            Host: ubuntu:9999
            Connection: keep-alive
            X-Requested-With: XMLHttpRequest
            User-Agent: Mozilla/5.0 (Windows NT 6.1; WOW64) AppleWebKit/537.11 (KHTML, like Gecko) Chrome/23.0.1271.97 Safari/537.11
            Accept:
            Referer: http://ubuntu:9999/index.html
            Accept-Encoding: gzip,deflate,sdch
            Accept-Language: en-GB,en-US;q=0.8,en;q=0.6
            Accept-Charset: ISO-8859-1,utf-8;q=0.7,*;q=0.3

            HTTP/1.1 200 OK
            Content-Type: application/json
            Content-Length: 2121
            Date: Thu, 29 Nov 2012 12:13:38 GMT
            Server: ubuntu

            {
            "VFR_HUD": {"msg": {"throttle": 0, "groundspeed": 0.0, "airspeed": 0.0, "climb": 0.0, "mavpackettype": "VFR_HUD", "alt": -0.47999998927116394, "heading": 108}, "index": 687, "time_usec": 0},
            "STATUSTEXT": {"msg": {"mavpackettype": "STATUSTEXT", "severity": 1, "text": "Initialising APM..."}, "index": 2, "time_usec": 0},
            "SYS_STATUS": {"msg": {"onboard_control_sensors_present": 4294966287, "load": 0, "battery_remaining": -1, "errors_count4": 0, "drop_rate_comm": 0, "errors_count2": 0, "errors_count3": 0, "errors_comm": 0, "current_battery": -1, "errors_count1": 0, "onboard_control_sensors_health": 4294966287, "mavpackettype": "SYS_STATUS", "onboard_control_sensors_enabled": 4294945807, "voltage_battery": 10080}, "index": 693, "time_usec": 0},
            "META_LINKQUALITY": {"msg": {"master_in": 11110, "mav_loss": 0, "mavpackettype": "META_LINKQUALITY", "master_out": 194, "packet_loss": 0.0}, "index": 194, "time_usec": 0},
            "ATTITUDE": {"msg": {"pitchspeed": -0.000976863200776279, "yaw": 1.8878594636917114, "rollspeed": -0.0030046366155147552, "time_boot_ms": 194676, "pitch": -0.09986469894647598, "mavpackettype": "ATTITUDE", "yawspeed": -0.0015030358918011189, "roll": -0.029391441494226456}, "index": 687, "time_usec": 0},
            "GPS_RAW_INT": {"msg": {"fix_type": 1, "cog": 0, "epv": 65535, "lon": 0, "time_usec": 0, "eph": 9999, "satellites_visible": 0, "lat": 0, "mavpackettype": "GPS_RAW_INT", "alt": 137000, "vel": 0}, "index": 687, "time_usec": 0},
            "HEARTBEAT": {"msg": {"custom_mode": 0, "system_status": 4, "base_mode": 81, "autopilot": 3, "mavpackettype": "HEARTBEAT", "type": 2, "mavlink_version": 3}, "index": 190, "time_usec": 0},
            "GPS_STATUS": {"msg": {"satellite_snr": "", "satellite_azimuth": "", "satellite_prn": "", "satellite_elevation": "", "satellites_visible": 0, "satellite_used": "", "mavpackettype": "GPS_STATUS"}, "index": 2, "time_usec": 0},
            "NAV_CONTROLLER_OUTPUT": {"msg": {"wp_dist": 0, "nav_pitch": 0.0, "target_bearing": 0, "nav_roll": 0.0, "aspd_error": 0.0, "alt_error": 0.0, "mavpackettype": "NAV_CONTROLLER_OUTPUT", "xtrack_error": 0.0, "nav_bearing": 0}, "index": 687, "time_usec": 0}}
                      */

                        JavaScriptSerializer serializer = new JavaScriptSerializer();

                        object[] data = new object[20];

                        Messagejson message = new Messagejson();

                        if (MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_ATTITUDE] != null)
                            message.ATTITUDE = new Message2() { index = MainV2.comPort.MAV.packetseencount[MAVLink.MAVLINK_MSG_ID_ATTITUDE], msg = MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_ATTITUDE].ByteArrayToStructure<MAVLink.mavlink_attitude_t>(6) };
                        if (MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_VFR_HUD] != null)
                            message.VFR_HUD = new Message2() { index = MainV2.comPort.MAV.packetseencount[MAVLink.MAVLINK_MSG_ID_VFR_HUD], msg = MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_VFR_HUD].ByteArrayToStructure<MAVLink.mavlink_vfr_hud_t>(6) };
                        if (MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT] != null)
                            message.NAV_CONTROLLER_OUTPUT = new Message2() { index = MainV2.comPort.MAV.packetseencount[MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT], msg = MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT].ByteArrayToStructure<MAVLink.mavlink_nav_controller_output_t>(6) };
                        if (MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT] != null)
                            message.GPS_RAW_INT = new Message2() { index = MainV2.comPort.MAV.packetseencount[MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT], msg = MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT].ByteArrayToStructure<MAVLink.mavlink_gps_raw_int_t>(6) };
                        if (MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_HEARTBEAT] != null)
                            message.HEARTBEAT = new Message2() { index = MainV2.comPort.MAV.packetseencount[MAVLink.MAVLINK_MSG_ID_HEARTBEAT], msg = MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_HEARTBEAT].ByteArrayToStructure<MAVLink.mavlink_heartbeat_t>(6) };
                        if (MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_GPS_STATUS] != null)
                            message.GPS_STATUS = new Message2() { index = MainV2.comPort.MAV.packetseencount[MAVLink.MAVLINK_MSG_ID_GPS_STATUS], msg = MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_GPS_STATUS].ByteArrayToStructure<MAVLink.mavlink_gps_status_t>(6) };
                        if (MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT] != null)
                            message.STATUSTEXT = new Message2() { index = MainV2.comPort.MAV.packetseencount[MAVLink.MAVLINK_MSG_ID_STATUSTEXT], msg = MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT].ByteArrayToStructure<MAVLink.mavlink_statustext_t>(6) };
                        if (MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_SYS_STATUS] != null)
                            message.SYS_STATUS = new Message2() { index = MainV2.comPort.MAV.packetseencount[MAVLink.MAVLINK_MSG_ID_SYS_STATUS], msg = MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_SYS_STATUS].ByteArrayToStructure<MAVLink.mavlink_sys_status_t>(6) };

                        message.META_LINKQUALITY = message.SYS_STATUS = new Message2() { index = packetindex, msg = new META_LINKQUALITY() { master_in = (int)MainV2.comPort.packetsnotlost, mavpackettype = "META_LINKQUALITY", master_out = MainV2.comPort.packetcount, packet_loss = 100 - MainV2.comPort.MAV.cs.linkqualitygcs } };

                        packetindex++;

                        string output = serializer.Serialize(message);

                        string header = "HTTP/1.1 200 OK\r\nContent-Type: application/json\r\nContent-Length: " + output.Length + "\r\n\r\n";
                        byte[] temp = asciiEncoding.GetBytes(header);
                        stream.Write(temp, 0, temp.Length);

                        temp = asciiEncoding.GetBytes(output);
                        stream.Write(temp, 0, temp.Length);

                        goto again;

                        //stream.Close();
                    }
                    else if (url.ToLower().Contains("/mav/"))
                    {
                        //C:\Users\hog\Desktop\DIYDrones\mavelous\modules\lib\mavelous_web

                        Regex rex = new Regex(@"([^\s]+)\s(.+)\sHTTP/1", RegexOptions.IgnoreCase);

                        Match match = rex.Match(url);

                        if (match.Success)
                        {
                            string fileurl = match.Groups[2].Value;

                            fileurl = fileurl.Replace("/mav/", "");

                            if (fileurl == "" || fileurl == "/")
                                fileurl = "index.html";

                            string header = "HTTP/1.1 200 OK\r\n";
                            if (fileurl.Contains(".html"))
                                header += "Content-Type: text/html\r\n\r\n";
                            else if (fileurl.Contains(".js"))
                                header += "Content-Type: application/x-javascript\r\n\r\n";
                            else if (fileurl.Contains(".css"))
                                header += "Content-Type: text/css\r\n\r\n";
                            else
                                header += "Content-Type: text/plain\r\n\r\n";
                            byte[] temp = asciiEncoding.GetBytes(header);
                            stream.Write(temp, 0, temp.Length);

                            BinaryReader file = new BinaryReader(File.Open(mavelous_web + fileurl, FileMode.Open, FileAccess.Read, FileShare.Read));
                            byte[] buffer = new byte[1024];
                            while (file.BaseStream.Position < file.BaseStream.Length)
                            {

                                int leng = file.Read(buffer, 0, buffer.Length);

                                stream.Write(buffer, 0, leng);
                            }
                            file.Close();
                            stream.Close();
                        }
                        else
                        {
                            string header = "HTTP/1.1 404 not found\r\nContent-Type: image/jpg\r\n\r\n";
                            byte[] temp = asciiEncoding.GetBytes(header);
                            stream.Write(temp, 0, temp.Length);

                            stream.Close();
                        }

                    }
                    else
                    {
                        Console.WriteLine(url);
                        string header = "HTTP/1.1 200 OK\r\nContent-Type: text/html\r\n\r\n";
                        byte[] temp = asciiEncoding.GetBytes(header);
                        stream.Write(temp, 0, temp.Length);

                        string content = @"
                <a href=/mav/>Mavelous</a>
            <a href=/mavlink/>Mavelous traffic</a>
            <a href=/hud.jpg>Hud image</a>
            <a href=/map.jpg>Map image </a>
            <a href=/both.jpg>Map & hud image</a>
            <a href=/hud.html>hud html5</a>
            <a href=/network.kml>network kml</a>

            ";
                        temp = asciiEncoding.GetBytes(content);
                        stream.Write(temp, 0, temp.Length);

                        stream.Close();
                    }

                    stream.Close();
                }
                catch (Exception ee)
                {
                    log.Error("Failed mjpg ", ee);
                    try
                    {
                        client.Close();
                    }
                    catch { }
                }
            }
        }
Esempio n. 7
0
        void mainloop()
        {
            DateTime nextsend = DateTime.Now;

            StreamWriter sw = new StreamWriter(File.OpenWrite("followmeraw.txt"));

            threadrun = true;
            while (threadrun)
            {
                try
                {
                    string line = comPort.ReadLine();

                    sw.WriteLine(line);

                    //string line = string.Format("$GP{0},{1:HHmmss},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},", "GGA", DateTime.Now.ToUniversalTime(), Math.Abs(lat * 100), MainV2.comPort.MAV.cs.lat < 0 ? "S" : "N", Math.Abs(lng * 100), MainV2.comPort.MAV.cs.lng < 0 ? "W" : "E", MainV2.comPort.MAV.cs.gpsstatus, MainV2.comPort.MAV.cs.satcount, MainV2.comPort.MAV.cs.gpshdop, MainV2.comPort.MAV.cs.alt, "M", 0, "M", "");
                    if (line.StartsWith("$GPGGA")) //
                    {
                        string[] items = line.Trim().Split(',','*');

                        if (items[15] != GetChecksum(line.Trim()))
                        {
                            Console.WriteLine("Bad Nmea line " + items[15] + " vs " + GetChecksum(line.Trim()));
                            continue;
                        }

                        if (items[6] == "0")
                        {
                            Console.WriteLine("No Fix");
                            continue;
                        }

                        gotolocation.Lat = double.Parse(items[2], CultureInfo.InvariantCulture) / 100.0;

                        gotolocation.Lat = (int)gotolocation.Lat + ((gotolocation.Lat - (int)gotolocation.Lat) / 0.60);

                        if (items[3] == "S")
                            gotolocation.Lat *= -1;

                        gotolocation.Lng = double.Parse(items[4], CultureInfo.InvariantCulture) / 100.0;

                        gotolocation.Lng = (int)gotolocation.Lng + ((gotolocation.Lng - (int)gotolocation.Lng) / 0.60);

                        if (items[5] == "W")
                            gotolocation.Lng *= -1;

                        gotolocation.Alt = intalt; // double.Parse(line.Substring(c9, c10 - c9 - 1)) +

                        gotolocation.Tag = "Sats "+ items[7] + " hdop " + items[8] ;

                    }

                    if (DateTime.Now > nextsend && gotolocation.Lat != 0 && gotolocation.Lng != 0 && gotolocation.Alt != 0) // 200 * 10 = 2 sec /// lastgotolocation != gotolocation &&
                    {
                        nextsend = DateTime.Now.AddMilliseconds(1000/updaterate);
                        Console.WriteLine("Sending follow wp " +DateTime.Now.ToString("h:MM:ss")+" "+ gotolocation.Lat + " " + gotolocation.Lng + " " +gotolocation.Alt);
                        lastgotolocation = new PointLatLngAlt(gotolocation);

                        Locationwp gotohere = new Locationwp();

                        gotohere.id = (byte)MAVLink.MAV_CMD.WAYPOINT;
                        gotohere.alt = (float)(gotolocation.Alt);
                        gotohere.lat = (gotolocation.Lat);
                        gotohere.lng = (gotolocation.Lng);

                        try
                        {
                            updateLocationLabel(gotohere);
                        }
                        catch { }

                        if (MainV2.comPort.BaseStream.IsOpen && MainV2.comPort.giveComport == false)
                        {
                            try
                            {
                                MainV2.comPort.giveComport = true;

                                MainV2.comPort.setGuidedModeWP(gotohere);

                                MainV2.comPort.giveComport = false;
                            }
                            catch { MainV2.comPort.giveComport = false; }
                        }
                    }
                }
                catch { System.Threading.Thread.Sleep((int)(1000 / updaterate)); }
            }

            sw.Close();
        }
Esempio n. 8
0
 private void updateLocationLabel(Locationwp plla)
 {
     this.BeginInvoke((MethodInvoker)delegate
      {
          LBL_location.Text = gotolocation.Lat + " " + gotolocation.Lng + " " + gotolocation.Alt +" "+ gotolocation.Tag;
     }
     );
 }
Esempio n. 9
0
        /// <summary>
        /// Writes the mission from the datagrid and values to the EEPROM
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void BUT_write_Click(object sender, EventArgs e)
        {
            if (CHK_altmode.Checked)
            {
                if (DialogResult.No == MessageBox.Show("Absolute Alt is ticked are you sure?", "Alt Mode", MessageBoxButtons.YesNo))
                {
                    CHK_altmode.Checked = false;
                }
            }

            // check for invalid grid data
            for (int a = 0; a < Commands.Rows.Count - 0; a++)
            {
                for (int b = 0; b < Commands.ColumnCount - 0; b++)
                {
                    double answer;
                    if (b >= 1 && b <= 4)
                    {
                        if (!double.TryParse(Commands[b, a].Value.ToString(), out answer))
                        {
                            MessageBox.Show("There are errors in your mission");
                            return;
                        }
                    }
                }
            }

            System.Threading.Thread t12 = new System.Threading.Thread(delegate()
            {
                try
                {
                    MAVLink port = MainV2.comPort;

                    if (!port.BaseStream.IsOpen)
                    {
                        throw new Exception("Please Connect First!");
                    }

                    MainV2.givecomport = true;

                    Locationwp home = new Locationwp();

                    try
                    {
                        home.id = (byte)MAVLink.MAV_CMD.WAYPOINT;
                        home.lat = (float.Parse(TXT_homelat.Text));
                        home.lng = (float.Parse(TXT_homelng.Text));
                        home.alt = (float.Parse(TXT_homealt.Text) / MainV2.cs.multiplierdist); // use saved home
                    }
                    catch { throw new Exception("Your home location is invalid"); }

                    port.setWPTotal((ushort)(Commands.Rows.Count + 1)); // + home

                    port.setWP(home, (ushort)0, MAVLink.MAV_FRAME.MAV_FRAME_GLOBAL, 0);

                    MAVLink.MAV_FRAME frame = MAVLink.MAV_FRAME.MAV_FRAME_GLOBAL_RELATIVE_ALT;

                    // process grid to memory eeprom
                    for (int a = 0; a < Commands.Rows.Count - 0; a++)
                    {
                        Locationwp temp = new Locationwp();
                        temp.id = (byte)(int)Enum.Parse(typeof(MAVLink.MAV_CMD), Commands.Rows[a].Cells[Command.Index].Value.ToString(), false);
                        temp.p1 = float.Parse(Commands.Rows[a].Cells[Param1.Index].Value.ToString());
                        if (temp.id < (byte)MAVLink.MAV_CMD.LAST || temp.id == (byte)MAVLink.MAV_CMD.DO_SET_HOME)
                        {
                            if (CHK_altmode.Checked)
                            {
                                frame = MAVLink.MAV_FRAME.MAV_FRAME_GLOBAL;
                            }
                            else
                            {
                                frame = MAVLink.MAV_FRAME.MAV_FRAME_GLOBAL_RELATIVE_ALT;
                            }
                        }

                        temp.alt = (float)(double.Parse(Commands.Rows[a].Cells[Alt.Index].Value.ToString()) / MainV2.cs.multiplierdist);
                        temp.lat = (float)(double.Parse(Commands.Rows[a].Cells[Lat.Index].Value.ToString()));
                        temp.lng = (float)(double.Parse(Commands.Rows[a].Cells[Lon.Index].Value.ToString()));

                        temp.p2 = (float)(double.Parse(Commands.Rows[a].Cells[Param2.Index].Value.ToString()));
                        temp.p3 = (float)(double.Parse(Commands.Rows[a].Cells[Param3.Index].Value.ToString()));
                        temp.p4 = (float)(double.Parse(Commands.Rows[a].Cells[Param4.Index].Value.ToString()));

                        port.setWP(temp, (ushort)(a + 1), frame, 0);
                    }

                    port.setWPACK();

                    if (CHK_holdalt.Checked)
                    {
                        port.setParam("ALT_HOLD_RTL", int.Parse(TXT_DefaultAlt.Text) / MainV2.cs.multiplierdist);
                    }
                    else
                    {
                        port.setParam("ALT_HOLD_RTL", -1);
                    }

                    port.setParam("WP_RADIUS", (byte)int.Parse(TXT_WPRad.Text) / MainV2.cs.multiplierdist);

                    try
                    {
                        port.setParam("WP_LOITER_RAD", (byte)(int.Parse(TXT_loiterrad.Text) / MainV2.cs.multiplierdist));
                    }
                    catch
                    {
                        port.setParam("LOITER_RAD", (byte)int.Parse(TXT_loiterrad.Text) / MainV2.cs.multiplierdist);
                    }

                }
                catch (Exception ex) { MessageBox.Show("Error : " + ex.ToString()); }

                MainV2.givecomport = false;

                try
                {
                    this.BeginInvoke((System.Threading.ThreadStart)delegate()
                    {
                        BUT_write.Enabled = true;
                    });
                }
                catch { }

            });
            t12.IsBackground = true;
            t12.Name = "Write wps";
            t12.Start();
            MainV2.threads.Add(t12);

            MainMap.Focus();

            BUT_write.Enabled = false;
        }
Esempio n. 10
0
        void mainloop()
        {
            DateTime nextsend = DateTime.Now;

            StreamWriter sw = new StreamWriter(File.OpenWrite(Settings.GetUserDataDirectory() + "followmeraw.txt"));

            threadrun = true;
            while (threadrun)
            {
                try
                {
                    string line = comPort.ReadLine();

                    sw.WriteLine(line);

                    //string line = string.Format("$GP{0},{1:HHmmss},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},", "GGA", DateTime.Now.ToUniversalTime(), Math.Abs(lat * 100), MainV2.comPort.MAV.cs.lat < 0 ? "S" : "N", Math.Abs(lng * 100), MainV2.comPort.MAV.cs.lng < 0 ? "W" : "E", MainV2.comPort.MAV.cs.gpsstatus, MainV2.comPort.MAV.cs.satcount, MainV2.comPort.MAV.cs.gpshdop, MainV2.comPort.MAV.cs.alt, "M", 0, "M", "");
                    if (line.StartsWith("$GPGGA") || line.StartsWith("$GNGGA")) //
                    {
                        string[] items = line.Trim().Split(',', '*');

                        if (items[items.Length - 1] != GetChecksum(line.Trim()))
                        {
                            Console.WriteLine("Bad Nmea line " + items[15] + " vs " + GetChecksum(line.Trim()));
                            continue;
                        }

                        if (items[6] == "0")
                        {
                            Console.WriteLine("No Fix");
                            continue;
                        }

                        gotolocation.Lat = double.Parse(items[2], CultureInfo.InvariantCulture) / 100.0;

                        gotolocation.Lat = (int)gotolocation.Lat + ((gotolocation.Lat - (int)gotolocation.Lat) / 0.60);

                        if (items[3] == "S")
                        {
                            gotolocation.Lat *= -1;
                        }

                        gotolocation.Lng = double.Parse(items[4], CultureInfo.InvariantCulture) / 100.0;

                        gotolocation.Lng = (int)gotolocation.Lng + ((gotolocation.Lng - (int)gotolocation.Lng) / 0.60);

                        if (items[5] == "W")
                        {
                            gotolocation.Lng *= -1;
                        }

                        gotolocation.Alt = intalt;

                        gotolocation.Tag = "Sats " + items[7] + " hdop " + items[8];
                    }


                    if (DateTime.Now > nextsend && gotolocation.Lat != 0 && gotolocation.Lng != 0 &&
                        gotolocation.Alt != 0) // 200 * 10 = 2 sec /// lastgotolocation != gotolocation &&
                    {
                        nextsend = DateTime.Now.AddMilliseconds(1000 / updaterate);
                        Console.WriteLine("Sending follow wp " + DateTime.Now.ToString("h:MM:ss") + " " +
                                          gotolocation.Lat + " " + gotolocation.Lng + " " + gotolocation.Alt);
                        lastgotolocation = new PointLatLngAlt(gotolocation);

                        Locationwp gotohere = new Locationwp();

                        gotohere.id  = (ushort)MAVLink.MAV_CMD.WAYPOINT;
                        gotohere.alt = (float)(gotolocation.Alt);
                        gotohere.lat = (gotolocation.Lat);
                        gotohere.lng = (gotolocation.Lng);

                        try
                        {
                            updateLocationLabel(gotohere);
                        }
                        catch
                        {
                        }

                        if (MainV2.comPort.BaseStream.IsOpen && MainV2.comPort.giveComport == false)
                        {
                            try
                            {
                                MainV2.comPort.setGuidedModeWP(gotohere, false);
                            }
                            catch
                            {
                            }
                        }
                    }
                }
                catch
                {
                    System.Threading.Thread.Sleep((int)(1000 / updaterate));
                }
            }

            sw.Close();
        }
Esempio n. 11
0
        /// <summary>
        /// Save wp to eeprom
        /// </summary>
        /// <param name="loc">location struct</param>
        /// <param name="index">wp no</param>
        /// <param name="frame">global or relative</param>
        /// <param name="current">0 = no , 2 = guided mode</param>
        public MAV_MISSION_RESULT setWP(Locationwp loc, ushort index, MAV_FRAME frame, byte current = 0, byte autocontinue = 1)
        {
            giveComport = true;
            mavlink_mission_item_t req = new mavlink_mission_item_t();

            req.target_system = sysid;
            req.target_component = compid; // MAVLINK_MSG_ID_MISSION_ITEM

            req.command = loc.id;

            req.current = current;
            req.autocontinue = autocontinue;

            req.frame = (byte)frame;
            req.y = (float)(loc.lng);
            req.x = (float)(loc.lat);
            req.z = (float)(loc.alt);

            req.param1 = loc.p1;
            req.param2 = loc.p2;
            req.param3 = loc.p3;
            req.param4 = loc.p4;

            req.seq = index;

            log.InfoFormat("setWP {6} frame {0} cmd {1} p1 {2} x {3} y {4} z {5}", req.frame, req.command, req.param1, req.x, req.y, req.z, index);

            // request
            generatePacket(MAVLINK_MSG_ID_MISSION_ITEM, req);

            DateTime start = DateTime.Now;
            int retrys = 10;

            while (true)
            {
                if (!(start.AddMilliseconds(150) > DateTime.Now))
                {
                    if (retrys > 0)
                    {
                        log.Info("setWP Retry " + retrys);
                        generatePacket(MAVLINK_MSG_ID_MISSION_ITEM, req);

                        start = DateTime.Now;
                        retrys--;
                        continue;
                    }
                    giveComport = false;
                    throw new Exception("Timeout on read - setWP");
                }
                byte[] buffer = readPacket();
                if (buffer.Length > 5)
                {
                    if (buffer[5] == MAVLINK_MSG_ID_MISSION_ACK)
                    {
                        var ans = buffer.ByteArrayToStructure<mavlink_mission_ack_t>(6);
                        log.Info("set wp " + index + " ACK 47 : " + buffer[5] + " ans " + Enum.Parse(typeof(MAV_MISSION_RESULT), ans.type.ToString()));

                        if (req.current == 2)
                        {
                            GuidedMode = req;
                        }
                        else if (req.current == 3)
                        {

                        }
                        else
                        {
                            wps[req.seq] = req;
                        }

                        return (MAV_MISSION_RESULT)ans.type;
                    }
                    else if (buffer[5] == MAVLINK_MSG_ID_MISSION_REQUEST)
                    {
                        var ans = buffer.ByteArrayToStructure<mavlink_mission_request_t>(6);
                        if (ans.seq == (index + 1))
                        {
                            log.Info("set wp doing " + index + " req " + ans.seq + " REQ 40 : " + buffer[5]);
                            giveComport = false;

                            if (req.current == 2)
                            {
                                GuidedMode = req;
                            }
                            else if (req.current == 3)
                            {

                            }
                            else
                            {
                                wps[req.seq] = req;
                            }

                            return MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED;
                        }
                        else
                        {
                            log.InfoFormat("set wp fail doing " + index + " req " + ans.seq + " ACK 47 or REQ 40 : " + buffer[5] + " seq {0} ts {1} tc {2}", req.seq, req.target_system, req.target_component);
                            //break;
                        }
                    }
                    else
                    {
                        //Console.WriteLine(DateTime.Now + " PC setwp " + buffer[5]);
                    }
                }
            }

            // return MAV_MISSION_RESULT.MAV_MISSION_INVALID;
        }
Esempio n. 12
0
        /// <summary>
        /// used to write a KML, update the Map view polygon, and update the row headers
        /// </summary>
        private void writeKML()
        {
            if (quickadd)
                return;

            pointlist = new List<PointLatLngAlt>();

            System.Diagnostics.Debug.WriteLine(DateTime.Now);
            try
            {
                if (objects != null) // hasnt been created yet
                {
                    objects.Markers.Clear();
                }

                string home;
                if (TXT_homealt.Text != "" && TXT_homelat.Text != "" && TXT_homelng.Text != "")
                {
                    home = string.Format("{0},{1},{2}\r\n", TXT_homelng.Text, TXT_homelat.Text, TXT_DefaultAlt.Text);
                    if (objects != null) // during startup
                    {
                        pointlist.Add(new PointLatLngAlt(double.Parse(TXT_homelat.Text), double.Parse(TXT_homelng.Text), (int)double.Parse(TXT_homealt.Text), "Home"));
                        addpolygonmarker("Home", double.Parse(TXT_homelng.Text), double.Parse(TXT_homelat.Text), 0);
                    }
                }
                else
                {
                    home = "";
                }

                double avglat = 0;
                double avglong = 0;
                double maxlat = -180;
                double maxlong = -180;
                double minlat = 180;
                double minlong = 180;
                double homealt = 0;
                try
                {
                    homealt = (int)double.Parse(TXT_homealt.Text);
                }
                catch { }
                if (CHK_altmode.Checked)
                {
                    homealt = 0; // for absolute we dont need to add homealt
                }

                int usable = 0;

                System.Threading.Thread t1 = new System.Threading.Thread(delegate()
                {
                    // thread for updateing row numbers
                    for (int a = 0; a < Commands.Rows.Count - 0; a++)
                    {
                        try
                        {
                            if (Commands.Rows[a].HeaderCell.Value == null)
                            {
                                    Commands.Rows[a].HeaderCell.Style.Alignment = DataGridViewContentAlignment.MiddleCenter;
                                    Commands.Rows[a].HeaderCell.Value = (a + 1).ToString();
                            }
                            // skip rows with the correct number
                            string rowno = Commands.Rows[a].HeaderCell.Value.ToString();
                            if (!rowno.Equals((a + 1).ToString()))
                            {
                                // this code is where the delay is when deleting.
                                Commands.Rows[a].HeaderCell.Value = (a + 1).ToString();
                            }
                        }
                        catch (Exception) { }
                    }
                });
                t1.Name = "Row number updater";
                t1.IsBackground = true;
                t1.Start();
                MainV2.threads.Add(t1);

                long temp = System.Diagnostics.Stopwatch.GetTimestamp();

                string lookat = "";
                for (int a = 0; a < Commands.Rows.Count - 0; a++)
                {
                    try
                    {
                        int command = (byte)(int)Enum.Parse(typeof(MAVLink.MAV_CMD), Commands.Rows[a].Cells[Command.Index].Value.ToString(), false);
                        if (command < (byte)MAVLink.MAV_CMD.LAST && command != (byte)MAVLink.MAV_CMD.TAKEOFF)
                        {
                            string cell2 = Commands.Rows[a].Cells[Alt.Index].Value.ToString(); // alt
                            string cell3 = Commands.Rows[a].Cells[Lat.Index].Value.ToString(); // lat
                            string cell4 = Commands.Rows[a].Cells[Lon.Index].Value.ToString(); // lng

                            if (cell4 == "0" || cell3 == "0")
                                continue;
                            if (cell4 == "?" || cell3 == "?")
                                continue;

                            pointlist.Add(new PointLatLngAlt(double.Parse(cell3), double.Parse(cell4), (int)double.Parse(cell2) + homealt, (a + 1).ToString()));
                            addpolygonmarker((a + 1).ToString(), double.Parse(cell4), double.Parse(cell3), (int)double.Parse(cell2));

                            avglong += double.Parse(Commands.Rows[a].Cells[Lon.Index].Value.ToString());
                            avglat += double.Parse(Commands.Rows[a].Cells[Lat.Index].Value.ToString());
                            usable++;

                            maxlong = Math.Max(double.Parse(Commands.Rows[a].Cells[Lon.Index].Value.ToString()), maxlong);
                            maxlat = Math.Max(double.Parse(Commands.Rows[a].Cells[Lat.Index].Value.ToString()), maxlat);
                            minlong = Math.Min(double.Parse(Commands.Rows[a].Cells[Lon.Index].Value.ToString()), minlong);
                            minlat = Math.Min(double.Parse(Commands.Rows[a].Cells[Lat.Index].Value.ToString()), minlat);

                            System.Diagnostics.Debug.WriteLine(temp - System.Diagnostics.Stopwatch.GetTimestamp());
                        }
                    }
                    catch (Exception e) { Console.WriteLine("writekml - bad wp data " + e.ToString()); }
                }

                if (usable > 0)
                {
                    avglat = avglat / usable;
                    avglong = avglong / usable;
                    double latdiff = maxlat - minlat;
                    double longdiff = maxlong - minlong;
                    float range = 4000;

                    Locationwp loc1 = new Locationwp();
                    loc1.lat = (float)(minlat);
                    loc1.lng = (float)(minlong);
                    Locationwp loc2 = new Locationwp();
                    loc2.lat = (float)(maxlat);
                    loc2.lng = (float)(maxlong);

                    //double distance = getDistance(loc1, loc2);  // same code as ardupilot
                    double distance = 2000;

                    if (usable > 1)
                    {
                        range = (float)(distance * 2);
                    }
                    else
                    {
                        range = 4000;
                    }

                    if (avglong != 0 && usable < 3)
                    {
                        // no autozoom
                        lookat = "<LookAt>     <longitude>" + (minlong + longdiff / 2).ToString(new System.Globalization.CultureInfo("en-US")) + "</longitude>     <latitude>" + (minlat + latdiff / 2).ToString(new System.Globalization.CultureInfo("en-US")) + "</latitude> <range>" + range + "</range> </LookAt>";
                        //MainMap.ZoomAndCenterMarkers("objects");
                        //MainMap.Zoom -= 1;
                        //MainMap_OnMapZoomChanged();
                    }
                }
                else if (home.Length > 5 && usable == 0)
                {
                    lookat = "<LookAt>     <longitude>" + TXT_homelng.Text.ToString(new System.Globalization.CultureInfo("en-US")) + "</longitude>     <latitude>" + TXT_homelat.Text.ToString(new System.Globalization.CultureInfo("en-US")) + "</latitude> <range>4000</range> </LookAt>";
                    MainMap.HoldInvalidation = true;
                    MainMap.ZoomAndCenterMarkers("objects");
                    MainMap.Zoom -= 2;
                    MainMap_OnMapZoomChanged();
                    MainMap.HoldInvalidation = false;
                }

                RegeneratePolygon();

                if (polygon != null && polygon.Points.Count > 0)
                {
                    double homedist = 0;

                    if (home.Length > 5)
                    {
                        pointlist.Add(new PointLatLngAlt(double.Parse(TXT_homelat.Text), double.Parse(TXT_homelng.Text), (int)double.Parse(TXT_homealt.Text), "Home"));

                        homedist = MainMap.Manager.GetDistance(polygon.Points[polygon.Points.Count - 1], polygon.Points[0]);

                        lbl_homedist.Text = rm.GetString("lbl_homedist.Text") + ": " + FormatDistance(homedist, true);
                    }
                    lbl_distance.Text = rm.GetString("lbl_distance.Text") + ": " + FormatDistance(polygon.Distance + homedist, false);
                }

                config(true);
            }
            catch (Exception ex)
            {
                Console.WriteLine(ex.ToString());
            }

            System.Diagnostics.Debug.WriteLine(DateTime.Now);
        }
        void mainloop()
        {
            DateTime nextsend = DateTime.Now;

            DateTime nextrallypntupdate = DateTime.Now;

            StreamWriter sw = new StreamWriter(Settings.GetUserDataDirectory() + "MovingBase.txt");

            threadrun = true;
            while (threadrun)
            {
                try
                {
                    string line = comPort.ReadLine();

                    sw.WriteLine(line);

                    //string line = string.Format("$GP{0},{1:HHmmss},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},", "GGA", DateTime.Now.ToUniversalTime(), Math.Abs(lat * 100), MainV2.comPort.MAV.cs.lat < 0 ? "S" : "N", Math.Abs(lng * 100), MainV2.comPort.MAV.cs.lng < 0 ? "W" : "E", MainV2.comPort.MAV.cs.gpsstatus, MainV2.comPort.MAV.cs.satcount, MainV2.comPort.MAV.cs.gpshdop, MainV2.comPort.MAV.cs.alt, "M", 0, "M", "");
                    if (line.StartsWith("$GPGGA") || line.StartsWith("$GNGGA")) //
                    {
                        string[] items = line.Trim().Split(',', '*');

                        if (items[items.Length - 1] != GetChecksum(line.Trim()))
                        {
                            Console.WriteLine("Bad Nmea line " + items[15] + " vs " + GetChecksum(line.Trim()));
                            continue;
                        }

                        if (items[6] == "0")
                        {
                            Console.WriteLine("No Fix");
                            continue;
                        }

                        gotolocation.Lat = double.Parse(items[2], CultureInfo.InvariantCulture) / 100.0;

                        gotolocation.Lat = (int)gotolocation.Lat + ((gotolocation.Lat - (int)gotolocation.Lat) / 0.60);

                        if (items[3] == "S")
                        {
                            gotolocation.Lat *= -1;
                        }

                        gotolocation.Lng = double.Parse(items[4], CultureInfo.InvariantCulture) / 100.0;

                        gotolocation.Lng = (int)gotolocation.Lng + ((gotolocation.Lng - (int)gotolocation.Lng) / 0.60);

                        if (items[5] == "W")
                        {
                            gotolocation.Lng *= -1;
                        }

                        gotolocation.Alt = double.Parse(items[9], CultureInfo.InvariantCulture);

                        gotolocation.Tag = "Sats " + items[7] + " hdop " + items[8];
                    }


                    if (DateTime.Now > nextsend && gotolocation.Lat != 0 && gotolocation.Lng != 0) // 200 * 10 = 2 sec /// lastgotolocation != gotolocation &&
                    {
                        nextsend = DateTime.Now.AddMilliseconds(1000 / updaterate);
                        Console.WriteLine("new home wp " + DateTime.Now.ToString("h:MM:ss") + " " + gotolocation.Lat +
                                          " " + gotolocation.Lng + " " + gotolocation.Alt);
                        lastgotolocation = new PointLatLngAlt(gotolocation);

                        Locationwp gotohere = new Locationwp();

                        gotohere.id  = (ushort)MAVLink.MAV_CMD.WAYPOINT;
                        gotohere.alt = (float)(gotolocation.Alt);
                        gotohere.lat = (gotolocation.Lat);
                        gotohere.lng = (gotolocation.Lng);

                        if (chk_relalt.Checked)
                        {
                            gotohere.alt = gotohere.alt - (float)MainV2.comPort.MAV.cs.HomeAlt;
                        }

                        try
                        {
                            updateLocationLabel(gotohere);
                        }
                        catch
                        {
                        }

                        MainV2.comPort.MAV.cs.MovingBase = gotolocation;

                        // plane only
                        if (updaterallypnt && DateTime.Now > nextrallypntupdate)
                        {
                            nextrallypntupdate = DateTime.Now.AddSeconds(5);
                            try
                            {
                                MainV2.comPort.setParam((byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent, "RALLY_TOTAL", 1);

                                MainV2.comPort.setRallyPoint(0,
                                                             new PointLatLngAlt(gotolocation)
                                {
                                    Alt =
                                        gotolocation.Alt + double.Parse(Settings.Instance["TXT_DefaultAlt"].ToString())
                                },
                                                             0, 0, 0, (byte)(float)MainV2.comPort.MAV.param["RALLY_TOTAL"]);

                                MainV2.comPort.setParam((byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent, "RALLY_TOTAL", 1);
                            }
                            catch (Exception ex)
                            {
                                Console.WriteLine(ex.ToString());
                            }
                        }
                    }
                }
                catch
                {
                    System.Threading.Thread.Sleep((int)(1000 / updaterate));
                }
            }

            sw.Close();
        }
Esempio n. 14
0
        void readQGC110wpfile(string file)
        {
            int wp_count = 0;
            bool error = false;
            List<Locationwp> cmds = new List<Locationwp>();

            try
            {
                StreamReader sr = new StreamReader(file); //"defines.h"
                string header = sr.ReadLine();
                if (header == null || !header.Contains("QGC WPL 110"))
                {
                    MessageBox.Show("Invalid Waypoint file");
                    return;
                }
                while (!error && !sr.EndOfStream)
                {
                    string line = sr.ReadLine();
                    // waypoints

                    if (line.StartsWith("#"))
                        continue;

                    string[] items = line.Split(new char[] { (char)'\t', ' ' }, StringSplitOptions.RemoveEmptyEntries);

                    if (items.Length <= 9)
                        continue;

                    try
                    {

                        Locationwp temp = new Locationwp();
                        if (items[2] == "3")
                        { // abs MAV_FRAME_GLOBAL_RELATIVE_ALT=3
                            temp.options = 1;
                        }
                        else
                        {
                            temp.options = 0;
                        }
                        temp.id = (byte)(int)Enum.Parse(typeof(MAVLink.MAV_CMD), items[3], false);
                        temp.p1 = float.Parse(items[4], new System.Globalization.CultureInfo("en-US"));

                        if (temp.id == 99)
                            temp.id = 0;

                        temp.alt = (float)(double.Parse(items[10], new System.Globalization.CultureInfo("en-US")));
                        temp.lat = (float)(double.Parse(items[8], new System.Globalization.CultureInfo("en-US")));
                        temp.lng = (float)(double.Parse(items[9], new System.Globalization.CultureInfo("en-US")));

                        temp.p2 = (float)(double.Parse(items[5], new System.Globalization.CultureInfo("en-US")));
                        temp.p3 = (float)(double.Parse(items[6], new System.Globalization.CultureInfo("en-US")));
                        temp.p4 = (float)(double.Parse(items[7], new System.Globalization.CultureInfo("en-US")));

                        cmds.Add(temp);

                        wp_count++;

                    }
                    catch { MessageBox.Show("Line invalid\n" + line); }

                    if (wp_count == byte.MaxValue)
                    {
                        MessageBox.Show("To many Waypoints!!!");
                        break;
                    }

                }

                sr.Close();

                processToScreen(cmds);

                writeKML();

                MainMap.ZoomAndCenterMarkers("objects");
            }
            catch (Exception ex)
            {
                MessageBox.Show("Can't open file! " + ex.ToString());
            }
        }
Esempio n. 15
0
        public List <PointLatLngAlt> drawPoints = new List <PointLatLngAlt>();  //点集
        public void getWPs()
        {
            bool succeed           = false;
            List <Locationwp> cmds = new List <Locationwp>();



            UpLoadForm up = new UpLoadForm();

            up.SetMessage(4, 0);
            up.Show();
            int cmdcount = getWPCount();

            up.SetMessage(5, cmdcount);



            Program.mav_msg_handler.mission_items.Clear();
            ushort a = 0;

            for (a = 0; a < cmdcount; a++)
            {
                up.SetCount1(a, cmdcount);
                Locationwp wp = getWP(a, ref succeed);

                if (succeed)
                {
                    cmds.Add(wp);
                }
                else
                {
                    break;
                }

                if (up.isCancel)
                {
                    ListMessage("已取消下载航点...");
                    up.Close();
                    return;
                }


                Application.DoEvents();
            }

            if (succeed)
            {
                up.SetMessage(7, 0);
                drawPoints.Clear();
                foreach (Locationwp loc in cmds)
                {
                    PointLatLngAlt PL = new PointLatLngAlt();
                    PL.Lat = loc.lat;
                    PL.Lng = loc.lng;
                    PL.Alt = loc.alt;
                    drawPoints.Add(PL);
                }
            }
            else
            {
                up.SetMessage(8, 0);
            }
            Thread.Sleep(500);
            up.Close();
        }
Esempio n. 16
0
        private void goHereToolStripMenuItem_Click(object sender, EventArgs e)
        {
            if (!MainV2.comPort.BaseStream.IsOpen)
            {
                CustomMessageBox.Show("Please Connect First");
                return;
            }

            if (MainV2.comPort.MAV.GuidedMode.z == 0)
            {
                flyToHereAltToolStripMenuItem_Click(null, null);

                if (MainV2.comPort.MAV.GuidedMode.z == 0)
                    return;
            }

            if (gotolocation.Lat == 0 || gotolocation.Lng == 0)
            {
                CustomMessageBox.Show("Bad Lat/Long");
                return;
            }

            Locationwp gotohere = new Locationwp();

            gotohere.id = (byte)MAVLink.MAV_CMD.WAYPOINT;
            gotohere.alt = (float)(MainV2.comPort.MAV.GuidedMode.z); // back to m
            gotohere.lat = (gotolocation.Lat);
            gotohere.lng = (gotolocation.Lng);

            try
            {
                MainV2.comPort.setGuidedModeWP(gotohere);
            }
            catch (Exception ex) { MainV2.comPort.giveComport = false; CustomMessageBox.Show("Error sending command : " + ex.Message); }
        }
Esempio n. 17
0
        private Locationwp getWP(ushort index, ref bool succeed)
        {
            Locationwp loc = new Locationwp();

            MAVLink.mavlink_mission_request_t req = new MAVLink.mavlink_mission_request_t();
            req.target_system    = TARGET_SYSTEM_ID;
            req.target_component = TARGET_SYS_COMPID;
            req.seq = index;
            MAVLink.mavlink_mission_item_t wp = new MAVLink.mavlink_mission_item_t();
            bool found_ret = false;


            for (int i = 0; i < 3; i++)
            {
                generatePacket((byte)MAVLink.MAVLINK_MSG_ID.MISSION_REQUEST, req);

                Program.mav_msg_handler.SetTimeout(5);
                while (found_ret == false)
                {
                    if (Program.mav_msg_handler.Wait())
                    {
                        this.ListMessage("获取航点坐标" + index.ToString() + "超时失败[" + (i + 1).ToString() + "]");
                        break;
                    }

                    Application.DoEvents();

                    if (Program.mav_msg_handler.mission_items.ContainsKey(req.seq))
                    {
                        found_ret = true;
                        wp        = Program.mav_msg_handler.mission_items[req.seq];
                        break;
                    }
                }


                if (found_ret == false)
                {
                    succeed = false;
                }
                else
                {
                    succeed     = true;
                    loc.options = (byte)(wp.frame);
                    loc.id      = (byte)(wp.command);
                    loc.p1      = (wp.param1);
                    loc.p2      = (wp.param2);
                    loc.p3      = (wp.param3);
                    loc.p4      = (wp.param4);

                    loc.alt = ((wp.z));
                    loc.lat = ((wp.x));
                    loc.lng = ((wp.y));

                    this.ListMessage("获取航点坐标" + index.ToString() + "成功...");
                    break;
                }
            }



            return(loc);
        }
Esempio n. 18
0
        public void setGuidedModeWP(Locationwp gotohere)
        {
            if (gotohere.alt == 0 || gotohere.lat == 0 || gotohere.lng == 0)
                return;

            giveComport = true;

            try
            {
                gotohere.id = (byte)MAV_CMD.WAYPOINT;

                MAV_MISSION_RESULT ans = setWP(gotohere, 0, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT, (byte)2);

                if (ans != MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED)
                    throw new Exception("Guided Mode Failed");
            }
            catch (Exception ex) { log.Error(ex); }

            giveComport = false;
        }
Esempio n. 19
0
        /// <summary>
        /// Gets specfied WP
        /// </summary>
        /// <param name="index"></param>
        /// <returns>WP</returns>
        public Locationwp getWP(ushort index)
        {
            giveComport = true;
            Locationwp loc = new Locationwp();
            mavlink_mission_request_t req = new mavlink_mission_request_t();

            req.target_system = sysid;
            req.target_component = compid;

            req.seq = index;

            //Console.WriteLine("getwp req "+ DateTime.Now.Millisecond);

            // request
            generatePacket(MAVLINK_MSG_ID_MISSION_REQUEST, req);

            DateTime start = DateTime.Now;
            int retrys = 5;

            while (true)
            {
                if (!(start.AddMilliseconds(800) > DateTime.Now)) // apm times out after 1000ms
                {
                    if (retrys > 0)
                    {
                        log.Info("getWP Retry " + retrys);
                        generatePacket(MAVLINK_MSG_ID_MISSION_REQUEST, req);
                        start = DateTime.Now;
                        retrys--;
                        continue;
                    }
                    giveComport = false;
                    throw new Exception("Timeout on read - getWP");
                }
                //Console.WriteLine("getwp read " + DateTime.Now.Millisecond);
                byte[] buffer = readPacket();
                //Console.WriteLine("getwp readend " + DateTime.Now.Millisecond);
                if (buffer.Length > 5)
                {
                    if (buffer[5] == MAVLINK_MSG_ID_MISSION_ITEM)
                    {
                        //Console.WriteLine("getwp ans " + DateTime.Now.Millisecond);

                        //Array.Copy(buffer, 6, buffer, 0, buffer.Length - 6);

                        var wp = buffer.ByteArrayToStructure<mavlink_mission_item_t>(6);

                        loc.options = (byte)(wp.frame & 0x1);
                        loc.id = (byte)(wp.command);
                        loc.p1 = (wp.param1);
                        loc.p2 = (wp.param2);
                        loc.p3 = (wp.param3);
                        loc.p4 = (wp.param4);

                        loc.alt = ((wp.z));
                        loc.lat = ((wp.x));
                        loc.lng = ((wp.y));
                        /*
                        if (MAV.cs.firmware == MainV2.Firmwares.ArduPlane)
                        {
                            switch (loc.id)
                            {					// Switch to map APM command fields inot MAVLink command fields
                                case (byte)MAV_CMD.LOITER_TURNS:
                                case (byte)MAV_CMD.TAKEOFF:
                                case (byte)MAV_CMD.DO_SET_HOME:
                                    //case (byte)MAV_CMD.DO_SET_ROI:
                                    loc.alt = (float)((wp.z));
                                    loc.lat = (float)((wp.x));
                                    loc.lng = (float)((wp.y));
                                    loc.p1 = (float)wp.param1;
                                    break;

                                case (byte)MAV_CMD.CONDITION_CHANGE_ALT:
                                    loc.lat = (int)wp.param1;
                                    loc.p1 = 0;
                                    break;

                                case (byte)MAV_CMD.LOITER_TIME:
                                    if (MainV2.APMFirmware == MainV2.Firmwares.ArduPlane)
                                    {
                                        loc.p1 = (byte)(wp.param1 / 10);	// APM loiter time is in ten second increments
                                    }
                                    else
                                    {
                                        loc.p1 = (byte)wp.param1;
                                    }
                                    break;

                                case (byte)MAV_CMD.CONDITION_DELAY:
                                case (byte)MAV_CMD.CONDITION_DISTANCE:
                                    loc.lat = (int)wp.param1;
                                    break;

                                case (byte)MAV_CMD.DO_JUMP:
                                    loc.lat = (int)wp.param2;
                                    loc.p1 = (byte)wp.param1;
                                    break;

                                case (byte)MAV_CMD.DO_REPEAT_SERVO:
                                    loc.lng = (int)wp.param4;
                                    goto case (byte)MAV_CMD.DO_CHANGE_SPEED;
                                case (byte)MAV_CMD.DO_REPEAT_RELAY:
                                case (byte)MAV_CMD.DO_CHANGE_SPEED:
                                    loc.lat = (int)wp.param3;
                                    loc.alt = (int)wp.param2;
                                    loc.p1 = (byte)wp.param1;
                                    break;

                                case (byte)MAV_CMD.DO_SET_PARAMETER:
                                case (byte)MAV_CMD.DO_SET_RELAY:
                                case (byte)MAV_CMD.DO_SET_SERVO:
                                    loc.alt = (int)wp.param2;
                                    loc.p1 = (byte)wp.param1;
                                    break;

                                case (byte)MAV_CMD.WAYPOINT:
                                    loc.p1 = (byte)wp.param1;
                                    break;
                            }
                        }
                        */
                        log.InfoFormat("getWP {0} {1} {2} {3} {4} opt {5}", loc.id, loc.p1, loc.alt, loc.lat, loc.lng, loc.options);

                        break;
                    }
                    else
                    {
                        log.Info(DateTime.Now + " PC getwp " + buffer[5]);
                    }
                }
            }
            giveComport = false;
            return loc;
        }
Esempio n. 20
0
 public PointLatLngAlt(Locationwp locwp)
 {
     this.Lat = locwp.lat;
     this.Lng = locwp.lng;
     this.Alt = locwp.alt;
 }
Esempio n. 21
0
        /// <summary>
        /// Read from waypoint writter *.h file
        /// </summary>
        /// <param name="file">File Path</param>
        /// <returns></returns>
        bool readwaypointwritterfile(string file)
        {
            byte wp_rad = 30;
            byte loit_rad = 45;
            int alt_hold = 100;
            byte wp_count = 0;
            bool error = false;
            List<Locationwp> cmds = new List<Locationwp>();

            cmds.Add(new Locationwp());

            try
            {
                StreamReader sr = new StreamReader(file); //"defines.h"
                while (!error && !sr.EndOfStream)
                {
                    string line = sr.ReadLine();
                    // defines
                    Regex regex2 = new Regex(@"define\s+([^\s]+)\s+([^\s]+)", RegexOptions.IgnoreCase);
                    if (regex2.IsMatch(line))
                    {
                        MatchCollection matchs = regex2.Matches(line);
                        for (int i = 0; i < matchs.Count; i++)
                        {
                            if (matchs[i].Groups[1].Value.ToString().Equals("WP_RADIUS"))
                                wp_rad = (byte)double.Parse(matchs[i].Groups[2].Value.ToString());
                            if (matchs[i].Groups[1].Value.ToString().Equals("LOITER_RADIUS"))
                                loit_rad = (byte)double.Parse(matchs[i].Groups[2].Value.ToString());
                            if (matchs[i].Groups[1].Value.ToString().Equals("ALT_TO_HOLD"))
                                alt_hold = (int)double.Parse(matchs[i].Groups[2].Value.ToString());
                        }
                    }
                    // waypoints
                    regex2 = new Regex(@"([^,{]+),([^,]+),([^,]+),([^,]+),([^,}]+)", RegexOptions.IgnoreCase);
                    if (regex2.IsMatch(line))
                    {
                        MatchCollection matchs = regex2.Matches(line);
                        for (int i = 0; i < matchs.Count; i++)
                        {
                            Locationwp temp = new Locationwp();
                            temp.options = 1;
                            temp.id = (byte)(int)Enum.Parse(typeof(MAVLink.MAV_CMD), matchs[i].Groups[1].Value.ToString().Replace("NAV_", ""), false);
                            temp.p1 = byte.Parse(matchs[i].Groups[2].Value.ToString());

                            if (temp.id < (byte)MAVLink.MAV_CMD.LAST)
                            {
                                temp.alt = (float)(double.Parse(matchs[i].Groups[3].Value.ToString(), new System.Globalization.CultureInfo("en-US")));
                                temp.lat = (float)(double.Parse(matchs[i].Groups[4].Value.ToString(), new System.Globalization.CultureInfo("en-US")));
                                temp.lng = (float)(double.Parse(matchs[i].Groups[5].Value.ToString(), new System.Globalization.CultureInfo("en-US")));
                            }
                            else
                            {
                                temp.alt = (float)(double.Parse(matchs[i].Groups[3].Value.ToString(), new System.Globalization.CultureInfo("en-US")));
                                temp.lat = (float)(double.Parse(matchs[i].Groups[4].Value.ToString(), new System.Globalization.CultureInfo("en-US")));
                                temp.lng = (float)(double.Parse(matchs[i].Groups[5].Value.ToString(), new System.Globalization.CultureInfo("en-US")));
                            }
                            cmds.Add(temp);

                            wp_count++;
                            if (wp_count == byte.MaxValue)
                                break;
                        }
                        if (wp_count == byte.MaxValue)
                        {
                            MessageBox.Show("To many Waypoints!!!");
                            break;
                        }
                    }
                }

                sr.Close();

                TXT_DefaultAlt.Text = (alt_hold).ToString();
                TXT_WPRad.Text = (wp_rad).ToString();
                TXT_loiterrad.Text = (loit_rad).ToString();

                processToScreen(cmds);

                writeKML();

                MainMap.ZoomAndCenterMarkers("objects");
            }
            catch (Exception ex)
            {
                MessageBox.Show("Can't open file! " + ex.ToString());
                return false;
            }
            return true;
        }
Esempio n. 22
0
        public void DoAcceptTcpClientCallback(IAsyncResult ar)
        {
            // Get the listener that handles the client request.
            TcpListener listener = (TcpListener)ar.AsyncState;

            // End the operation and display the received data on
            // the console.
            using (
                TcpClient client = listener.EndAcceptTcpClient(ar))
            {
                // Signal the calling thread to continue.
                tcpClientConnected.Set();



                try
                {
                    // Get a stream object for reading and writing
                    log.Info("Accepted Client " + client.Client.RemoteEndPoint.ToString());
                    //client.SendBufferSize = 100 * 1024; // 100kb
                    //client.LingerState.Enabled = true;
                    //client.NoDelay = true;

                    // makesure we have valid image
                    GCSViews.FlightData.mymap.streamjpgenable = true;
                    GCSViews.FlightData.myhud.streamjpgenable = true;

                    NetworkStream stream = client.GetStream();

                    // 3 seconds
                    stream.ReadTimeout = 3000;

again:

                    var asciiEncoding = new ASCIIEncoding();

                    var request = new byte[1024];

                    int    len  = stream.Read(request, 0, request.Length);
                    string head = System.Text.Encoding.ASCII.GetString(request, 0, len);
                    log.Info(head);

                    int index = head.IndexOf('\n');

                    string url = head.Substring(0, index - 1);
                    //url = url.Replace("\r", "");
                    //url = url.Replace("GET ","");
                    //url = url.Replace(" HTTP/1.0", "");
                    //url = url.Replace(" HTTP/1.1", "");

                    if (url.Contains("websocket"))
                    {
                        using (var writer = new StreamWriter(stream, Encoding.Default))
                        {
                            writer.WriteLine("HTTP/1.1 101 WebSocket Protocol Handshake");
                            writer.WriteLine("Upgrade: WebSocket");
                            writer.WriteLine("Connection: Upgrade");
                            writer.WriteLine("WebSocket-Location: ws://localhost:56781/websocket/server");

                            int start = head.IndexOf("Sec-WebSocket-Key:") + 19;
                            int end   = head.IndexOf('\r', start);
                            if (end == -1)
                            {
                                end = head.IndexOf('\n', start);
                            }
                            string accept = ComputeWebSocketHandshakeSecurityHash09(head.Substring(start, end - start));

                            writer.WriteLine("Sec-WebSocket-Accept: " + accept);

                            writer.WriteLine("Server: APM Planner");

                            writer.WriteLine("");

                            writer.Flush();

                            while (client.Connected)
                            {
                                Thread.Sleep(200);
                                log.Debug(stream.DataAvailable + " " + client.Available);

                                while (client.Available > 0)
                                {
                                    Console.Write(stream.ReadByte());
                                }

                                byte[] packet = new byte[256];

                                string sendme = MainV2.comPort.MAV.cs.roll + "," + MainV2.comPort.MAV.cs.pitch + "," + MainV2.comPort.MAV.cs.yaw;

                                packet[0] = 0x81; // fin - binary
                                packet[1] = (byte)sendme.Length;

                                int i = 2;
                                foreach (char ch in sendme)
                                {
                                    packet[i++] = (byte)ch;
                                }

                                stream.Write(packet, 0, i);

                                //break;
                            }
                        }
                    }
                    else if (url.Contains("georefnetwork.kml"))
                    {
                        string header = "HTTP/1.1 200 OK\r\nContent-Type: application/vnd.google-earth.kml+xml\r\nContent-Length: " + georefkml.Length + "\r\n\r\n";
                        byte[] temp   = asciiEncoding.GetBytes(header);
                        stream.Write(temp, 0, temp.Length);

                        byte[] buffer = Encoding.ASCII.GetBytes(georefkml);

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

                        goto again;

                        //stream.Close();
                    }
                    else if (url.Contains("network.kml"))
                    {
                        SharpKml.Dom.Document kml = new SharpKml.Dom.Document();

                        SharpKml.Dom.Placemark pmplane = new SharpKml.Dom.Placemark();
                        pmplane.Name = "P/Q ";

                        pmplane.Visibility = true;

                        SharpKml.Dom.Location loc = new SharpKml.Dom.Location();
                        loc.Latitude  = MainV2.comPort.MAV.cs.lat;
                        loc.Longitude = MainV2.comPort.MAV.cs.lng;
                        loc.Altitude  = MainV2.comPort.MAV.cs.alt;

                        if (loc.Altitude < 0)
                        {
                            loc.Altitude = 0.01;
                        }

                        SharpKml.Dom.Orientation ori = new SharpKml.Dom.Orientation();
                        ori.Heading = MainV2.comPort.MAV.cs.yaw;
                        ori.Roll    = -MainV2.comPort.MAV.cs.roll;
                        ori.Tilt    = -MainV2.comPort.MAV.cs.pitch;

                        SharpKml.Dom.Scale sca = new SharpKml.Dom.Scale();

                        sca.X = 2;
                        sca.Y = 2;
                        sca.Z = 2;

                        SharpKml.Dom.Model model = new SharpKml.Dom.Model();
                        model.Location     = loc;
                        model.Orientation  = ori;
                        model.AltitudeMode = SharpKml.Dom.AltitudeMode.Absolute;
                        model.Scale        = sca;

                        SharpKml.Dom.Link link = new SharpKml.Dom.Link();
                        link.Href = new Uri("block_plane_0.dae", UriKind.Relative);

                        model.Link = link;

                        pmplane.Geometry = model;

                        SharpKml.Dom.LookAt la = new SharpKml.Dom.LookAt()
                        {
                            Altitude     = loc.Altitude.Value,
                            Latitude     = loc.Latitude.Value,
                            Longitude    = loc.Longitude.Value,
                            Tilt         = 80,
                            Heading      = MainV2.comPort.MAV.cs.yaw,
                            AltitudeMode = SharpKml.Dom.AltitudeMode.Absolute,
                            Range        = 50
                        };

                        kml.Viewpoint = la;

                        kml.AddFeature(pmplane);

                        SharpKml.Dom.CoordinateCollection coords = new SharpKml.Dom.CoordinateCollection();

                        foreach (var point in MainV2.comPort.MAV.wps.Values)
                        {
                            coords.Add(new SharpKml.Base.Vector(point.x, point.y, point.z));
                        }

                        SharpKml.Dom.LineString ls = new SharpKml.Dom.LineString();
                        ls.AltitudeMode = SharpKml.Dom.AltitudeMode.RelativeToGround;
                        ls.Coordinates  = coords;

                        SharpKml.Dom.Placemark pm = new SharpKml.Dom.Placemark()
                        {
                            Geometry = ls
                        };

                        kml.AddFeature(pm);

                        SharpKml.Base.Serializer serializer = new SharpKml.Base.Serializer();
                        serializer.Serialize(kml);

                        byte[] buffer = Encoding.ASCII.GetBytes(serializer.Xml);

                        string header = "HTTP/1.1 200 OK\r\nContent-Type: application/vnd.google-earth.kml+xml\r\nContent-Length: " + buffer.Length + "\r\n\r\n";
                        byte[] temp   = asciiEncoding.GetBytes(header);
                        stream.Write(temp, 0, temp.Length);

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

                        goto again;

                        //stream.Close();
                    }
                    else if (url.Contains("block_plane_0.dae"))
                    {
                        string header = "HTTP/1.1 200 OK\r\nContent-Type: text/plain\r\n\r\n";
                        byte[] temp   = asciiEncoding.GetBytes(header);
                        stream.Write(temp, 0, temp.Length);

                        BinaryReader file   = new BinaryReader(File.Open("block_plane_0.dae", FileMode.Open, FileAccess.Read, FileShare.Read));
                        byte[]       buffer = new byte[1024];
                        while (file.PeekChar() != -1)
                        {
                            int leng = file.Read(buffer, 0, buffer.Length);

                            stream.Write(buffer, 0, leng);
                        }
                        file.Close();
                        stream.Close();
                    }
                    else if (url.Contains("hud.html"))
                    {
                        string header = "HTTP/1.1 200 OK\r\nContent-Type: text/html\r\n\r\n";
                        byte[] temp   = asciiEncoding.GetBytes(header);
                        stream.Write(temp, 0, temp.Length);

                        BinaryReader file   = new BinaryReader(File.Open("hud.html", FileMode.Open, FileAccess.Read, FileShare.Read));
                        byte[]       buffer = new byte[1024];
                        while (file.PeekChar() != -1)
                        {
                            int leng = file.Read(buffer, 0, buffer.Length);

                            stream.Write(buffer, 0, leng);
                        }
                        file.Close();
                        stream.Close();
                    }
                    else if (url.ToLower().Contains("hud.jpg") || url.ToLower().Contains("map.jpg") || url.ToLower().Contains("both.jpg"))
                    {
                        refreshmap();

                        string header = "HTTP/1.1 200 OK\r\nContent-Type: multipart/x-mixed-replace;boundary=APMPLANNER\r\n\r\n--APMPLANNER\r\n";
                        byte[] temp   = asciiEncoding.GetBytes(header);
                        stream.Write(temp, 0, temp.Length);

                        while (client.Connected)
                        {
                            System.Threading.Thread.Sleep(200); // 5hz
                            byte[] data = null;

                            if (url.ToLower().Contains("hud"))
                            {
                                GCSViews.FlightData.myhud.streamjpgenable = true;
                                data = GCSViews.FlightData.myhud.streamjpg.ToArray();
                            }
                            else if (url.ToLower().Contains("map"))
                            {
                                GCSViews.FlightData.mymap.streamjpgenable = true;
                                data = GCSViews.FlightData.mymap.streamjpg.ToArray();
                            }
                            else
                            {
                                GCSViews.FlightData.mymap.streamjpgenable = true;
                                GCSViews.FlightData.myhud.streamjpgenable = true;
                                Image img1   = Image.FromStream(GCSViews.FlightData.myhud.streamjpg);
                                Image img2   = Image.FromStream(GCSViews.FlightData.mymap.streamjpg);
                                int   bigger = img1.Height > img2.Height ? img1.Height : img2.Height;
                                Image imgout = new Bitmap(img1.Width + img2.Width, bigger);

                                Graphics grap = Graphics.FromImage(imgout);

                                grap.DrawImageUnscaled(img1, 0, 0);
                                grap.DrawImageUnscaled(img2, img1.Width, 0);

                                MemoryStream streamjpg = new MemoryStream();
                                imgout.Save(streamjpg, System.Drawing.Imaging.ImageFormat.Jpeg);
                                data = streamjpg.ToArray();
                            }

                            header = "Content-Type: image/jpeg\r\nContent-Length: " + data.Length + "\r\n\r\n";
                            temp   = asciiEncoding.GetBytes(header);
                            stream.Write(temp, 0, temp.Length);

                            stream.Write(data, 0, data.Length);

                            header = "\r\n--APMPLANNER\r\n";
                            temp   = asciiEncoding.GetBytes(header);
                            stream.Write(temp, 0, temp.Length);
                        }
                        GCSViews.FlightData.mymap.streamjpgenable = false;
                        GCSViews.FlightData.myhud.streamjpgenable = false;
                        stream.Close();
                    }
                    else if (url.Contains("/guided?"))
                    {
                        //http://127.0.0.1:56781/guided?lat=-34&lng=117.8&alt=30

                        Regex rex = new Regex(@"lat=([\-\.0-9]+)&lng=([\-\.0-9]+)&alt=([\.0-9]+)", RegexOptions.IgnoreCase);

                        Match match = rex.Match(url);

                        if (match.Success)
                        {
                            Locationwp gwp = new Locationwp()
                            {
                                lat = double.Parse(match.Groups[1].Value),
                                lng = double.Parse(match.Groups[2].Value),
                                alt = float.Parse(match.Groups[3].Value)
                            };
                            try
                            {
                                MainV2.comPort.setGuidedModeWP(gwp);
                            }
                            catch { }

                            string header = "HTTP/1.1 200 OK\r\n\r\nSent Guide Mode Wp";
                            byte[] temp   = asciiEncoding.GetBytes(header);
                            stream.Write(temp, 0, temp.Length);
                        }
                        else
                        {
                            string header = "HTTP/1.1 200 OK\r\n\r\nFailed Guide Mode Wp";
                            byte[] temp   = asciiEncoding.GetBytes(header);
                            stream.Write(temp, 0, temp.Length);
                        }
                        stream.Close();
                    }
                    else if (url.ToLower().Contains(".jpg"))
                    {
                        Regex rex = new Regex(@"([^\s]+)\s(.+)\sHTTP/1", RegexOptions.IgnoreCase);

                        Match match = rex.Match(url);

                        if (match.Success)
                        {
                            string fileurl = match.Groups[2].Value;

                            using (Image orig = Image.FromFile(georefimagepath + fileurl))
                                using (Image resi = ResizeImage(orig, new Size(640, 480)))
                                    using (MemoryStream memstream = new MemoryStream())
                                    {
                                        resi.Save(memstream, System.Drawing.Imaging.ImageFormat.Jpeg);

                                        memstream.Position = 0;

                                        string header = "HTTP/1.1 200 OK\r\nContent-Type: image/jpg\r\nContent-Length: " + memstream.Length + "\r\n\r\n";
                                        byte[] temp   = asciiEncoding.GetBytes(header);
                                        stream.Write(temp, 0, temp.Length);

                                        BinaryReader file   = new BinaryReader(memstream);
                                        byte[]       buffer = new byte[1024];
                                        while (file.BaseStream.Position < file.BaseStream.Length)
                                        {
                                            int leng = file.Read(buffer, 0, buffer.Length);

                                            stream.Write(buffer, 0, leng);
                                        }
                                        file.Close();
                                        resi.Dispose();
                                        orig.Dispose();
                                    }

                            goto again;

                            //stream.Close();
                        }
                        else
                        {
                            string header = "HTTP/1.1 404 not found\r\nContent-Type: image/jpg\r\n\r\n";
                            byte[] temp   = asciiEncoding.GetBytes(header);
                            stream.Write(temp, 0, temp.Length);
                        }
                        stream.Close();
                    }
                    else if (url.ToLower().Contains("post /guide"))
                    {
                        Regex rex = new Regex(@"lat"":([\-\.0-9]+),""lon"":([\-\.0-9]+),""alt"":([\.0-9]+)", RegexOptions.IgnoreCase);

                        Match match = rex.Match(head);

                        if (match.Success)
                        {
                            Locationwp gwp = new Locationwp()
                            {
                                lat = double.Parse(match.Groups[1].Value),
                                lng = double.Parse(match.Groups[2].Value),
                                alt = float.Parse(match.Groups[3].Value)
                            };
                            try
                            {
                                MainV2.comPort.setGuidedModeWP(gwp);
                            }
                            catch { }

                            string header = "HTTP/1.1 200 OK\r\n\r\nSent Guide Mode Wp";
                            byte[] temp   = asciiEncoding.GetBytes(header);
                            stream.Write(temp, 0, temp.Length);
                        }
                        else
                        {
                            string header = "HTTP/1.1 200 OK\r\n\r\nFailed Guide Mode Wp";
                            byte[] temp   = asciiEncoding.GetBytes(header);
                            stream.Write(temp, 0, temp.Length);
                        }
                        stream.Close();
                    }
                    else if (url.ToLower().Contains("/command_long"))
                    {
                        string header = "HTTP/1.1 404 not found\r\nContent-Type: image/jpg\r\n\r\n";
                        byte[] temp   = asciiEncoding.GetBytes(header);
                        stream.Write(temp, 0, temp.Length);

                        stream.Close();
                    }
                    else if (url.ToLower().Contains("/rcoverride"))
                    {
                        string header = "HTTP/1.1 404 not found\r\nContent-Type: image/jpg\r\n\r\n";
                        byte[] temp   = asciiEncoding.GetBytes(header);
                        stream.Write(temp, 0, temp.Length);

                        stream.Close();
                    }
                    else if (url.ToLower().Contains("/get_mission"))
                    {
                        string header = "HTTP/1.1 404 not found\r\nContent-Type: image/jpg\r\n\r\n";
                        byte[] temp   = asciiEncoding.GetBytes(header);
                        stream.Write(temp, 0, temp.Length);

                        stream.Close();
                    }
                    else if (url.ToLower().Contains("/mavlink/"))
                    {
                        /*
                         * GET /mavlink/ATTITUDE+VFR_HUD+NAV_CONTROLLER_OUTPUT+META_WAYPOINT+GPS_RAW_INT+HEARTBEAT+META_LINKQUALITY+GPS_STATUS+STATUSTEXT+SYS_STATUS?_=1355828718540 HTTP/1.1
                         * Host: ubuntu:9999
                         * Connection: keep-alive
                         * X-Requested-With: XMLHttpRequest
                         * User-Agent: Mozilla/5.0 (Windows NT 6.1; WOW64) AppleWebKit/537.11 (KHTML, like Gecko) Chrome/23.0.1271.97 Safari/537.11
                         * Accept:
                         * Referer: http://ubuntu:9999/index.html
                         * Accept-Encoding: gzip,deflate,sdch
                         * Accept-Language: en-GB,en-US;q=0.8,en;q=0.6
                         * Accept-Charset: ISO-8859-1,utf-8;q=0.7,*;q=0.3
                         *
                         * HTTP/1.1 200 OK
                         * Content-Type: application/json
                         * Content-Length: 2121
                         * Date: Thu, 29 Nov 2012 12:13:38 GMT
                         * Server: ubuntu
                         *
                         * {
                         * "VFR_HUD": {"msg": {"throttle": 0, "groundspeed": 0.0, "airspeed": 0.0, "climb": 0.0, "mavpackettype": "VFR_HUD", "alt": -0.47999998927116394, "heading": 108}, "index": 687, "time_usec": 0},
                         * "STATUSTEXT": {"msg": {"mavpackettype": "STATUSTEXT", "severity": 1, "text": "Initialising APM..."}, "index": 2, "time_usec": 0},
                         * "SYS_STATUS": {"msg": {"onboard_control_sensors_present": 4294966287, "load": 0, "battery_remaining": -1, "errors_count4": 0, "drop_rate_comm": 0, "errors_count2": 0, "errors_count3": 0, "errors_comm": 0, "current_battery": -1, "errors_count1": 0, "onboard_control_sensors_health": 4294966287, "mavpackettype": "SYS_STATUS", "onboard_control_sensors_enabled": 4294945807, "voltage_battery": 10080}, "index": 693, "time_usec": 0},
                         * "META_LINKQUALITY": {"msg": {"master_in": 11110, "mav_loss": 0, "mavpackettype": "META_LINKQUALITY", "master_out": 194, "packet_loss": 0.0}, "index": 194, "time_usec": 0},
                         * "ATTITUDE": {"msg": {"pitchspeed": -0.000976863200776279, "yaw": 1.8878594636917114, "rollspeed": -0.0030046366155147552, "time_boot_ms": 194676, "pitch": -0.09986469894647598, "mavpackettype": "ATTITUDE", "yawspeed": -0.0015030358918011189, "roll": -0.029391441494226456}, "index": 687, "time_usec": 0},
                         * "GPS_RAW_INT": {"msg": {"fix_type": 1, "cog": 0, "epv": 65535, "lon": 0, "time_usec": 0, "eph": 9999, "satellites_visible": 0, "lat": 0, "mavpackettype": "GPS_RAW_INT", "alt": 137000, "vel": 0}, "index": 687, "time_usec": 0},
                         * "HEARTBEAT": {"msg": {"custom_mode": 0, "system_status": 4, "base_mode": 81, "autopilot": 3, "mavpackettype": "HEARTBEAT", "type": 2, "mavlink_version": 3}, "index": 190, "time_usec": 0},
                         * "GPS_STATUS": {"msg": {"satellite_snr": "", "satellite_azimuth": "", "satellite_prn": "", "satellite_elevation": "", "satellites_visible": 0, "satellite_used": "", "mavpackettype": "GPS_STATUS"}, "index": 2, "time_usec": 0},
                         * "NAV_CONTROLLER_OUTPUT": {"msg": {"wp_dist": 0, "nav_pitch": 0.0, "target_bearing": 0, "nav_roll": 0.0, "aspd_error": 0.0, "alt_error": 0.0, "mavpackettype": "NAV_CONTROLLER_OUTPUT", "xtrack_error": 0.0, "nav_bearing": 0}, "index": 687, "time_usec": 0}}
                         */

                        JavaScriptSerializer serializer = new JavaScriptSerializer();

                        object[] data = new object[20];


                        Messagejson message = new Messagejson();


                        if (MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_ATTITUDE] != null)
                        {
                            message.ATTITUDE = new Message2()
                            {
                                index = MainV2.comPort.MAV.packetseencount[MAVLink.MAVLINK_MSG_ID_ATTITUDE], msg = MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_ATTITUDE].ByteArrayToStructure <MAVLink.mavlink_attitude_t>(6)
                            }
                        }
                        ;
                        if (MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_VFR_HUD] != null)
                        {
                            message.VFR_HUD = new Message2()
                            {
                                index = MainV2.comPort.MAV.packetseencount[MAVLink.MAVLINK_MSG_ID_VFR_HUD], msg = MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_VFR_HUD].ByteArrayToStructure <MAVLink.mavlink_vfr_hud_t>(6)
                            }
                        }
                        ;
                        if (MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT] != null)
                        {
                            message.NAV_CONTROLLER_OUTPUT = new Message2()
                            {
                                index = MainV2.comPort.MAV.packetseencount[MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT], msg = MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT].ByteArrayToStructure <MAVLink.mavlink_nav_controller_output_t>(6)
                            }
                        }
                        ;
                        if (MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT] != null)
                        {
                            message.GPS_RAW_INT = new Message2()
                            {
                                index = MainV2.comPort.MAV.packetseencount[MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT], msg = MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT].ByteArrayToStructure <MAVLink.mavlink_gps_raw_int_t>(6)
                            }
                        }
                        ;
                        if (MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_HEARTBEAT] != null)
                        {
                            message.HEARTBEAT = new Message2()
                            {
                                index = MainV2.comPort.MAV.packetseencount[MAVLink.MAVLINK_MSG_ID_HEARTBEAT], msg = MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_HEARTBEAT].ByteArrayToStructure <MAVLink.mavlink_heartbeat_t>(6)
                            }
                        }
                        ;
                        if (MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_GPS_STATUS] != null)
                        {
                            message.GPS_STATUS = new Message2()
                            {
                                index = MainV2.comPort.MAV.packetseencount[MAVLink.MAVLINK_MSG_ID_GPS_STATUS], msg = MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_GPS_STATUS].ByteArrayToStructure <MAVLink.mavlink_gps_status_t>(6)
                            }
                        }
                        ;
                        if (MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT] != null)
                        {
                            message.STATUSTEXT = new Message2()
                            {
                                index = MainV2.comPort.MAV.packetseencount[MAVLink.MAVLINK_MSG_ID_STATUSTEXT], msg = MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT].ByteArrayToStructure <MAVLink.mavlink_statustext_t>(6)
                            }
                        }
                        ;
                        if (MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_SYS_STATUS] != null)
                        {
                            message.SYS_STATUS = new Message2()
                            {
                                index = MainV2.comPort.MAV.packetseencount[MAVLink.MAVLINK_MSG_ID_SYS_STATUS], msg = MainV2.comPort.MAV.packets[MAVLink.MAVLINK_MSG_ID_SYS_STATUS].ByteArrayToStructure <MAVLink.mavlink_sys_status_t>(6)
                            }
                        }
                        ;

                        message.META_LINKQUALITY = message.SYS_STATUS = new Message2()
                        {
                            index = packetindex, msg = new META_LINKQUALITY()
                            {
                                master_in = (int)MainV2.comPort.packetsnotlost, mavpackettype = "META_LINKQUALITY", master_out = MainV2.comPort.packetcount, packet_loss = 100 - MainV2.comPort.MAV.cs.linkqualitygcs
                            }
                        };

                        packetindex++;

                        string output = serializer.Serialize(message);

                        string header = "HTTP/1.1 200 OK\r\nContent-Type: application/json\r\nContent-Length: " + output.Length + "\r\n\r\n";
                        byte[] temp   = asciiEncoding.GetBytes(header);
                        stream.Write(temp, 0, temp.Length);

                        temp = asciiEncoding.GetBytes(output);
                        stream.Write(temp, 0, temp.Length);

                        goto again;

                        //stream.Close();
                    }
                    else if (url.ToLower().Contains("/mav/"))
                    {
                        //C:\Users\hog\Desktop\DIYDrones\mavelous\modules\lib\mavelous_web


                        Regex rex = new Regex(@"([^\s]+)\s(.+)\sHTTP/1", RegexOptions.IgnoreCase);

                        Match match = rex.Match(url);

                        if (match.Success)
                        {
                            string fileurl = match.Groups[2].Value;

                            fileurl = fileurl.Replace("/mav/", "");

                            if (fileurl == "" || fileurl == "/")
                            {
                                fileurl = "index.html";
                            }

                            string header = "HTTP/1.1 200 OK\r\n";
                            if (fileurl.Contains(".html"))
                            {
                                header += "Content-Type: text/html\r\n\r\n";
                            }
                            else if (fileurl.Contains(".js"))
                            {
                                header += "Content-Type: application/x-javascript\r\n\r\n";
                            }
                            else if (fileurl.Contains(".css"))
                            {
                                header += "Content-Type: text/css\r\n\r\n";
                            }
                            else
                            {
                                header += "Content-Type: text/plain\r\n\r\n";
                            }
                            byte[] temp = asciiEncoding.GetBytes(header);
                            stream.Write(temp, 0, temp.Length);


                            BinaryReader file   = new BinaryReader(File.Open(mavelous_web + fileurl, FileMode.Open, FileAccess.Read, FileShare.Read));
                            byte[]       buffer = new byte[1024];
                            while (file.BaseStream.Position < file.BaseStream.Length)
                            {
                                int leng = file.Read(buffer, 0, buffer.Length);

                                stream.Write(buffer, 0, leng);
                            }
                            file.Close();
                            stream.Close();
                        }
                        else
                        {
                            string header = "HTTP/1.1 404 not found\r\nContent-Type: image/jpg\r\n\r\n";
                            byte[] temp   = asciiEncoding.GetBytes(header);
                            stream.Write(temp, 0, temp.Length);

                            stream.Close();
                        }
                    }
                    else
                    {
                        Console.WriteLine(url);
                        string header = "HTTP/1.1 200 OK\r\nContent-Type: text/html\r\n\r\n";
                        byte[] temp   = asciiEncoding.GetBytes(header);
                        stream.Write(temp, 0, temp.Length);

                        string content = @"
                <a href=/mav/>Mavelous</a>
<a href=/mavlink/>Mavelous traffic</a>
<a href=/hud.jpg>Hud image</a>
<a href=/map.jpg>Map image </a>
<a href=/both.jpg>Map & hud image</a>
<a href=/hud.html>hud html5</a>
<a href=/network.kml>network kml</a>

";
                        temp = asciiEncoding.GetBytes(content);
                        stream.Write(temp, 0, temp.Length);

                        stream.Close();
                    }

                    stream.Close();
                }
                catch (Exception ee)
                {
                    log.Error("Failed mjpg ", ee);
                    try
                    {
                        client.Close();
                    }
                    catch { }
                }
            }
        }
Esempio n. 23
0
        public void setNewWPAlt(Locationwp gotohere)
        {
            giveComport = true;

            try
            {
                gotohere.id = (byte)MAV_CMD.WAYPOINT;

                MAV_MISSION_RESULT ans = setWP(gotohere, 0, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT, (byte)3);

                if (ans != MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED)
                    throw new Exception("Alt Change Failed");
            }
            catch (Exception ex) { giveComport = false; log.Error(ex); throw; }

            giveComport = false;
        }
        private void goHereToolStripMenuItem_Click(object sender, EventArgs e)
        {
            if (!MainV2.comPort.BaseStream.IsOpen)
            {
                CustomMessageBox.Show("Please Connect First");
                return;
            }

            string alt = "100";

            if (MainV2.cs.firmware == MainV2.Firmwares.ArduCopter2)
            {
                alt = (10 * MainV2.cs.multiplierdist).ToString("0");
            }
            else
            {
                alt = (100 * MainV2.cs.multiplierdist).ToString("0");
            }
            if (DialogResult.Cancel == Common.InputBox("Enter Alt", "Enter Guided Mode Alt", ref alt))
                return;

            int intalt = (int)(100 * MainV2.cs.multiplierdist);
            if (!int.TryParse(alt, out intalt))
            {
                CustomMessageBox.Show("Bad Alt");
                return;
            }

            if (gotolocation.Lat == 0 || gotolocation.Lng == 0)
            {
                CustomMessageBox.Show("Bad Lat/Long");
                return;
            }

            Locationwp gotohere = new Locationwp();

            gotohere.id = (byte)MAVLink.MAV_CMD.WAYPOINT;
            gotohere.alt = (float)(intalt / MainV2.cs.multiplierdist); // back to m
            gotohere.lat = (float)(gotolocation.Lat);
            gotohere.lng = (float)(gotolocation.Lng);

            try
            {
                MainV2.giveComport = true;

                MainV2.comPort.setWP(gotohere, 0, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT, (byte)2);

                GuidedModeWP = new PointLatLngAlt(gotohere.lat, gotohere.lng, gotohere.alt,"Guided Mode");

                MainV2.giveComport = false;
            }
            catch (Exception ex) { MainV2.giveComport = false; CustomMessageBox.Show("Error sending command : " + ex.Message); }
        }
Esempio n. 25
0
        public void sendWPS(List <PointLatLngAlt> Points)
        {
            MAVLink.MAV_FRAME frame = MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT;
            Locationwp        home  = new Locationwp();

            try
            {
                home.id  = (byte)MAVLink.MAV_CMD.WAYPOINT;
                home.lat = 23.1647061918178;
                home.lng = 113.455638885498;
                home.alt = 20; // use saved home
            }
            catch { throw new Exception("Your home location is invalid"); }

            ushort count = (ushort)(Points.Count + 1);//含家地址

            UpLoadForm upform = new UpLoadForm();

            upform.StartPosition = FormStartPosition.Manual;
            upform.Location      = new System.Drawing.Point((this.form.Width - upform.Width) / 2, (this.form.Height - upform.Height) / 2);

            //   this.upform.Visible = true;

            // Application.DoEvents();

            upform.Show();



            //    Application.DoEvents();
            upform.SetMessage(0, count);
            // this.upform.ShowDialog();
            //  Thread.Sleep(100);

            setWPTotal(count);
            this.ListMessage("已发送航点数量...");

            upform.SetMessage(1, 0);

            bool mission_uploading = true;

            Locationwp[] wp_location = new Locationwp[count - 1];
            int          i           = 0;

            foreach (PointLatLngAlt point in Points)
            {
                wp_location[i].id  = (byte)MAVLink.MAV_CMD.WAYPOINT;
                wp_location[i].lat = point.Lat;
                wp_location[i].lng = point.Lng;
                wp_location[i].alt = point.Alt;
                i++;
            }
            int seq = 0;

            bool isSendTotalSuccess = false;
            int  cnt = 0;

            while (mission_uploading)
            {
                Program.mav_msg_handler.mission_request = false;
                Program.mav_msg_handler.mission_ack     = false;
                Program.mav_msg_handler.SetTimeout(5);
                while (true)
                {
                    if (Program.mav_msg_handler.Wait())
                    {
                        if (isSendTotalSuccess)
                        {
                            ListMessage("发送航点超时出错");
                            mission_uploading = false;
                            break;
                        }
                        else
                        {
                            cnt++;
                            if (cnt >= 3)
                            {
                                ListMessage("发送航点数量超时出错");
                                mission_uploading = false;
                                break;
                            }
                            else
                            {
                                ListMessage("发送航点数量超时出错,准备重发...");
                                setWPTotal(count);
                                break;
                            }
                        }
                    }

                    if (upform.isCancel)
                    {
                        this.ListMessage("您已经取消航点发送...");
                        upform.Close();
                        return;
                    }

                    if (Program.mav_msg_handler.mission_ack)
                    {
                        //  this.ListMessage("收到ACK...");
                        if (Program.mav_msg_handler.mavlink_mission_ack.type != 0)
                        {
                            ListMessage("发送航点出错");
                        }
                        else
                        {
                            ListMessage("航点上传完毕");
                        }
                        mission_uploading = false;
                        break;
                    }

                    if (Program.mav_msg_handler.mission_request == true)
                    {
                        isSendTotalSuccess = true;
                        //   this.ListMessage("收到REQUEST...");
                        seq = Program.mav_msg_handler.mavlink_mission_request.seq;
                        if (seq == 0)
                        {
                            setWP(home, 0, MAVLink.MAV_FRAME.GLOBAL, 0);
                            ListMessage("正在发送HOME...");
                        }
                        else
                        {
                            setWP(wp_location[seq - 1], (ushort)(seq), frame, 0);
                            ListMessage("正在发送航点" + seq.ToString() + "...");
                            upform.SetMessage(2, seq);
                            upform.SetCount(seq, count);
                        }
                        break;
                    }
                    Application.DoEvents();
                }
            }


            Thread.Sleep(500);
            upform.SetMessage(3, 0);
            Thread.Sleep(500);
            setWPACK();
            upform.Close();
        }