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; } ); } }
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); }
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); } }
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 { } } } }
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(); }
private void updateLocationLabel(Locationwp plla) { this.BeginInvoke((MethodInvoker)delegate { LBL_location.Text = gotolocation.Lat + " " + gotolocation.Lng + " " + gotolocation.Alt +" "+ gotolocation.Tag; } ); }
/// <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; }
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(); }
/// <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; }
/// <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(); }
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()); } }
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(); }
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); } }
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); }
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; }
/// <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; }
public PointLatLngAlt(Locationwp locwp) { this.Lat = locwp.lat; this.Lng = locwp.lng; this.Alt = locwp.alt; }
/// <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; }
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 { } } } }
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); } }
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(); }