public static List<PointLatLngAlt> getAirports(PointLatLngAlt centerpoint) { log.Info("getAirports " + centerpoint); // check if we have moved 66% from our last cache center point if (currentcenter.GetDistance(centerpoint) < ((proximity/3)*2)) { return cache; } log.Info("getAirports - regen list"); // generate a new list currentcenter = centerpoint; cache.Clear(); foreach (PointLatLngAlt item in airports) { if (item.GetDistance(centerpoint) < proximity) { cache.Add(item); } } return cache; }
public List<PointLatLngAlt> calcspline(PointLatLngAlt currentpos, PointLatLngAlt p1, PointLatLngAlt p2) { List<PointLatLngAlt> answer = new List<PointLatLngAlt>(); spline_t = spline_t_sq = 0.0f; spline_p0 = currentpos; spline_p1 = p1; spline_p2 = p2; spline_p0_prime = new Vector3(); double yaw = (currentpos.GetBearing(p1) + 360) % 360; spline_p0_prime.x = .000001 * Math.Sin(yaw * deg2rad); spline_p0_prime.y = .000001 * Math.Cos(yaw * deg2rad); spline_p0_prime.z = spline_p1.z - spline_p0.z; initialize_spline_segment(); int steps = 30; foreach (var step in range(steps +1)) { spline_t = (1f / steps) * step; spline_t_sq = spline_t * spline_t; evaluate_spline(); answer.Add(new PointLatLngAlt(spline_target.x, spline_target.y, spline_target.z, "")); } return answer; }
void QueueSendGrid(object nothing) { issending = true; // 8 across - 7 down // cycle though the bitmask to check what we need to send (8*7) for (byte i = 0; i < 56; i++) { // check to see if the ap requested this box. if ((lastrequest.mask & ((ulong)1 << i)) > 0) { // get the requested lat and lon double lat = lastrequest.lat / 1e7; double lon = lastrequest.lon / 1e7; // get the distance between grids int bitgridspacing = lastrequest.grid_spacing * 4; // get the new point, based on our current bit. var newplla = new PointLatLngAlt(lat, lon).location_offset(bitgridspacing * (i % 8), bitgridspacing * (int)Math.Floor(i / 8.0)); // send a 4*4 grid, based on the lat lon of the bitmask SendGrid(newplla.Lat, newplla.Lng, lastrequest.grid_spacing, i); // 12hz = (43+6) * 12 = 588 bps System.Threading.Thread.Sleep(1000/12); } } issending = false; }
void SendGrid(double lat, double lon, ushort grid_spacing, byte bit) { log.Info("SendGrid "+ lat + " " + lon + " space " + grid_spacing + " bit "+ bit); MAVLink.mavlink_terrain_data_t resp = new MAVLink.mavlink_terrain_data_t(); resp.grid_spacing = grid_spacing; resp.lat = lastrequest.lat; resp.lon = lastrequest.lon; resp.gridbit = bit; resp.data = new short[16]; for (int i = 0; i < (4 * 4); i++) { int y = i % 4; int x = i / 4; PointLatLngAlt plla = new PointLatLngAlt(lat, lon).location_offset(x * grid_spacing, y * grid_spacing); double alt = srtm.getAltitude(plla.Lat, plla.Lng); resp.data[i] = (short)alt; } MainV2.comPort.sendPacket(resp); }
public PointLatLngAlt(PointLatLngAlt plla) { this.Lat = plla.Lat; this.Lng = plla.Lng; this.Alt = plla.Alt; this.color = plla.color; this.Tag = plla.Tag; }
public utmpos(PointLatLngAlt pos) { double[] dd = pos.ToUTM(pos.GetUTMZone()); this.x = dd[0]; this.y = dd[1]; this.zone = pos.GetUTMZone(); this.Tag = null; }
public override void SendCommand() { if (masterpos.Lat == 0 || masterpos.Lng == 0) return; Console.WriteLine(DateTime.Now); Console.WriteLine("Leader {0} {1} {2}",masterpos.Lat,masterpos.Lng,masterpos.Alt); int a = 0; foreach (var port in MainV2.Comports) { if (port == Leader) continue; PointLatLngAlt target = new PointLatLngAlt(masterpos); try { //convert Wgs84ConversionInfo to utm CoordinateTransformationFactory ctfac = new CoordinateTransformationFactory(); GeographicCoordinateSystem wgs84 = GeographicCoordinateSystem.WGS84; int utmzone = (int)((masterpos.Lng - -186.0) / 6.0); IProjectedCoordinateSystem utm = ProjectedCoordinateSystem.WGS84_UTM(utmzone, masterpos.Lat < 0 ? false : true); ICoordinateTransformation trans = ctfac.CreateFromCoordinateSystems(wgs84, utm); double[] pll1 = { target.Lng, target.Lat }; double[] p1 = trans.MathTransform.Transform(pll1); // add offsets to utm p1[0] += ((HIL.Vector3)offsets[port]).x; p1[1] += ((HIL.Vector3)offsets[port]).y; // convert back to wgs84 IMathTransform inversedTransform = trans.MathTransform.Inverse(); double[] point = inversedTransform.Transform(p1); target.Lat = point[1]; target.Lng = point[0]; target.Alt += ((HIL.Vector3)offsets[port]).z; port.setGuidedModeWP(new Locationwp() { alt = (float)target.Alt, lat = target.Lat, lng = target.Lng, id = (byte)MAVLink.MAV_CMD.WAYPOINT }); Console.WriteLine("{0} {1} {2} {3}", port.ToString(), target.Lat, target.Lng, target.Alt); } catch (Exception ex) { Console.WriteLine("Failed to send command " + port.ToString() + "\n" + ex.ToString()); } a++; } }
public override void Update() { if (MainV2.comPort.MAV.cs.lat == 0 || MainV2.comPort.MAV.cs.lng == 0) return; if (Leader == null) Leader = MainV2.comPort; masterpos = new PointLatLngAlt(Leader.MAV.cs.lat, Leader.MAV.cs.lng, Leader.MAV.cs.alt, ""); }
// pv_latlon_to_vector - convert lat/lon coordinates to a position vector public Vector3 pv_location_to_vector(PointLatLngAlt loc) { PointLatLngAlt home = MainV2.comPort.MAV.cs.HomeLocation; scaleLongDown = longitude_scale(home); scaleLongUp = 1.0f / scaleLongDown; Vector3 tmp = new Vector3((loc.Lat * 1.0e7f - home.Lat * 1.0e7f) * LATLON_TO_CM, (loc.Lng * 1.0e7f - home.Lng * 1.0e7f) * LATLON_TO_CM * scaleLongDown, loc.Alt * 100); return tmp; }
public static PointLatLngAlt ProjectPoint() { PointLatLngAlt currentlocation = new PointLatLngAlt(MainV2.comPort.MAV.cs.lat, MainV2.comPort.MAV.cs.lng); double yawangle = MainV2.comPort.MAV.cs.campointc; double rollangle = MainV2.comPort.MAV.cs.campointb; double pitchangle = MainV2.comPort.MAV.cs.campointa; if ((float) MainV2.comPort.MAV.param["MNT_TYPE"] == 4) { yawangle = MainV2.comPort.MAVlist[71].cs.yaw; rollangle = MainV2.comPort.MAVlist[71].cs.roll; pitchangle = MainV2.comPort.MAVlist[71].cs.pitch; } int distout = 10; PointLatLngAlt newpos = PointLatLngAlt.Zero; //dist = Math.Tan((90 + pitchangle) * deg2rad) * (MainV2.comPort.MAV.cs.alt); while (distout < 1000) { // get a projected point to test intersection against - not using slope distance PointLatLngAlt newposdist = currentlocation.newpos(yawangle + MainV2.comPort.MAV.cs.yaw, distout); newposdist.Alt = srtm.getAltitude(newposdist.Lat, newposdist.Lng).alt; // get another point 50 infront PointLatLngAlt newposdist2 = currentlocation.newpos(yawangle + MainV2.comPort.MAV.cs.yaw, distout + 50); newposdist2.Alt = srtm.getAltitude(newposdist2.Lat, newposdist2.Lng).alt; // get the flat terrain distance out - at 0 alt double distflat = Math.Tan((90 + pitchangle) * deg2rad) * (MainV2.comPort.MAV.cs.altasl); // x is dist from plane, y is alt var newpoint = FindLineIntersection(new PointF(0,MainV2.comPort.MAV.cs.altasl), new PointF((float)distflat, 0), new PointF((float)distout, (float)newposdist.Alt), new PointF((float)distout + 50, (float)newposdist2.Alt)); if (newpoint.X != 0) { newpos = newposdist2; break; } distout += 50; } //Console.WriteLine("pitch " + pitchangle.ToString("0.000") + " yaw " + yawangle.ToString("0.000") + " dist" + dist.ToString("0.000")); //PointLatLngAlt newpos = currentlocation.newpos( yawangle + MainV2.comPort.MAV.cs.yaw, dist); //Console.WriteLine(newpos); return newpos; }
public static void POIAdd(PointLatLngAlt Point, string tag) { // local copy PointLatLngAlt pnt = Point; pnt.Tag = tag + "\n" + pnt.ToString(); POI.POIs.Add(pnt); if (POIModified != null) POIModified(null, null); }
public static void AddAirport(PointLatLngAlt plla) { // foreach (PointLatLngAlt item in airports) { // if (item.GetDistance(plla) < 200) { // return; } } airports.Add(plla); }
public static void POIDelete(PointLatLngAlt Point) { for (int a = 0; a < POI.POIs.Count; a++) { if (POI.POIs[a].Point() == Point) { POI.POIs.RemoveAt(a); if (POIModified != null) POIModified(null, null); return; } } }
public static void POIAdd(PointLatLngAlt Point) { if (Point == null) return; PointLatLngAlt pnt = Point; string output = ""; if (DialogResult.OK != InputBox.Show("POI", "Enter ID", ref output)) return; POIAdd(Point, output); }
public PointLatLngAlt ToLLA() { IProjectedCoordinateSystem utm = ProjectedCoordinateSystem.WGS84_UTM(Math.Abs(zone), zone < 0 ? false : true); ICoordinateTransformation trans = ctfac.CreateFromCoordinateSystems(wgs84, utm); // get leader utm coords double[] pll = trans.MathTransform.Inverse().Transform(this); PointLatLngAlt ans = new PointLatLngAlt(pll[1], pll[0]); if (this.Tag != null) ans.Tag = this.Tag.ToString(); return ans; }
public static List<PointLatLngAlt> GeneratePath(MAVState MAV) { List<PointLatLngAlt> result = new List<PointLatLngAlt>(); MAVLink.mavlink_mission_item_t? prevwp = null; int a = -1; foreach (var wp in MAV.wps.Values) { a++; if (!prevwp.HasValue) { // change firstwp/aka home to valid alt prevwp = new MAVLink.mavlink_mission_item_t?(new MAVLink.mavlink_mission_item_t() { x=wp.x,y = wp.y, z = 0}); continue; } if (wp.command != (byte)MAVLink.MAV_CMD.WAYPOINT && wp.command != (byte)MAVLink.MAV_CMD.SPLINE_WAYPOINT) continue; var wp1 = new PointLatLngAlt(prevwp.Value); var wp2 = new PointLatLngAlt(wp); var bearing = wp1.GetBearing(wp2); var distwps = wp1.GetDistance(wp2); var startalt = wp1.Alt; var endalt = wp2.Alt; if (startalt == 0) { startalt = endalt; } if(distwps > 5000) continue; for (double d = 0.1; d < distwps; d += 0.1) { var pnt = wp1.newpos(bearing, d); pnt.Alt = startalt + (d/distwps) * (endalt-startalt); result.Add(pnt); } prevwp = wp; } return result; }
PointLatLngAlt FindTrailPnt(PointLatLngAlt from) { // get the start point for the distance int start = trail.IndexOf(from); for (int i = start+1; i < trail.Count; i++) { double dist = from.GetDistance(trail[i]); // 2d distance if (dist > FollowDistance) { return trail[i]; } } return null; }
public static void POIAdd(PointLatLngAlt Point) { PointLatLngAlt pnt = Point; string output = ""; if (DialogResult.OK != InputBox.Show("POI", "Enter ID", ref output)) return; pnt.Tag = output + "\n" + pnt.ToString(); POI.POIs.Add(pnt); if (POIModified != null) POIModified(null, null); }
public static void POIEdit(PointLatLngAlt Point) { string output = ""; if (DialogResult.OK != InputBox.Show("POI", "Enter ID", ref output)) return; for (int a = 0; a < POI.POIs.Count; a++) { if (POI.POIs[a].Point() == Point) { POI.POIs[a].Tag = output + "\n" + Point.ToString(); if (POIModified != null) POIModified(null, null); return; } } }
public static void AddAirport(PointLatLngAlt plla) { lock (locker) { if (checkdups) { foreach (PointLatLngAlt item in airports) { if (item.GetDistance(plla) < 1000) // 1000m { return; } } } airports.Add(plla); newairports = true; } }
bool ReceviedPacket(byte[] rawpacket) { if (rawpacket[5] == (byte)MAVLink.MAVLINK_MSG_ID.TERRAIN_REQUEST) { MAVLink.mavlink_terrain_request_t packet = rawpacket.ByteArrayToStructure<MAVLink.mavlink_terrain_request_t>(); lastrequest = packet; log.Info("received TERRAIN_REQUEST " + packet.lat / 1e7 + " " + packet.lon / 1e7 + " space " + packet.grid_spacing + " " + Convert.ToString((long)packet.mask, 2)); // 8 across - 7 down // cycle though the bitmask to check what we need to send (8*7) for (byte i = 0; i < 56; i++) { // check to see if the ap requested this box. if ((lastrequest.mask & ((ulong)1 << i)) > 0) { // get the requested lat and lon double lat = lastrequest.lat / 1e7; double lon = lastrequest.lon / 1e7; // get the distance between grids int bitgridspacing = lastrequest.grid_spacing * 4; // get the new point, based on our current bit. var newplla = new PointLatLngAlt(lat, lon).location_offset(bitgridspacing * (i % 8),bitgridspacing * (int)Math.Floor(i / 8.0)); // send a 4*4 grid, based on the lat lon of the bitmask SendGrid(newplla.Lat, newplla.Lng, lastrequest.grid_spacing, i); } } } else if (rawpacket[5] == (byte)MAVLink.MAVLINK_MSG_ID.TERRAIN_REPORT) { MAVLink.mavlink_terrain_report_t packet = rawpacket.ByteArrayToStructure<MAVLink.mavlink_terrain_report_t>(); log.Info("received TERRAIN_REPORT " + packet.lat / 1e7 + " " + packet.lon / 1e7 + " " + packet.loaded + " " + packet.pending); } return false; }
public static void ReadOurairports(string fn) { string[] lines = File.ReadAllLines(fn); foreach (var line in lines) { string[] items = line.Split(','); if (items.Length == 0) continue; if (items[0] == "\"id\"") continue; try { string name = items[3]; int latOffset = 0; while (name[0] == '"' && name[name.Length - 1] != '"') { latOffset += 1; name = name + "," + items[3 + latOffset]; } name.Trim('"'); double lat = double.Parse(items[4 + latOffset].Trim('"'), CultureInfo.InvariantCulture); double lng = double.Parse(items[5 + latOffset].Trim('"'), CultureInfo.InvariantCulture); double alt = 0; //double alt = double.Parse(items[6 + latOffset].Trim('"')) * 0.3048; var newap = new PointLatLngAlt(lat, lng, alt, name); airports.Add(newap); Console.WriteLine(newap); } catch { } } }
public static List<PointLatLngAlt> getAirports(PointLatLngAlt centerpoint) { lock (locker) { DateTime start = DateTime.Now; //log.Info("getAirports " + centerpoint); // check if we have moved 66% from our last cache center point if (currentcenter.GetDistance(centerpoint) < ((proximity / 3) * 2)) { if (!newairports) return cache; } newairports = false; log.Info("getAirports - regen list"); // generate a new list currentcenter = centerpoint; cache.Clear(); foreach (PointLatLngAlt item in airports) { if (item.GetDistance(centerpoint) < proximity) { cache.Add(item); } } log.Info("getAirports done " + (DateTime.Now - start).TotalSeconds + " sec"); return cache; } }
public void ProcessClient(object clientobj) { var client = clientobj as TcpClient; using (client) { 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.myhud.streamjpgenable = true; NetworkStream stream = client.GetStream(); // 5 seconds - default for httpd 2.2+ stream.ReadTimeout = 5000; goto skipagain; again: log.Info("doing Again"); skipagain: var asciiEncoding = new ASCIIEncoding(); var request = new byte[1024 * 4]; int len = 0; // handle header try { len = stream.Read(request, 0, request.Length); } catch { return; } string head = System.Text.Encoding.ASCII.GetString(request, 0, len); log.Info(head); int index = head.IndexOf('\n'); if (index == -1) { return; } 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", ""); //Tracking.AddEvent("HTTPServer", "Get", url, ""); ///////////////////////////////////////////////////////////////// if (url.Contains(" /websocket/server")) { 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: Mission Planner"); writer.WriteLine(""); writer.Flush(); while (client.Connected) { while (client.Available > 0) { var bydata = stream.ReadByte(); Console.Write(bydata.ToString("X2")); if (bydata == 0x88) { return; } } byte[] packet = new byte[1024 * 32]; var cs = JsonConvert.SerializeObject(MainV2.comPort.MAV.cs); var wps = JsonConvert.SerializeObject(MainV2.comPort.MAV.wps); foreach (var sendme in new[] { cs, wps }) { int i = 0; var tosend = sendme.Length; packet[i++] = 0x81; // fin - utf if (tosend <= 125) { packet[i++] = (byte)(tosend); } else { packet[i++] = 126; // nomask - 2 byte length packet[i++] = (byte)(tosend >> 8); packet[i++] = (byte)(tosend & 0xff); } foreach (char ch in sendme) { packet[i++] = (byte)ch; } stream.Write(packet, 0, i); stream.Flush(); } Thread.Sleep(200); } } } ///////////////////////////////////////////////////////////////// else if (url.Contains(" /websocket/raw")) { 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/raw"); 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); if (head.Contains("Sec-WebSocket-Protocol:")) { writer.WriteLine("Sec-WebSocket-Protocol: binary"); } writer.WriteLine("Server: Mission Planner"); writer.WriteLine(""); writer.Flush(); EventHandler <MAVLink.MAVLinkMessage> action = null; action = (sender, message) => { var sendme = message.buffer; try { byte[] packet = new byte[1024 * 32]; int i = 0; var tosend = sendme.Length; packet[i++] = 0x82; // fin - data if (tosend <= 125) { packet[i++] = (byte)(tosend); } else { packet[i++] = 126; // nomask - 2 byte length packet[i++] = (byte)(tosend >> 8); packet[i++] = (byte)(tosend & 0xff); } foreach (char ch in sendme) { packet[i++] = (byte)ch; } stream.WriteAsync(packet, 0, i); stream.FlushAsync(); } catch { ((MAVLinkInterface)sender).OnPacketReceived -= action; stream.Close(); client.Close(); } }; MainV2.comPort.OnPacketReceived += action; while (client.Connected) { while (client.Available > 0) { var bydata = stream.ReadByte(); Console.Write(bydata.ToString("X2")); if (bydata == 0x88) { return; } } Thread.Sleep(200); } MainV2.comPort.OnPacketReceived -= action; } } ///////////////////////////////////////////////////////////////// else if (url.Contains(" /georefnetwork.kml")) { byte[] buffer = Encoding.ASCII.GetBytes(georefkml); string header = "HTTP/1.1 200 OK\r\nServer: here\r\nKeep-Alive: timeout=15, max=100\r\nConnection: Keep-Alive\r\nCache-Control: no-cache\r\nContent-Type: application/vnd.google-earth.kml+xml\r\nX-Pad: avoid browser bug\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); stream.Flush(); goto again; //stream.Close(); } ///////////////////////////////////////////////////////////////// else if (url.Contains(" /location.kml")) { SharpKml.Dom.Document kml = new SharpKml.Dom.Document(); foreach (var mavLinkInterface in MainV2.Comports) { foreach (var MAV in mavLinkInterface.MAVlist) { SharpKml.Dom.Placemark pmplane = new SharpKml.Dom.Placemark(); pmplane.Name = "P/Q " + MAV.cs.altasl; pmplane.Visibility = true; SharpKml.Dom.Location loc = new SharpKml.Dom.Location(); loc.Latitude = MAV.cs.lat; loc.Longitude = MAV.cs.lng; loc.Altitude = MAV.cs.altasl; if (loc.Altitude < 0) { loc.Altitude = 0.01; } SharpKml.Dom.Orientation ori = new SharpKml.Dom.Orientation(); ori.Heading = MAV.cs.yaw; ori.Roll = -MAV.cs.roll; ori.Tilt = -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; kml.AddFeature(pmplane); } } SharpKml.Dom.LookAt la = new SharpKml.Dom.LookAt() { Altitude = MainV2.comPort.MAV.cs.altasl, Latitude = MainV2.comPort.MAV.cs.lat, Longitude = MainV2.comPort.MAV.cs.lng, Tilt = 80, Heading = MainV2.comPort.MAV.cs.yaw, AltitudeMode = SharpKml.Dom.AltitudeMode.Absolute, Range = 50 }; if (la.Latitude.Value != 0 && la.Longitude.Value != 0) { kml.Viewpoint = la; } 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; } else if (url.Contains(" /network.kml")) { byte[] buffer = Encoding.ASCII.GetBytes(@"<?xml version=""1.0"" encoding=""UTF-8""?> <kml xmlns=""http://www.opengis.net/kml/2.2"" xmlns:gx=""http://www.google.com/kml/ext/2.2"" xmlns:kml=""http://www.opengis.net/kml/2.2"" xmlns:atom=""http://www.w3.org/2005/Atom""> <Folder> <name> Network Links </name> <open> 1 </open> <NetworkLink> <name> View Centered Placemark</name> <open> 1 </open> <refreshVisibility> 0 </refreshVisibility> <flyToView> 1 </flyToView> <Link> <href> http://127.0.0.1:56781/location.kml</href> <refreshMode> onInterval </refreshMode> <refreshInterval> 1 </refreshInterval> <viewRefreshTime> 1 </viewRefreshTime> </Link> </NetworkLink> <NetworkLink> <name> View Centered Placemark</name> <open> 1 </open> <refreshVisibility> 0 </refreshVisibility> <flyToView> 0 </flyToView> <Link> <href> http://127.0.0.1:56781/wps.kml</href> </Link> </NetworkLink> </Folder> </kml>"); 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); stream.Flush(); goto again; } else if (url.Contains(" /wps.kml")) { SharpKml.Dom.Document kml = new SharpKml.Dom.Document(); SharpKml.Dom.CoordinateCollection coords = new SharpKml.Dom.CoordinateCollection(); PointLatLngAlt home = null; // draw track try { foreach (var point in GCSViews.FlightPlanner.instance.pointlist) { if (point == null) { continue; } if (point.Tag.ToLower().Contains("home")) { home = point; } coords.Add(new SharpKml.Base.Vector(point.Lat, point.Lng, point.Alt)); } } catch { } var altmode = SharpKml.Dom.AltitudeMode.Absolute; foreach (var point in GCSViews.FlightPlanner.instance.pointlist) { if (point == null) { continue; } SharpKml.Dom.Placemark wp = new SharpKml.Dom.Placemark(); wp.Name = "WP " + point.Tag + " Alt: " + point.Alt; SharpKml.Dom.Point wppoint = new SharpKml.Dom.Point(); wppoint.AltitudeMode = altmode; wppoint.Coordinate = new Vector() { Latitude = point.Lat, Longitude = point.Lng, Altitude = point.Alt }; wp.Geometry = wppoint; kml.AddFeature(wp); } SharpKml.Dom.LineString ls = new SharpKml.Dom.LineString(); ls.AltitudeMode = altmode; ls.Coordinates = coords; ls.Extrude = false; ls.Tessellate = true; Style style = new Style(); style.Id = "yellowLineGreenPoly"; unchecked { style.Line = new LineStyle(new Color32((int)0xff00ffff), 4); } Style style2 = new Style(); style2.Id = "yellowLineGreenPoly"; style2.Line = new LineStyle(new Color32((int)0x7f00ffff), 4); // above ground SharpKml.Dom.Placemark pm = new SharpKml.Dom.Placemark() { Geometry = ls, Name = "WPs", StyleSelector = style }; kml.AddFeature(pm); // on ground SharpKml.Dom.LineString ls2 = new SharpKml.Dom.LineString(); ls2.Coordinates = coords; ls2.Extrude = false; ls2.Tessellate = true; ls2.AltitudeMode = SharpKml.Dom.AltitudeMode.ClampToGround; SharpKml.Dom.Placemark pm2 = new SharpKml.Dom.Placemark() { Geometry = ls2, Name = "onground", StyleSelector = style2 }; kml.AddFeature(pm2); 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); stream.Flush(); goto again; //stream.Close(); } ///////////////////////////////////////////////////////////////// else if (url.Contains(" /hud.html")) { var file = Xamarin.Properties.Resources.hud; string header = "HTTP/1.1 200 OK\r\nConnection: close\r\nContent-Type: text/html\r\nContent-Length: " + file.Length + "\r\n\r\n"; byte[] temp = asciiEncoding.GetBytes(header); stream.Write(temp, 0, temp.Length); byte[] buffer = ASCIIEncoding.ASCII.GetBytes(file); stream.Write(buffer, 0, buffer.Length); 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("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}} */ object[] data = new object[20]; if (MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.ATTITUDE) != null) { var tmsg = MainV2.comPort.MAV.getPacket((uint)MAVLink.MAVLINK_MSG_ID.ATTITUDE) .ToStructure <MAVLink.mavlink_attitude_t>(); var json = JsonConvert.SerializeObject(tmsg); var name = MAVLink.MAVLINK_MESSAGE_INFOS.GetMessageInfo((uint)MAVLink.MAVLINK_MSG_ID.ATTITUDE).name; } Messagejson message = new Messagejson(); if (MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.ATTITUDE) != null) { message.ATTITUDE = new Message2() { index = 1, msg = MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.ATTITUDE) .ToStructure <MAVLink.mavlink_attitude_t>() } } ; if (MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.VFR_HUD) != null) { message.VFR_HUD = new Message2() { index = 1, msg = MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.VFR_HUD) .ToStructure <MAVLink.mavlink_vfr_hud_t>() } } ; if (MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.NAV_CONTROLLER_OUTPUT) != null) { message.NAV_CONTROLLER_OUTPUT = new Message2() { index = 1, msg = MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.NAV_CONTROLLER_OUTPUT) .ToStructure <MAVLink.mavlink_nav_controller_output_t>() } } ; if (MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.GPS_RAW_INT) != null) { message.GPS_RAW_INT = new Message2() { index = 1, msg = MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.GPS_RAW_INT) .ToStructure <MAVLink.mavlink_gps_raw_int_t>() } } ; if (MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.HEARTBEAT) != null) { message.HEARTBEAT = new Message2() { index = 1, msg = MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.HEARTBEAT) .ToStructure <MAVLink.mavlink_heartbeat_t>() } } ; if (MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.GPS_STATUS) != null) { message.GPS_STATUS = new Message2() { index = 1, msg = MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.GPS_STATUS) .ToStructure <MAVLink.mavlink_gps_status_t>() } } ; if (MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.STATUSTEXT) != null) { message.STATUSTEXT = new Message2() { index = 1, msg = MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.STATUSTEXT) .ToStructure <MAVLink.mavlink_statustext_t>() } } ; if (MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.SYS_STATUS) != null) { message.SYS_STATUS = new Message2() { index = 1, msg = MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.SYS_STATUS) .ToStructure <MAVLink.mavlink_sys_status_t>() } } ; message.META_LINKQUALITY = message.SYS_STATUS = new Message2() { index = packetindex, time_usec = 0, msg = new META_LINKQUALITY() { master_in = (int)MainV2.comPort.MAV.packetsnotlost, mavpackettype = "META_LINKQUALITY", master_out = MainV2.comPort.packetcount, packet_loss = 100 - MainV2.comPort.MAV.cs.linkqualitygcs, mav_loss = 0 } }; packetindex++; string output = JsonConvert.SerializeObject(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); stream.Flush(); 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 = WebUtility.UrlDecode(match.Groups[2].Value); fileurl = fileurl.Replace("/mav/", ""); if (fileurl == "" || fileurl == "/") { fileurl = "index.html"; } if (File.Exists(mavelous_web + fileurl)) { string header = "HTTP/1.1 200 OK\r\n"; if (fileurl.Contains(".htm")) { header += "Content-Type: text/html\r\n"; } else if (fileurl.Contains(".js")) { header += "Content-Type: application/x-javascript\r\n"; } else if (fileurl.Contains(".css")) { header += "Content-Type: text/css\r\n"; } else { header += "Content-Type: text/plain\r\n"; } var fileinfo = new FileInfo(mavelous_web + fileurl); var filetime = fileinfo.LastWriteTimeUtc.ToString("ddd, dd MMM yyyy HH:mm:ss") + " GMT"; var modified = Regex.Match(head, "If-Modified-Since:(.*)", RegexOptions.IgnoreCase); if (modified.Success && modified.Groups[1].Value.Trim().ToLower() == filetime.ToLower()) { string header2 = "HTTP/1.1 304 not modified\r\nConnection: Keep-Alive\r\n\r\n"; byte[] temp2 = asciiEncoding.GetBytes(header2); stream.Write(temp2, 0, temp2.Length); stream.Flush(); goto again; } header += "Connection: keep-alive\r\nLast-Modified: " + filetime + "\r\nContent-Length: " + fileinfo.Length + "\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.Flush(); goto again; } } ///////////////////////////////////////////////////////////////// { string header = "HTTP/1.1 404 not found\r\nConnection: Keep-Alive\r\nContent-Length: 0\r\nContent -Type: text/plain\r\n\r\n"; byte[] temp = asciiEncoding.GetBytes(header); stream.Write(temp, 0, temp.Length); stream.Flush(); goto again; } } ///////////////////////////////////////////////////////////////// else if (url.ToLower().Contains(" / ")) { Console.WriteLine(url); string header = "HTTP/1.1 200 OK\r\nConnection: close\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> <a href=/georefnetwork.kml>georef kml</a> "; temp = asciiEncoding.GetBytes(content); stream.Write(temp, 0, temp.Length); } ///////////////////////////////////////////////////////////////// else { string header = "HTTP/1.1 404 not found\r\nContent-Type: text/plain\r\n\r\n"; byte[] temp = asciiEncoding.GetBytes(header); stream.Write(temp, 0, temp.Length); } stream.Close(); log.Info("Close http " + url); client.Close(); } catch (Exception ee) { log.Error("Failed http ", ee); } } }
public bool LoadFile(string filename) { FileName = filename; log.InfoFormat("GeoTiff {0}", filename); using (Tiff tiff = Tiff.Open(filename, "r")) { width = tiff.GetField(TiffTag.IMAGEWIDTH)[0].ToInt(); height = tiff.GetField(TiffTag.IMAGELENGTH)[0].ToInt(); bits = tiff.GetField(TiffTag.BITSPERSAMPLE)[0].ToInt(); //https://www.awaresystems.be/imaging/tiff/tifftags/sampleformat.html type = tiff.GetField(TiffTag.SAMPLEFORMAT)[0].ToInt(); modelscale = tiff.GetField(TiffTag.GEOTIFF_MODELPIXELSCALETAG); tiepoint = tiff.GetField(TiffTag.GEOTIFF_MODELTIEPOINTTAG); var GeoKeyDirectoryTag = tiff.GetField((TiffTag)34735); var KeyDirectoryVersion = BitConverter.ToUInt16(GeoKeyDirectoryTag[1].ToByteArray(), 0); var KeyRevision = BitConverter.ToUInt16(GeoKeyDirectoryTag[1].ToByteArray(), 2); var MinorRevision = BitConverter.ToUInt16(GeoKeyDirectoryTag[1].ToByteArray(), 4); var NumberOfKeys = BitConverter.ToUInt16(GeoKeyDirectoryTag[1].ToByteArray(), 6); ProjectedCSTypeGeoKey = 0; for (int i = 8; i < 8 + NumberOfKeys * 8; i += 8) { var KeyID = BitConverter.ToUInt16(GeoKeyDirectoryTag[1].ToByteArray(), i); var TIFFTagLocation = BitConverter.ToUInt16(GeoKeyDirectoryTag[1].ToByteArray(), i + 2); var Count = BitConverter.ToUInt16(GeoKeyDirectoryTag[1].ToByteArray(), i + 4); var Value_Offset = BitConverter.ToUInt16(GeoKeyDirectoryTag[1].ToByteArray(), i + 6); log.InfoFormat("GeoKeyDirectoryTag ID={0} TagLoc={1} Count={2} Value/offset={3}", (GKID)KeyID, TIFFTagLocation, Count, Value_Offset); if (KeyID == (int)GKID.ProjectedCSTypeGeoKey) { ProjectedCSTypeGeoKey = Value_Offset; } if (KeyID == (int)GKID.GTRasterTypeGeoKey) { GTRasterTypeGeoKey = Value_Offset; } if (TIFFTagLocation != 0) { if (TIFFTagLocation == 34737) { var value = tiff.GetField((TiffTag)TIFFTagLocation)[1].ToByteArray().Skip(Value_Offset) .Take(Count); log.InfoFormat("GeoKeyDirectoryTag ID={0} Value={1}", (GKID)KeyID, Encoding.ASCII.GetString(value.ToArray())); } if (TIFFTagLocation == 34736) { /* * var value = tiff.GetField((TiffTag)TIFFTagLocation)[1].ToByteArray().Skip(Value_Offset*8) * .Take(Count*8); * log.InfoFormat("GeoKeyDirectoryTag ID={0} Value={1}", (GKID) KeyID, value); */ } } } GeoAsciiParamsTag = tiff.GetField((TiffTag)34737); if (GeoAsciiParamsTag != null && GeoAsciiParamsTag.Length == 2) { log.InfoFormat("GeoAsciiParamsTag {0}", GeoAsciiParamsTag[1]); } i = BitConverter.ToDouble(tiepoint[1].ToByteArray(), 0); j = BitConverter.ToDouble(tiepoint[1].ToByteArray(), 0 + 8); k = BitConverter.ToDouble(tiepoint[1].ToByteArray(), 0 + 16); x = BitConverter.ToDouble(tiepoint[1].ToByteArray(), 0 + 24); y = BitConverter.ToDouble(tiepoint[1].ToByteArray(), 0 + 32); z = BitConverter.ToDouble(tiepoint[1].ToByteArray(), 0 + 40); log.InfoFormat("Tie Point ({0},{1},{2}) --> ({3},{4},{5})", i, j, k, x, y, z); xscale = BitConverter.ToDouble(modelscale[1].ToByteArray(), 0); yscale = BitConverter.ToDouble(modelscale[1].ToByteArray(), 0 + 8); zscale = BitConverter.ToDouble(modelscale[1].ToByteArray(), 0 + 16); log.InfoFormat("Scale ({0},{1},{2})", xscale, yscale, zscale); // wgs84 utm if (ProjectedCSTypeGeoKey >= 32601 && ProjectedCSTypeGeoKey <= 32760) { if (ProjectedCSTypeGeoKey > 32700) { UTMZone = (ProjectedCSTypeGeoKey - 32700) * -1; var pnt = PointLatLngAlt.FromUTM(UTMZone, x, y); var pnt2 = PointLatLngAlt.FromUTM(UTMZone, x + width * xscale, y + height * yscale); var pnt3 = PointLatLngAlt.FromUTM(UTMZone, x + width * xscale, y); var pnt4 = PointLatLngAlt.FromUTM(UTMZone, x, y + height * yscale); ymin = Math.Min(Math.Min(Math.Min(pnt.Lat, pnt2.Lat), pnt3.Lat), pnt4.Lat); xmin = Math.Min(Math.Min(Math.Min(pnt.Lng, pnt2.Lng), pnt3.Lng), pnt4.Lng); ymax = Math.Max(Math.Max(Math.Max(pnt.Lat, pnt2.Lat), pnt3.Lat), pnt4.Lat); xmax = Math.Max(Math.Max(Math.Max(pnt.Lng, pnt2.Lng), pnt3.Lng), pnt4.Lng); } if (ProjectedCSTypeGeoKey < 32700) { UTMZone = ProjectedCSTypeGeoKey - 32600; var pnt = PointLatLngAlt.FromUTM(UTMZone, x, y); var pnt2 = PointLatLngAlt.FromUTM(UTMZone, x + width * xscale, y - height * yscale); var pnt3 = PointLatLngAlt.FromUTM(UTMZone, x + width * xscale, y); var pnt4 = PointLatLngAlt.FromUTM(UTMZone, x, y - height * yscale); ymin = Math.Min(Math.Min(Math.Min(pnt.Lat, pnt2.Lat), pnt3.Lat), pnt4.Lat); xmin = Math.Min(Math.Min(Math.Min(pnt.Lng, pnt2.Lng), pnt3.Lng), pnt4.Lng); ymax = Math.Max(Math.Max(Math.Max(pnt.Lat, pnt2.Lat), pnt3.Lat), pnt4.Lat); xmax = Math.Max(Math.Max(Math.Max(pnt.Lng, pnt2.Lng), pnt3.Lng), pnt4.Lng); } } // etrs89 utm if (ProjectedCSTypeGeoKey >= 3038 && ProjectedCSTypeGeoKey <= 3051) { UTMZone = ProjectedCSTypeGeoKey - 3012; // 3038 - 26 var pnt = PointLatLngAlt.FromUTM(UTMZone, x, y); var pnt2 = PointLatLngAlt.FromUTM(UTMZone, x + width * xscale, y - height * yscale); var pnt3 = PointLatLngAlt.FromUTM(UTMZone, x + width * xscale, y); var pnt4 = PointLatLngAlt.FromUTM(UTMZone, x, y - height * yscale); ymin = Math.Min(Math.Min(Math.Min(pnt.Lat, pnt2.Lat), pnt3.Lat), pnt4.Lat); xmin = Math.Min(Math.Min(Math.Min(pnt.Lng, pnt2.Lng), pnt3.Lng), pnt4.Lng); ymax = Math.Max(Math.Max(Math.Max(pnt.Lat, pnt2.Lat), pnt3.Lat), pnt4.Lat); xmax = Math.Max(Math.Max(Math.Max(pnt.Lng, pnt2.Lng), pnt3.Lng), pnt4.Lng); } if (ProjectedCSTypeGeoKey >= 25828 && ProjectedCSTypeGeoKey <= 25838) { UTMZone = ProjectedCSTypeGeoKey - 25800; // 3038 - 26 var pnt = PointLatLngAlt.FromUTM(UTMZone, x, y); var pnt2 = PointLatLngAlt.FromUTM(UTMZone, x + width * xscale, y - height * yscale); var pnt3 = PointLatLngAlt.FromUTM(UTMZone, x + width * xscale, y); var pnt4 = PointLatLngAlt.FromUTM(UTMZone, x, y - height * yscale); ymin = Math.Min(Math.Min(Math.Min(pnt.Lat, pnt2.Lat), pnt3.Lat), pnt4.Lat); xmin = Math.Min(Math.Min(Math.Min(pnt.Lng, pnt2.Lng), pnt3.Lng), pnt4.Lng); ymax = Math.Max(Math.Max(Math.Max(pnt.Lat, pnt2.Lat), pnt3.Lat), pnt4.Lat); xmax = Math.Max(Math.Max(Math.Max(pnt.Lng, pnt2.Lng), pnt3.Lng), pnt4.Lng); } Area = new RectLatLng(ymax, xmin, xmax - xmin, ymax - ymin); log.InfoFormat("Coverage {0}", Area.ToString()); if (GTRasterTypeGeoKey == 1) { // starts from top left so x + y - x += xscale / 2.0; y -= yscale / 2.0; } log.InfoFormat("Start Point ({0},{1},{2}) --> ({3},{4},{5})", i, j, k, x, y, z); lock (index) GeoTiff.index.Add(this); /* * * short numberOfDirectories = tiff.NumberOfDirectories(); * for (short d = 0; d < numberOfDirectories; ++d) * { * tiff.SetDirectory((short)d); * * for (ushort t = ushort.MinValue; t < ushort.MaxValue; ++t) * { * TiffTag tag = (TiffTag)t; * FieldValue[] value = tiff.GetField(tag); * if (value != null) * { * for (int j2 = 0; j2 < value.Length; j2++) * { * Console.WriteLine("{0} : {1} : {2}", tag.ToString(), value[j2].Value.GetType().ToString(), value[j2].ToString()); * } * } * } * } */ } return(true); }
public static srtm.altresponce getAltitude(double lat, double lng, double zoom = 16) { lock (index) if (index.Count == 0) { return(srtm.altresponce.Invalid); } var answer = new srtm.altresponce(); foreach (var geotiffdata in index.ToArray()) { if (geotiffdata.Area.Contains(lat, lng)) { // get answer var xf = map(lat, geotiffdata.Area.Top, geotiffdata.Area.Bottom, 0, geotiffdata.height - 1); var yf = map(lng, geotiffdata.Area.Left, geotiffdata.Area.Right, 0, geotiffdata.width - 1); //wgs84 && etrs89 if (geotiffdata.ProjectedCSTypeGeoKey >= 3038 && geotiffdata.ProjectedCSTypeGeoKey <= 3051 || geotiffdata.ProjectedCSTypeGeoKey >= 32601 && geotiffdata.ProjectedCSTypeGeoKey <= 32760 || geotiffdata.ProjectedCSTypeGeoKey >= 25828 && geotiffdata.ProjectedCSTypeGeoKey <= 25838) { var pnt = PointLatLngAlt.ToUTM((geotiffdata.UTMZone) * 1, lat, lng); xf = map(pnt[1], geotiffdata.y, geotiffdata.y - geotiffdata.height * geotiffdata.yscale, 0, geotiffdata.height - 1); yf = map(pnt[0], geotiffdata.x, geotiffdata.x + geotiffdata.width * geotiffdata.xscale, 0, geotiffdata.width - 1); } int x_int = (int)xf; double x_frac = xf - x_int; int y_int = (int)yf; double y_frac = yf - y_int; //could be on one of the other images if (x_int < 0 || y_int < 0 || x_int >= geotiffdata.width - 1 || y_int >= geotiffdata.height - 1) { continue; } double alt00 = GetAlt(geotiffdata, x_int, y_int); double alt10 = GetAlt(geotiffdata, x_int + 1, y_int); double alt01 = GetAlt(geotiffdata, x_int, y_int + 1); double alt11 = GetAlt(geotiffdata, x_int + 1, y_int + 1); double v1 = avg(alt00, alt10, x_frac); double v2 = avg(alt01, alt11, x_frac); double v = avg(v1, v2, y_frac); if (v > -1000) { answer.currenttype = srtm.tiletype.valid; } if (alt00 < -1000 || alt10 < -1000 || alt01 < -1000 || alt11 < -1000) { answer.currenttype = srtm.tiletype.invalid; } answer.alt = v; answer.altsource = "GeoTiff"; return(answer); } } return(srtm.altresponce.Invalid); }
public List <object> LocationToFence(List <Locationwp> list) { FencePolygon pol = new FencePolygon(); foreach (var locationwp in list) { var plla = new PointLatLngAlt(locationwp); switch (locationwp.id) { case (ushort)MAVLink.MAV_CMD.FENCE_POLYGON_VERTEX_EXCLUSION: { pol.Mode = FencePolygon.PolyType.Exclusive; pol.Points.Add(plla); if (pol.Points.Count == locationwp.p1) { Fences.Add(pol); pol = new FencePolygon(); } } break; case (ushort)MAVLink.MAV_CMD.FENCE_POLYGON_VERTEX_INCLUSION: { pol.Mode = FencePolygon.PolyType.Inclusive; pol.Points.Add(plla); if (pol.Points.Count == locationwp.p1) { Fences.Add(pol); pol = new FencePolygon(); } } break; case (ushort)MAVLink.MAV_CMD.FENCE_CIRCLE_EXCLUSION: { var cir = new FenceCircle() { Mode = FenceCircle.PolyType.Exclusive, Center = plla, Radius = locationwp.p1 }; Fences.Add(cir); } break; case (ushort)MAVLink.MAV_CMD.FENCE_CIRCLE_INCLUSION: { var cir = new FenceCircle() { Mode = FenceCircle.PolyType.Inclusive, Center = plla, Radius = locationwp.p1 }; Fences.Add(cir); } break; case (ushort)MAVLink.MAV_CMD.FENCE_RETURN_POINT: Fences.Add(new FenceReturn() { Return = plla }); break; default: break; } } return(Fences); }
public static void StartTimer() { if (tmr.Enabled || timerRenable.Enabled) { return; } stop = false; timerRenable.Interval = 60000; timerRenable.Tick += (snd, obj) => { tmr.Enabled = true; timerRenable.Enabled = false; }; tmr.Interval = 3000; tmr.Tick += (snd, obj) => { //if (MainV2.comPort == null || !MainV2.comPort.BaseStream.IsOpen) return; if (MainV2.comPort.MAV == null || MainV2.comPort.MAV.cs == null) { return; } if (MainV2.comPort.MAV.cs.Location == null) { return; } if (MainV2.comPort.MAV.cs.Location.Lat == 0 && MainV2.comPort.MAV.cs.Location.Lng == 0) { return; } //if (MainV2.comPort.MAV.cs.gpsstatus <= 3) return; System.ComponentModel.BackgroundWorker bgWorker = new System.ComponentModel.BackgroundWorker(); bool error = false; tmr.Enabled = timerRenable.Enabled = false; bgWorker.DoWork += (sender, obj2) => { try { //http://public-api.adsbexchange.com/VirtualRadar/AircraftList.json?lat=33.433638&lng=-112.008113&fDstL=0&fDstU=37.04 using (var client = new System.Net.WebClient()) { string request = string.Format("http://public-api.adsbexchange.com/VirtualRadar/AircraftList.json?lat={0}&lng={1}&fDstL=0&fDstU=37.04", MainV2.comPort.MAV.cs.Location.Lat, MainV2.comPort.MAV.cs.Location.Lng); if (!string.IsNullOrEmpty(ldv)) { request = string.Format("http://public-api.adsbexchange.com/VirtualRadar/AircraftList.json?lat={0}&lng={1}&fDstL=0&fDstU=37.04&ldv={2}", MainV2.comPort.MAV.cs.Location.Lat, MainV2.comPort.MAV.cs.Location.Lng, ldv); } var json = client.DownloadString(request); var serializer = new System.Web.Script.Serialization.JavaScriptSerializer(); RootObject adsbexchange = serializer.Deserialize <RootObject>(json); // TODO: do something with the model //System.Diagnostics.Debug.WriteLine("Found " + adsbexchange.acList.Count + " aircraft within 20nm"); if (adsbexchange == null) { return; } if (!string.IsNullOrEmpty(adsbexchange.lastDv)) { ldv = adsbexchange.lastDv; } lock (MainV2.instance.adsbPlanes) { float multi = Settings.Instance["distunits"] == "Feet" ? 3.28084f : 1f; foreach (AcList vehicle in adsbexchange.acList) { string id = vehicle.Id.ToString(); //vehicle alt is always feet, if set to meters change //GAlt is altitude adjusted for local air pressure PointLatLngAlt vehicleplla = new PointLatLngAlt(vehicle.Lat, vehicle.Long, vehicle.GAlt * 3.2808399f); double distance = MainV2.comPort.MAV.cs.Location.GetDistance(vehicleplla) * 0.000539957; double altsep = vehicleplla.Alt - (MainV2.comPort.MAV.cs.altasl * multi); if (MainV2.instance.adsbPlanes.ContainsKey(id)) { // update existing MainV2.instance.adsbPlanes[id].Lat = vehicle.Lat; MainV2.instance.adsbPlanes[id].Lng = vehicle.Long; MainV2.instance.adsbPlanes[id].Alt = vehicleplla.Alt; MainV2.instance.adsbPlanes[id].Heading = (float)vehicle.Trak; MainV2.instance.adsbPlanes[id].Time = DateTime.Now; MainV2.instance.adsbPlanes[id].CallSign = vehicle.Call; MainV2.instance.adsbPlanes[id].Tag = vehicle.Icao; // + Environment.NewLine + species + "/" + vehicle.Call + Environment.NewLine + vehicle.Mdl; MainV2.instance.adsbPlanes[id].Vsi = vehicle.Vsi; MainV2.instance.adsbPlanes[id].Species = (adsb.ADSBSpecies)vehicle.Species; MainV2.instance.adsbPlanes[id].type = vehicle.Type; } else { // create new plane MainV2.instance.adsbPlanes[id] = new adsb.PointLatLngAltHdg(vehicle.Lat, vehicle.Long, vehicleplla.Alt, (float)vehicle.Trak, id, DateTime.Now) { CallSign = vehicle.Call, Tag = vehicle.Icao, DisplayICAO = true, Vsi = vehicle.Vsi, Species = (adsb.ADSBSpecies)vehicle.Species, type = vehicle.Type }; } if (distance <= 2 && altsep <= 152.4 && altsep >= -152.4) //closer than 2NM and verticle seperation <= 500 feet { MainV2.instance.adsbPlanes[id].ThreatLevel = MAVLink.MAV_COLLISION_THREAT_LEVEL.HIGH; } else if (distance <= 5 && altsep <= 457.2 && altsep >= -457.2) //greater than 2NM but less than 5NM and verticle seperation <= 1500 feet { MainV2.instance.adsbPlanes[id].ThreatLevel = MAVLink.MAV_COLLISION_THREAT_LEVEL.LOW; } else { MainV2.instance.adsbPlanes[id].ThreatLevel = MAVLink.MAV_COLLISION_THREAT_LEVEL.NONE; } try { //send adsb to mav if (MainV2.comPort.BaseStream.IsOpen) { MAVLink.mavlink_adsb_vehicle_t packet = new MAVLink.mavlink_adsb_vehicle_t(); packet.altitude = (int)(MainV2.instance.adsbPlanes[id].Alt * 1000); packet.altitude_type = (byte)MAVLink.ADSB_ALTITUDE_TYPE.GEOMETRIC; packet.callsign = ASCIIEncoding.ASCII.GetBytes(MainV2.instance.adsbPlanes[id].CallSign == null ? MainV2.instance.adsbPlanes[id].Tag : MainV2.instance.adsbPlanes[id].CallSign); packet.emitter_type = (byte)MAVLink.ADSB_EMITTER_TYPE.NO_INFO; packet.heading = (ushort)(MainV2.instance.adsbPlanes[id].Heading * 100); packet.lat = (int)(MainV2.instance.adsbPlanes[id].Lat * 1e7); packet.lon = (int)(MainV2.instance.adsbPlanes[id].Lng * 1e7); packet.ICAO_address = uint.Parse(id, System.Globalization.NumberStyles.HexNumber); packet.flags = (ushort)(MAVLink.ADSB_FLAGS.VALID_ALTITUDE | MAVLink.ADSB_FLAGS.VALID_COORDS | MAVLink.ADSB_FLAGS.VALID_HEADING | MAVLink.ADSB_FLAGS.VALID_CALLSIGN); //send to current connected MainV2.comPort.sendPacket(packet, MainV2.comPort.MAV.sysid, MainV2.comPort.MAV.compid); } } catch { } } foreach (var key in MainV2.instance.adsbPlanes.Keys) { adsb.PointLatLngAltHdg tmp; if (!adsbexchange.acList.Any(x => x.Id.ToString() == key && x.Gnd == false)) { MainV2.instance.adsbPlanes.TryRemove(key, out tmp); } } } } } catch (System.Net.WebException webException) { error = true; timerRenable.Enabled = true; } catch { } }; if (!error && !stop) { tmr.Enabled = true; } bgWorker.RunWorkerAsync(); }; tmr.Enabled = true; }
public static PointLatLngAlt getIntersectionWithTerrain(PointLatLngAlt start, PointLatLngAlt end) { int distout = 0; int stepsize = 50; var maxdist = start.GetDistance(end); var bearing = start.GetBearing(end); var altdiff = end.Alt - start.Alt; PointLatLngAlt newpos = PointLatLngAlt.Zero; while (distout < maxdist) { // get a projected point to test intersection against - not using slope distance PointLatLngAlt terrainstart = start.newpos(bearing, distout); terrainstart.Alt = srtm.getAltitude(terrainstart.Lat, terrainstart.Lng).alt; // get another point stepsize infront PointLatLngAlt terrainend = start.newpos(bearing, distout + stepsize); terrainend.Alt = srtm.getAltitude(terrainend.Lat, terrainend.Lng).alt; // x is dist from start, y is alt var newpoint = FindLineIntersection(new PointF(0, (float)start.Alt), new PointF((float)maxdist, (float)end.Alt), new PointF((float)distout, (float)terrainstart.Alt), new PointF((float)distout + stepsize, (float)terrainend.Alt)); if (newpoint.X != 0) { newpos = start.newpos(bearing, newpoint.X); newpos.Alt = newpoint.Y; break; } distout += stepsize; } if (newpos == PointLatLngAlt.Zero) { newpos = end; } return(newpos); }
private PointLatLngAlt get_last_nav_coords(PointLatLngAlt lta_point, PointLatLngAlt land_point) { double bearing = land_point.GetBearing(lta_point); return(land_point.newpos(bearing, m_hwp_radius * 0.4f)); }
public static PointLatLngAlt ProjectPoint() { MainV2.comPort.GetMountStatus(); // this should be looking at rc_channel function yawchannel = (int)(float)MainV2.comPort.MAV.param["MNT_RC_IN_PAN"]; pitchchannel = (int)(float)MainV2.comPort.MAV.param["MNT_RC_IN_TILT"]; rollchannel = (int)(float)MainV2.comPort.MAV.param["MNT_RC_IN_ROLL"]; //if (!MainV2.comPort.BaseStream.IsOpen) // return PointLatLngAlt.Zero; PointLatLngAlt currentlocation = new PointLatLngAlt(MainV2.comPort.MAV.cs.lat, MainV2.comPort.MAV.cs.lng); double yawangle = MainV2.comPort.MAV.cs.campointc; double rollangle = MainV2.comPort.MAV.cs.campointb; double pitchangle = MainV2.comPort.MAV.cs.campointa; // if ((float) MainV2.comPort.MAV.param["MNT_TYPE"] == 4) { yawangle = MainV2.comPort.MAVlist[71].cs.yaw; rollangle = MainV2.comPort.MAVlist[71].cs.roll; pitchangle = MainV2.comPort.MAVlist[71].cs.pitch; } if (Math.Abs(rollangle) > 180 || yawangle == 0 && pitchangle == 0) { yawangle = ConvertPwmtoAngle(axis.yaw); //rollangle = ConvertPwmtoAngle(axis.roll); pitchangle = ConvertPwmtoAngle(axis.pitch) + MainV2.comPort.MAV.cs.pitch; pitchangle -= Math.Sin(yawangle * deg2rad) * MainV2.comPort.MAV.cs.roll; } // tan (o/a) // todo account for ground elevation change. int distout = 10; PointLatLngAlt newpos = PointLatLngAlt.Zero; //dist = Math.Tan((90 + pitchangle) * deg2rad) * (MainV2.comPort.MAV.cs.alt); while (distout < 1000) { // get a projected point to test intersection against - not using slope distance PointLatLngAlt newposdist = currentlocation.newpos(yawangle + MainV2.comPort.MAV.cs.yaw, distout); newposdist.Alt = srtm.getAltitude(newposdist.Lat, newposdist.Lng).alt; // get another point 50 infront PointLatLngAlt newposdist2 = currentlocation.newpos(yawangle + MainV2.comPort.MAV.cs.yaw, distout + 50); newposdist2.Alt = srtm.getAltitude(newposdist2.Lat, newposdist2.Lng).alt; // get the flat terrain distance out - at 0 alt double distflat = Math.Tan((90 + pitchangle) * deg2rad) * (MainV2.comPort.MAV.cs.altasl); // x is dist from plane, y is alt var newpoint = FindLineIntersection(new PointF(0,MainV2.comPort.MAV.cs.altasl), new PointF((float)distflat, 0), new PointF((float)distout, (float)newposdist.Alt), new PointF((float)distout + 50, (float)newposdist2.Alt)); if (newpoint.X != 0) { newpos = newposdist2; break; } distout += 50; } //Console.WriteLine("pitch " + pitchangle.ToString("0.000") + " yaw " + yawangle.ToString("0.000") + " dist" + dist.ToString("0.000")); //PointLatLngAlt newpos = currentlocation.newpos( yawangle + MainV2.comPort.MAV.cs.yaw, dist); //Console.WriteLine(newpos); return newpos; }
public void SetupM8P(ICommsSerial port, PointLatLngAlt basepos, int surveyindur = 0, double surveyinacc = 0, bool m8p_130plus = false) { port.BaseStream.Flush(); port.BaudRate = 9600; System.Threading.Thread.Sleep(100); // port config - 115200 - uart1 var packet = generate(0x6, 0x00, new byte[] { 0x01, 0x00, 0x00, 0x00, 0xD0, 0x08, 0x00, 0x00, 0x00, 0xC2, 0x01, 0x00, 0x23, 0x00, 0x23, 0x00, 0x00, 0x00, 0x00, 0x00 }); port.Write(packet, 0, packet.Length); System.Threading.Thread.Sleep(300); // port config - usb packet = generate(0x6, 0x00, new byte[] { 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x23, 0x00, 0x23, 0x00, 0x00, 0x00, 0x00, 0x00 }); port.Write(packet, 0, packet.Length); System.Threading.Thread.Sleep(300); port.BaseStream.Flush(); port.BaudRate = 115200; // port config - 115200 - uart1 packet = generate(0x6, 0x00, new byte[] { 0x01, 0x00, 0x00, 0x00, 0xD0, 0x08, 0x00, 0x00, 0x00, 0xC2, 0x01, 0x00, 0x23, 0x00, 0x23, 0x00, 0x00, 0x00, 0x00, 0x00 }); port.Write(packet, 0, packet.Length); System.Threading.Thread.Sleep(300); // port config - usb packet = generate(0x6, 0x00, new byte[] { 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x23, 0x00, 0x23, 0x00, 0x00, 0x00, 0x00, 0x00 }); port.Write(packet, 0, packet.Length); System.Threading.Thread.Sleep(300); // set rate to 1hz packet = generate(0x6, 0x8, new byte[] { 0xE8, 0x03, 0x01, 0x00, 0x01, 0x00 }); port.Write(packet, 0, packet.Length); System.Threading.Thread.Sleep(200); // set navmode to stationary packet = generate(0x6, 0x24, new byte[] { 0xFF, 0xFF, 0x02, 0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00 , 0xFA, 0x00, 0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 , 0x00, 0x00 }); port.Write(packet, 0, packet.Length); System.Threading.Thread.Sleep(200); // turn off all nmea for (int a = 0; a <= 0xf; a++) { if (a == 0xb || a == 0xc || a == 0xe) { continue; } turnon_off(port, 0xf0, (byte)a, 0); } // mon-ver poll_msg(port, 0xa, 0x4); // surveyin msg - for feedback turnon_off(port, 0x01, 0x3b, 1); // pvt msg - for feedback turnon_off(port, 0x01, 0x07, 1); // 1005 - 5s turnon_off(port, 0xf5, 0x05, 5); byte rate1 = 1; byte rate2 = 0; if (m8p_130plus) { rate1 = 0; rate2 = 1; } // 1074 - 1s turnon_off(port, 0xf5, 0x4a, rate2); // 1077 - 1s turnon_off(port, 0xf5, 0x4d, rate1); // 1084 - 1s turnon_off(port, 0xf5, 0x54, rate2); // 1087 - 1s turnon_off(port, 0xf5, 0x57, rate1); // 1124 - 1s turnon_off(port, 0xf5, 0x7c, rate2); // 1127 - 1s turnon_off(port, 0xf5, 0x7f, rate1); // 1230 - 5s turnon_off(port, 0xf5, 0xE6, 5); // rxm-raw/rawx - 1s turnon_off(port, 0x02, 0x15, 1); turnon_off(port, 0x02, 0x10, 1); // rxm-sfrb/sfrb - 2s turnon_off(port, 0x02, 0x13, 2); turnon_off(port, 0x02, 0x11, 2); System.Threading.Thread.Sleep(100); System.Threading.Thread.Sleep(100); if (basepos == PointLatLngAlt.Zero) { // survey in config - 60s and < 2m packet = generate(0x6, 0x71, new ubx_cfg_tmode3((uint)surveyindur, surveyinacc)); port.Write(packet, 0, packet.Length); } else { byte[] data = new ubx_cfg_tmode3(basepos.Lat, basepos.Lng, basepos.Alt); packet = generate(0x6, 0x71, data); port.Write(packet, 0, packet.Length); } System.Threading.Thread.Sleep(100); }
public void SetupM8P(ICommsSerial port, PointLatLngAlt basepos, int surveyindur = 0, double surveyinacc = 0) { // port config - 115200 - uart1 var packet = generate(0x6, 0x00, new byte[] { 0x01, 0x00, 0x00, 0x00, 0xD0, 0x08, 0x00, 0x00, 0x00, 0xC2, 0x01, 0x00, 0x23, 0x00, 0x23, 0x00, 0x00, 0x00, 0x00, 0x00 }); port.Write(packet, 0, packet.Length); System.Threading.Thread.Sleep(100); // port config - usb packet = generate(0x6, 0x00, new byte[] { 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x23, 0x00, 0x23, 0x00, 0x00, 0x00, 0x00, 0x00 }); port.Write(packet, 0, packet.Length); System.Threading.Thread.Sleep(100); // set rate to 1hz packet = generate(0x6, 0x8, new byte[] { 0xE8, 0x03, 0x01, 0x00, 0x01, 0x00 }); port.Write(packet, 0, packet.Length); System.Threading.Thread.Sleep(100); // turn off all nmea for (int a = 0; a <= 0xf; a++) { turnon_off(port, 0xf0, (byte)a, 0); } // surveyin msg - for feedback turnon_off(port, 0x01, 0x3b, 1); // pvt msg - for feedback turnon_off(port, 0x01, 0x07, 1); // 1005 - 5s turnon_off(port, 0xf5, 0x05, 5); // 1077 - 1s turnon_off(port, 0xf5, 0x4d, 1); // 1087 - 1s turnon_off(port, 0xf5, 0x57, 1); // rxm-raw/rawx - 1s turnon_off(port, 0x02, 0x15, 1); turnon_off(port, 0x02, 0x10, 1); // rxm-sfrb/sfrb - 5s turnon_off(port, 0x02, 0x13, 5); turnon_off(port, 0x02, 0x11, 5); System.Threading.Thread.Sleep(100); System.Threading.Thread.Sleep(100); if (basepos == PointLatLngAlt.Zero) { // survey in config - 60s and < 2m packet = generate(0x6, 0x71, new ubx_cfg_tmode3(60, 2)); port.Write(packet, 0, packet.Length); } else { byte[] data = new ubx_cfg_tmode3(basepos.Lat, basepos.Lng, basepos.Alt); packet = generate(0x6, 0x71, data); port.Write(packet, 0, packet.Length); } System.Threading.Thread.Sleep(100); }
public bool setFencePoint(byte index, PointLatLngAlt plla, byte fencepointcount) { mavlink_fence_point_t fp = new mavlink_fence_point_t(); fp.idx = index; fp.count = fencepointcount; fp.lat = (float) plla.Lat; fp.lng = (float) plla.Lng; fp.target_component = MAV.compid; fp.target_system = MAV.sysid; int retry = 3; PointLatLngAlt newfp; while (retry > 0) { generatePacket((byte) MAVLINK_MSG_ID.FENCE_POINT, fp); int counttemp = 0; newfp = getFencePoint(fp.idx, ref counttemp); if (newfp.GetDistance(plla) < 5) return true; retry--; } throw new Exception("Could not verify GeoFence Point"); }
private PointLatLngAlt get_loiter_coords(PointLatLngAlt wp_point, PointLatLngAlt land_point) { double bearing = land_point.GetBearing(wp_point); return(land_point.newpos(bearing, m_hwp_radius)); }
//http://www.chrobotics.com/library/understanding-euler-angles // x = north / roll // y = pitch / east // z = yaw / down public static List <PointLatLngAlt> calc(PointLatLngAlt plla, double R, double P, double Y, double hfov, double vfov) { // alt should be asl // P pitch where the center os pointing ie -80 // R roll GMapPolygon poly = new GMapPolygon(new List <PointLatLng>(), "rect"); double frontangle = (P * 0) + vfov / 2; double backangle = (P * 0) - vfov / 2; double leftangle = (R * 0) + hfov / 2; double rightangle = (R * 0) - hfov / 2; var fovh = Math.Tan(hfov / 2.0 * deg2rad) * 2.0; var fovv = Math.Tan(vfov / 2.0 * deg2rad) * 2.0; //DoDebug(); addtomap(plla, "plane"); Matrix3 dcm = new Matrix3(); dcm.from_euler(R * deg2rad, P * deg2rad, Y * deg2rad); dcm.normalize(); Vector3 center1 = new Vector3(0, 0, 10000); var test = dcm * center1; var bearing2 = Math.Atan2(test.y, test.x) * rad2deg; var newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y)); newpos2.Alt -= test.z; var cen = calcIntersection(plla, newpos2); var dist = plla.GetDistance(cen); addtomap(cen, "center " + dist.ToString("0")); // dcm.from_euler(R * deg2rad, P * deg2rad, Y * deg2rad); dcm.rotate(new Vector3(rightangle * deg2rad, 0, 0)); dcm.normalize(); dcm.rotate(new Vector3(0, frontangle * deg2rad, 0)); dcm.normalize(); test = dcm * center1; bearing2 = (Math.Atan2(test.y, test.x) * rad2deg); newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y)); newpos2.Alt -= test.z; //addtomap(newpos2, "tr2"); var groundpointtr = calcIntersection(plla, newpos2); poly.Points.Add(groundpointtr); addtomap(groundpointtr, "tr"); // dcm.from_euler(R * deg2rad, P * deg2rad, Y * deg2rad); dcm.rotate(new Vector3(leftangle * deg2rad, 0, 0)); dcm.normalize(); dcm.rotate(new Vector3(0, frontangle * deg2rad, 0)); dcm.normalize(); test = dcm * center1; bearing2 = Math.Atan2(test.y, test.x) * rad2deg; newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y)); newpos2.Alt -= test.z; var groundpointtl = calcIntersection(plla, newpos2); poly.Points.Add(groundpointtl); addtomap(groundpointtl, "tl"); // dcm.from_euler(R * deg2rad, P * deg2rad, Y * deg2rad); dcm.rotate(new Vector3(leftangle * deg2rad, 0, 0)); dcm.normalize(); dcm.rotate(new Vector3(0, backangle * deg2rad, 0)); dcm.normalize(); test = dcm * center1; bearing2 = Math.Atan2(test.y, test.x) * rad2deg; newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y)); newpos2.Alt -= test.z; var groundpointbl = calcIntersection(plla, newpos2); poly.Points.Add(groundpointbl); addtomap(groundpointbl, "bl"); // dcm.from_euler(R * deg2rad, P * deg2rad, Y * deg2rad); dcm.rotate(new Vector3(rightangle * deg2rad, 0, 0)); dcm.normalize(); dcm.rotate(new Vector3(0, backangle * deg2rad, 0)); dcm.normalize(); test = dcm * center1; bearing2 = Math.Atan2(test.y, test.x) * rad2deg; newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y)); newpos2.Alt -= test.z; var groundpointbr = calcIntersection(plla, newpos2); poly.Points.Add(groundpointbr); addtomap(groundpointbr, "br"); // polygons.Polygons.Clear(); polygons.Polygons.Add(poly); var ans = new List <PointLatLngAlt>(); ans.Add(groundpointtl); ans.Add(groundpointtr); ans.Add(groundpointbr); ans.Add(groundpointbl); return(ans); }
public static void POIAdd(PointLatLngAlt Point, string tag) { POIAdd(Point, tag, false, false); }
public void DownloadFence(MAVLinkInterface port, Action <int, string> progress = null) { var list = mav_mission.download(port, MAVLink.MAV_MISSION_TYPE.FENCE, progress); FencePolygon pol = new FencePolygon(); foreach (var locationwp in list) { var plla = new PointLatLngAlt(locationwp); switch (locationwp.id) { case (ushort)MAVLink.MAV_CMD.FENCE_POLYGON_VERTEX_EXCLUSION: { pol.Mode = FencePolygon.PolyType.Exclusive; pol.Points.Add(plla); if (pol.Points.Count == locationwp.p1) { Fences.Add(pol); pol = new FencePolygon(); } } break; case (ushort)MAVLink.MAV_CMD.FENCE_POLYGON_VERTEX_INCLUSION: { pol.Mode = FencePolygon.PolyType.Inclusive; pol.Points.Add(plla); if (pol.Points.Count == locationwp.p1) { Fences.Add(pol); pol = new FencePolygon(); } } break; case (ushort)MAVLink.MAV_CMD.FENCE_CIRCLE_EXCLUSION: { var cir = new FenceCircle() { Mode = FenceCircle.PolyType.Exclusive, Center = plla, Radius = locationwp.p1 }; Fences.Add(cir); } break; case (ushort)MAVLink.MAV_CMD.FENCE_CIRCLE_INCLUSION: { var cir = new FenceCircle() { Mode = FenceCircle.PolyType.Inclusive, Center = plla, Radius = locationwp.p1 }; Fences.Add(cir); } break; case (ushort)MAVLink.MAV_CMD.FENCE_RETURN_POINT: Fences.Add(new FenceReturn() { Return = plla }); break; default: break; } } }
private void map_MouseMove(object sender, MouseEventArgs e) { PointLatLng point = map.FromLocalToLatLng(e.X, e.Y); currentMousePosition = point; if (MouseDownStart == point) return; if (!isMouseDown) { // update mouse pos display //SetMouseDisplay(point.Lat, point.Lng, 0); } //draging if (e.Button == MouseButtons.Left && isMouseDown) { isMouseDraging = true; if (CurrentGMapMarker != null) { PointLatLng pnew = map.FromLocalToLatLng(e.X, e.Y); CurrentGMapMarker.Position = pnew; list[CurrentGMapMarkerIndex] = new PointLatLngAlt(pnew); domainUpDown1_ValueChanged(sender, e); } else // left click pan { double latdif = MouseDownStart.Lat - point.Lat; double lngdif = MouseDownStart.Lng - point.Lng; try { lock (thisLock) { map.Position = new PointLatLng(map.Position.Lat + latdif, map.Position.Lng + lngdif); } } catch { } } } }
public static async Task <List <PointLatLngAlt> > CreateGridAsync(List <PointLatLngAlt> polygon, double altitude, double distance, double spacing, double angle, double overshoot1, double overshoot2, StartPosition startpos, bool shutter, float minLaneSeparation, float leadin1, float leadin2, PointLatLngAlt HomeLocation, bool useextendedendpoint = true) { return(await Task.Run((() => CreateGrid(polygon, altitude, distance, spacing, angle, overshoot1, overshoot2, startpos, shutter, minLaneSeparation, leadin1, leadin2, HomeLocation, useextendedendpoint))) .ConfigureAwait(false)); }
public static List <PointLatLngAlt> CreateRotary(List <PointLatLngAlt> polygon, double altitude, double distance, double spacing, double angle, double overshoot1, double overshoot2, StartPosition startpos, bool shutter, float minLaneSeparation, float leadin, PointLatLngAlt HomeLocation, int clockwise_laps, bool match_spiral_perimeter) { spacing = 0; if (distance < 0.1) { distance = 0.1; } if (polygon.Count == 0) { return(new List <PointLatLngAlt>()); } List <utmpos> ans = new List <utmpos>(); // utm zone distance calcs will be done in int utmzone = polygon[0].GetUTMZone(); // utm position list List <utmpos> utmpositions = utmpos.ToList(PointLatLngAlt.ToUTM(utmzone, polygon), utmzone); // close the loop if its not already if (utmpositions[0] != utmpositions[utmpositions.Count - 1]) { utmpositions.Add(utmpositions[0]); // make a full loop } var maxlane = 200; // (Centroid(utmpositions).GetDistance(utmpositions[0]) / distance); ClipperLib.ClipperOffset clipperOffset = new ClipperLib.ClipperOffset(); clipperOffset.AddPath(utmpositions.Select(a => { return(new ClipperLib.IntPoint(a.x * 1000.0, a.y * 1000.0)); }).ToList(), ClipperLib.JoinType.jtMiter, ClipperLib.EndType.etClosedPolygon); for (int lane = 0; lane < maxlane; lane++) { List <utmpos> ans1 = new List <utmpos>(); ClipperLib.PolyTree tree = new ClipperLib.PolyTree(); clipperOffset.Execute(ref tree, (Int64)(distance * 1000.0 * -lane)); if (tree.ChildCount == 0) { break; } if (lane < clockwise_laps || clockwise_laps < 0) { ClipperLib.Clipper.ReversePaths(ClipperLib.Clipper.PolyTreeToPaths(tree)); } foreach (var treeChild in tree.Childs) { ans1 = treeChild.Contour.Select(a => new utmpos(a.X / 1000.0, a.Y / 1000.0, utmzone)) .ToList(); if (lane == 0 && clockwise_laps != 1 && match_spiral_perimeter) { ans1.Insert(0, ans1.Last <utmpos>()); // start at the last point of the first calculated lap // to make a closed polygon on the first trip around } if (lane == clockwise_laps - 1) { ans1.Add(ans1.First <utmpos>()); // revisit the first waypoint on this lap to cleanly exit the CW pattern } if (ans.Count() > 2) { var start1 = ans[ans.Count() - 1]; var end1 = ans[ans.Count() - 2]; var start2 = ans1[0]; var end2 = ans1[ans1.Count() - 1]; } ans.AddRange(ans1); } } // set the altitude on all points return(ans.Select(plla => { var a = plla.ToLLA(); a.Alt = altitude; a.Tag = "S"; return a; }).ToList()); }
//http://www.chrobotics.com/library/understanding-euler-angles // x = north / roll // y = pitch / east // z = yaw / down public static List <PointLatLngAlt> calc(PointLatLngAlt plla, double R, double P, double Y, double hfov, double vfov) { // alt should be asl // P pitch where the center os pointing ie -80 // R roll // if roll and pitch is 0, use the quick method. if (R == 0 && P == 0) { // calc fov in m on the ground at 0 alt var fovh = Math.Tan(hfov / 2.0 * deg2rad) * plla.Alt; var fovv = Math.Tan(vfov / 2.0 * deg2rad) * plla.Alt; var fovd = Math.Sqrt(fovh * fovh + fovv * fovv); // where we put our footprint var ans2 = new List <PointLatLngAlt>(); // calc bearing from center to corner var bearing1 = Math.Atan2(fovh, fovv) * rad2deg; // calc first corner point var newpos1 = plla.newpos(bearing1 + Y, Math.Sqrt(fovh * fovh + fovv * fovv)); // set alt to 0, as we used the hypot for distance and fov is based on 0 alt newpos1.Alt = 0; // calc intersection from center to new point and return ground hit point var cen1 = calcIntersection(plla, newpos1); // add to our footprint poly ans2.Add(cen1); addtomap(cen1, "cen1"); //repeat newpos1 = plla.newpos(Y - bearing1, Math.Sqrt(fovh * fovh + fovv * fovv)); newpos1.Alt = 0; cen1 = calcIntersection(plla, newpos1); ans2.Add(cen1); addtomap(cen1, "cen2"); newpos1 = plla.newpos(bearing1 + Y - 180, Math.Sqrt(fovh * fovh + fovv * fovv)); newpos1.Alt = 0; cen1 = calcIntersection(plla, newpos1); ans2.Add(cen1); addtomap(cen1, "cen3"); newpos1 = plla.newpos(Y - bearing1 - 180, Math.Sqrt(fovh * fovh + fovv * fovv)); newpos1.Alt = 0; cen1 = calcIntersection(plla, newpos1); ans2.Add(cen1); addtomap(cen1, "cen4"); addtomap(plla, "plane"); return(ans2); } GMapPolygon poly = new GMapPolygon(new List <PointLatLng>(), "rect"); double frontangle = (P * 0) + vfov / 2; double backangle = (P * 0) - vfov / 2; double leftangle = (R * 0) + hfov / 2; double rightangle = (R * 0) - hfov / 2; addtomap(plla, "plane"); Matrix3 dcm = new Matrix3(); dcm.from_euler(R * deg2rad, P * deg2rad, Y * deg2rad); dcm.normalize(); Vector3 center1 = new Vector3(0, 0, 10000); var test = dcm * center1; var bearing2 = Math.Atan2(test.y, test.x) * rad2deg; var newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y)); newpos2.Alt -= test.z; var cen = calcIntersection(plla, newpos2); var dist = plla.GetDistance(cen); addtomap(cen, "center " + dist.ToString("0")); // dcm.from_euler(R * deg2rad, P * deg2rad, Y * deg2rad); dcm.rotate(new Vector3(rightangle * deg2rad, 0, 0)); dcm.normalize(); dcm.rotate(new Vector3(0, frontangle * deg2rad, 0)); dcm.normalize(); /* * var mx = new Matrix3(); * var my = new Matrix3(); * var mz = new Matrix3(); * * mx.from_euler((rightangle + R) * deg2rad, 0, 0); * my.from_euler(0, (frontangle + P) * deg2rad, 0); * mz.from_euler(0, 0, Y * deg2rad); * * printdcm(mx); * printdcm(my); * printdcm(mz); * printdcm(my * mx); * printdcm(mz * my * mx); * * test = mz * my * mx * center1; */ test = dcm * center1; bearing2 = (Math.Atan2(test.y, test.x) * rad2deg); newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y)); newpos2.Alt -= test.z; //addtomap(newpos2, "tr2"); var groundpointtr = calcIntersection(plla, newpos2); poly.Points.Add(groundpointtr); addtomap(groundpointtr, "tr"); // dcm.from_euler(R * deg2rad, P * deg2rad, Y * deg2rad); dcm.rotate(new Vector3(leftangle * deg2rad, 0, 0)); dcm.normalize(); dcm.rotate(new Vector3(0, frontangle * deg2rad, 0)); dcm.normalize(); test = dcm * center1; bearing2 = Math.Atan2(test.y, test.x) * rad2deg; newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y)); newpos2.Alt -= test.z; var groundpointtl = calcIntersection(plla, newpos2); poly.Points.Add(groundpointtl); addtomap(groundpointtl, "tl"); // dcm.from_euler(R * deg2rad, P * deg2rad, Y * deg2rad); dcm.rotate(new Vector3(leftangle * deg2rad, 0, 0)); dcm.normalize(); dcm.rotate(new Vector3(0, backangle * deg2rad, 0)); dcm.normalize(); test = dcm * center1; bearing2 = Math.Atan2(test.y, test.x) * rad2deg; newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y)); newpos2.Alt -= test.z; var groundpointbl = calcIntersection(plla, newpos2); poly.Points.Add(groundpointbl); addtomap(groundpointbl, "bl"); // dcm.from_euler(R * deg2rad, P * deg2rad, Y * deg2rad); dcm.rotate(new Vector3(rightangle * deg2rad, 0, 0)); dcm.normalize(); dcm.rotate(new Vector3(0, backangle * deg2rad, 0)); dcm.normalize(); test = dcm * center1; bearing2 = Math.Atan2(test.y, test.x) * rad2deg; newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y)); newpos2.Alt -= test.z; var groundpointbr = calcIntersection(plla, newpos2); poly.Points.Add(groundpointbr); addtomap(groundpointbr, "br"); // polygons.Polygons.Clear(); polygons.Polygons.Add(poly); var ans = new List <PointLatLngAlt>(); ans.Add(groundpointtl); ans.Add(groundpointtr); ans.Add(groundpointbr); ans.Add(groundpointbl); return(ans); }
public SightGen(PointLatLng location, List <PointLatLng> pointslist, double alt, PointLatLng aircraftlocation, double aircraft_alt) { range = Settings.Instance.GetFloat("Propagation_Range"); clearance = Settings.Instance.GetFloat("Propagation_Clearance"); base_height = Settings.Instance.GetFloat("Propagation_Height"); var converge = Settings.Instance.GetInt32("Propagation_Converge"); var rotational = Settings.Instance.GetInt32("Propagation_Rotational"); if (rotational == 0) { rotational = 1; } drone_alt = aircraft_alt; // enable rf when there is no drone connected // use home loc + max(base height , clearance) + 0.1; if (drone_alt == 0) { drone_alt = alt + Math.Max(base_height, clearance) + 0.1; } homealt = alt; var startlocation = new PointLatLngAlt(location) { Alt = homealt + base_height }; var dronelocation = new PointLatLngAlt(aircraftlocation) { Alt = drone_alt - clearance }; SortedDictionary <double, PointLatLngAlt> sortedlist = new SortedDictionary <double, PointLatLngAlt>(); Parallel.ForEach(Extensions.SteppedRange(0, 2 * (float)Math.PI, Math.PI / 180 * rotational), angle => //Terrain intercept scan full 360 { List <PointLatLngAlt> pointends = new List <PointLatLngAlt>(); var carryon = true; var up = false; var down = false; float triangle = 45 * (float)Math.PI / 180; //radians var Max = 90 * (float)Math.PI / 180; //radians float Min = 0; //radians // calc LOS vector and intersection with terrain //point = srtm.getIntersectionWithTerrain(startlocation, endlocation); PointLatLngAlt point = null; var newpos = new PointLatLngAlt(location).newpos(angle * MathHelper.rad2deg, range * 1000); pointends.Clear(); pointends.Add(location); pointends.Add(newpos); while (carryon && Min + converge * (float)Math.PI / 180 < Max) //Breaks if terrain intercept found or convergence occurs { point = getSRTMAltPath(pointends, triangle, ref carryon, ref up, ref down); //DEM data if (up) { Min = triangle; } else if (down) { Max = triangle; } triangle = (Max + Min) / 2; } lock (sortedlist) sortedlist.Add(angle, point); }); sortedlist.Values.ForEach(a => pointslist.Add(a)); pointslist.Add(pointslist[0]); pointslist.DeDupOrderedList(); }
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.myhud.streamjpgenable = true; NetworkStream stream = client.GetStream(); // 5 seconds - default for httpd 2.2+ stream.ReadTimeout = 5000; goto skipagain; again: log.Info("doing Again"); skipagain: var asciiEncoding = new ASCIIEncoding(); var request = new byte[1024 * 4]; int len = 0; // handle header try { len = stream.Read(request, 0, request.Length); } catch { return; } 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", ""); Tracking.AddEvent("HTTPServer", "Get", url, ""); ///////////////////////////////////////////////////////////////// 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: Mission 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[1024]; string sendme = MainV2.comPort.MAV.cs.roll + "," + MainV2.comPort.MAV.cs.pitch + "," + MainV2.comPort.MAV.cs.yaw + "," + MainV2.comPort.MAV.cs.lat + "," + MainV2.comPort.MAV.cs.lng + "," + MainV2.comPort.MAV.cs.alt; 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")) { byte[] buffer = Encoding.ASCII.GetBytes(georefkml); string header = "HTTP/1.1 200 OK\r\nServer: here\r\nKeep-Alive: timeout=15, max=100\r\nConnection: Keep-Alive\r\nCache-Control: no-cache\r\nContent-Type: application/vnd.google-earth.kml+xml\r\nX-Pad: avoid browser bug\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("network.kml")) { SharpKml.Dom.Document kml = new SharpKml.Dom.Document(); SharpKml.Dom.Placemark pmplane = new SharpKml.Dom.Placemark(); pmplane.Name = "P/Q " + MainV2.comPort.MAV.cs.altasl; 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.altasl; 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 }; if (loc.Latitude.Value != 0 && loc.Longitude.Value != 0) { kml.Viewpoint = la; kml.AddFeature(pmplane); } SharpKml.Dom.CoordinateCollection coords = new SharpKml.Dom.CoordinateCollection(); //if (loc.Latitude.Value != 0 && loc.Longitude.Value != 0) { //foreach (var point in MainV2.comPort.MAV.wps.Values) { // coords.Add(new SharpKml.Base.Vector(point.x, point.y, point.z)); } } //else { PointLatLngAlt home = null; // draw track try { foreach (var point in GCSViews.FlightPlanner.instance.fullpointlist) { if (point.Tag.ToLower().Contains("home")) { home = point; } if (point != null) { coords.Add(new SharpKml.Base.Vector(point.Lat, point.Lng, point.Alt)); } } } catch { } foreach (var point in GCSViews.FlightPlanner.instance.fullpointlist) { if (point == null) { continue; } SharpKml.Dom.Placemark wp = new SharpKml.Dom.Placemark(); wp.Name = "WP " + point.Tag + " Alt: " + point.Alt; SharpKml.Dom.Point wppoint = new SharpKml.Dom.Point(); var altmode = SharpKml.Dom.AltitudeMode.RelativeToGround; wppoint.AltitudeMode = altmode; wppoint.Coordinate = new Vector() { Latitude = point.Lat, Longitude = point.Lng, Altitude = point.Alt }; wp.Geometry = wppoint; kml.AddFeature(wp); } } SharpKml.Dom.LineString ls = new SharpKml.Dom.LineString(); ls.AltitudeMode = SharpKml.Dom.AltitudeMode.RelativeToGround; ls.Coordinates = coords; ls.Extrude = false; ls.Tessellate = true; Style style = new Style(); style.Id = "yellowLineGreenPoly"; style.Line = new LineStyle(new Color32(HexStringToColor("ff00ffff")), 4); Style style2 = new Style(); style2.Id = "yellowLineGreenPoly"; style2.Line = new LineStyle(new Color32(HexStringToColor("7f00ffff")), 4); // above ground SharpKml.Dom.Placemark pm = new SharpKml.Dom.Placemark() { Geometry = ls, Name = "WPs", StyleSelector = style }; kml.AddFeature(pm); // on ground SharpKml.Dom.LineString ls2 = new SharpKml.Dom.LineString(); ls2.Coordinates = coords; ls2.Extrude = false; ls2.Tessellate = true; ls2.AltitudeMode = SharpKml.Dom.AltitudeMode.ClampToGround; SharpKml.Dom.Placemark pm2 = new SharpKml.Dom.Placemark() { Geometry = ls2, Name = "onground", StyleSelector = style2 }; kml.AddFeature(pm2); 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=PLANNER\r\n\r\n--PLANNER\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")) { data = GetControlJpegRaw(GCSViews.FlightData.mymap); } else { GCSViews.FlightData.myhud.streamjpgenable = true; Image img1 = Image.FromStream(GCSViews.FlightData.myhud.streamjpg); Image img2 = GetControlJpeg(GCSViews.FlightData.mymap); 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--PLANNER\r\n"; temp = asciiEncoding.GetBytes(header); stream.Write(temp, 0, temp.Length); } 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\nServer: here\r\nKeep-Alive: timeout=15, max=100\r\nConnection: Keep-Alive\r\nContent-Type: image/jpg\r\nX-Pad: avoid browser bug\r\nContent-Length: " + memstream.Length + "\r\n\r\n"; byte[] temp = asciiEncoding.GetBytes(header); stream.Write(temp, 0, temp.Length); using (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); } } } 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.getPacket((byte)MAVLink.MAVLINK_MSG_ID.ATTITUDE) != null) { message.ATTITUDE = new Message2() { index = 1, msg = MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.ATTITUDE) .ByteArrayToStructure <MAVLink.mavlink_attitude_t>() } } ; if (MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.VFR_HUD) != null) { message.VFR_HUD = new Message2() { index = 1, msg = MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.VFR_HUD) .ByteArrayToStructure <MAVLink.mavlink_vfr_hud_t>() } } ; if (MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.NAV_CONTROLLER_OUTPUT) != null) { message.NAV_CONTROLLER_OUTPUT = new Message2() { index = 1, msg = MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.NAV_CONTROLLER_OUTPUT) .ByteArrayToStructure <MAVLink.mavlink_nav_controller_output_t>() } } ; if (MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.GPS_RAW_INT) != null) { message.GPS_RAW_INT = new Message2() { index = 1, msg = MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.GPS_RAW_INT) .ByteArrayToStructure <MAVLink.mavlink_gps_raw_int_t>() } } ; if (MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.HEARTBEAT) != null) { message.HEARTBEAT = new Message2() { index = 1, msg = MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.HEARTBEAT) .ByteArrayToStructure <MAVLink.mavlink_heartbeat_t>() } } ; if (MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.GPS_STATUS) != null) { message.GPS_STATUS = new Message2() { index = 1, msg = MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.GPS_STATUS) .ByteArrayToStructure <MAVLink.mavlink_gps_status_t>() } } ; if (MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.STATUSTEXT) != null) { message.STATUSTEXT = new Message2() { index = 1, msg = MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.STATUSTEXT) .ByteArrayToStructure <MAVLink.mavlink_statustext_t>() } } ; if (MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.SYS_STATUS) != null) { message.SYS_STATUS = new Message2() { index = 1, msg = MainV2.comPort.MAV.getPacket((byte)MAVLink.MAVLINK_MSG_ID.SYS_STATUS) .ByteArrayToStructure <MAVLink.mavlink_sys_status_t>() } } ; message.META_LINKQUALITY = message.SYS_STATUS = new Message2() { index = packetindex, time_usec = 0, msg = new META_LINKQUALITY() { master_in = (int)MainV2.comPort.MAV.packetsnotlost, mavpackettype = "META_LINKQUALITY", master_out = MainV2.comPort.packetcount, packet_loss = 100 - MainV2.comPort.MAV.cs.linkqualitygcs, mav_loss = 0 } }; 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> <a href=/georefnetwork.kml>georef kml</a> "; temp = asciiEncoding.GetBytes(content); stream.Write(temp, 0, temp.Length); } stream.Close(); log.Info("Close http " + url); } catch (Exception ee) { log.Error("Failed http ", ee); } } }
public static PointLatLngAlt ProjectPoint() { MainV2.comPort.GetMountStatus(); // this should be looking at rc_channel function yawchannel = (int)(float)MainV2.comPort.MAV.param["MNT_RC_IN_PAN"]; pitchchannel = (int)(float)MainV2.comPort.MAV.param["MNT_RC_IN_TILT"]; rollchannel = (int)(float)MainV2.comPort.MAV.param["MNT_RC_IN_ROLL"]; //if (!MainV2.comPort.BaseStream.IsOpen) // return PointLatLngAlt.Zero; PointLatLngAlt currentlocation = new PointLatLngAlt(MainV2.comPort.MAV.cs.lat, MainV2.comPort.MAV.cs.lng); double yawangle = MainV2.comPort.MAV.cs.campointc; double rollangle = MainV2.comPort.MAV.cs.campointb; double pitchangle = MainV2.comPort.MAV.cs.campointa; // if ((double)MainV2.comPort.MAV.param["MNT_TYPE"] == 4) { yawangle = MainV2.comPort.MAVlist[MainV2.comPort.sysidcurrent, 67].cs.yaw; rollangle = MainV2.comPort.MAVlist[MainV2.comPort.sysidcurrent, 67].cs.roll; pitchangle = MainV2.comPort.MAVlist[MainV2.comPort.sysidcurrent, 67].cs.pitch; } if (Math.Abs(rollangle) > 180 || yawangle == 0 && pitchangle == 0) { yawangle = ConvertPwmtoAngle(axis.yaw); //rollangle = ConvertPwmtoAngle(axis.roll); pitchangle = ConvertPwmtoAngle(axis.pitch) + MainV2.comPort.MAV.cs.pitch; pitchangle -= Math.Sin(yawangle * MathHelper.deg2rad) * MainV2.comPort.MAV.cs.roll; } // tan (o/a) // todo account for ground elevation change. int distout = 10; PointLatLngAlt newpos = PointLatLngAlt.Zero; //dist = Math.Tan((90 + pitchangle) * MathHelper.deg2rad) * (MainV2.comPort.MAV.cs.alt); while (distout < 1000) { // get a projected point to test intersection against - not using slope distance PointLatLngAlt newposdist = currentlocation.newpos(yawangle + MainV2.comPort.MAV.cs.yaw, distout); newposdist.Alt = srtm.getAltitude(newposdist.Lat, newposdist.Lng).alt; // get another point 50 infront PointLatLngAlt newposdist2 = currentlocation.newpos(yawangle + MainV2.comPort.MAV.cs.yaw, distout + 50); newposdist2.Alt = srtm.getAltitude(newposdist2.Lat, newposdist2.Lng).alt; // get the flat terrain distance out - at 0 alt double distflat = Math.Tan((90 + pitchangle) * MathHelper.deg2rad) * (MainV2.comPort.MAV.cs.altasl); // x is dist from plane, y is alt var newpoint = FindLineIntersection(new PointF(0, MainV2.comPort.MAV.cs.altasl), new PointF((float)distflat, 0), new PointF((float)distout, (float)newposdist.Alt), new PointF((float)distout + 50, (float)newposdist2.Alt)); if (newpoint.X != 0) { newpos = newposdist2; break; } distout += 50; } //Console.WriteLine("pitch " + pitchangle.ToString("0.000") + " yaw " + yawangle.ToString("0.000") + " dist" + dist.ToString("0.000")); //PointLatLngAlt newpos = currentlocation.newpos( yawangle + MainV2.comPort.MAV.cs.yaw, dist); //Console.WriteLine(newpos); return(newpos); }
static void addtomap(PointLatLngAlt pos, string tag) { }
public static List <PointLatLngAlt> CreateRotary(List <PointLatLngAlt> polygon, double altitude, double distance, double spacing, double angle, double overshoot1, double overshoot2, StartPosition startpos, bool shutter, float minLaneSeparation, float leadin, PointLatLngAlt HomeLocation) { spacing = 0; if (distance < 0.1) { distance = 0.1; } if (polygon.Count == 0) { return(new List <PointLatLngAlt>()); } List <utmpos> ans = new List <utmpos>(); // utm zone distance calcs will be done in int utmzone = polygon[0].GetUTMZone(); // utm position list List <utmpos> utmpositions = utmpos.ToList(PointLatLngAlt.ToUTM(utmzone, polygon), utmzone); // close the loop if its not already if (utmpositions[0] != utmpositions[utmpositions.Count - 1]) { utmpositions.Add(utmpositions[0]); // make a full loop } var maxlane = 200; // (Centroid(utmpositions).GetDistance(utmpositions[0]) / distance); ClipperLib.ClipperOffset clipperOffset = new ClipperLib.ClipperOffset(); clipperOffset.AddPath(utmpositions.Select(a => { return(new ClipperLib.IntPoint(a.x * 1000.0, a.y * 1000.0)); }).ToList(), ClipperLib.JoinType.jtMiter, ClipperLib.EndType.etClosedPolygon); for (int lane = 0; lane < maxlane; lane++) { List <utmpos> ans1 = new List <utmpos>(); ClipperLib.PolyTree tree = new ClipperLib.PolyTree(); clipperOffset.Execute(ref tree, (Int64)(distance * 1000.0 * -lane)); if (tree.ChildCount == 0) { break; } ans1 = tree.GetFirst().Contour.Select(a => new utmpos(a.X / 1000.0, a.Y / 1000.0, utmzone)).ToList(); if (ans.Count() > 2) { var start1 = ans[ans.Count() - 1]; var end1 = ans[ans.Count() - 2]; var start2 = ans1[0]; var end2 = ans1[ans1.Count() - 1]; var intersection = FindLineIntersectionExtension(start1, end1, start2, end2); if (intersection != utmpos.Zero) { //ans.Add(intersection); } } ans.AddRange(ans1); } // set the altitude on all points return(ans.Select(plla => { var a = plla.ToLLA(); a.Alt = altitude; a.Tag = "S"; return a; }).ToList()); }
public bool LoadFile(string filename) { FileName = filename; log.InfoFormat("GeoTiff {0}", filename); using (Tiff tiff = Tiff.Open(filename, "r")) { width = tiff.GetField(TiffTag.IMAGEWIDTH)[0].ToInt(); height = tiff.GetField(TiffTag.IMAGELENGTH)[0].ToInt(); bits = tiff.GetField(TiffTag.BITSPERSAMPLE)[0].ToInt(); //https://www.awaresystems.be/imaging/tiff/tifftags/sampleformat.html type = tiff.GetField(TiffTag.SAMPLEFORMAT)[0].ToInt(); modelscale = tiff.GetField(TiffTag.GEOTIFF_MODELPIXELSCALETAG); tiepoint = tiff.GetField(TiffTag.GEOTIFF_MODELTIEPOINTTAG); for (int i = 0; i < tiff.GetTagListCount(); i += 1) { var tagno = tiff.GetTagListEntry(i); var tag = (TiffTag)tagno; var info = tiff.GetField((TiffTag)tagno); log.InfoFormat("tiff ID={0} ? {1} len={2}", tag, (GKID)tagno, info.Length); } var GeoKeyDirectoryTag = tiff.GetField((TiffTag)34735); var KeyDirectoryVersion = BitConverter.ToUInt16(GeoKeyDirectoryTag[1].ToByteArray(), 0); var KeyRevision = BitConverter.ToUInt16(GeoKeyDirectoryTag[1].ToByteArray(), 2); var MinorRevision = BitConverter.ToUInt16(GeoKeyDirectoryTag[1].ToByteArray(), 4); var NumberOfKeys = BitConverter.ToUInt16(GeoKeyDirectoryTag[1].ToByteArray(), 6); ProjectedCSTypeGeoKey = 0; for (int i = 8; i < 8 + NumberOfKeys * 8; i += 8) { var KeyID = BitConverter.ToUInt16(GeoKeyDirectoryTag[1].ToByteArray(), i); var TIFFTagLocation = BitConverter.ToUInt16(GeoKeyDirectoryTag[1].ToByteArray(), i + 2); var Count = BitConverter.ToUInt16(GeoKeyDirectoryTag[1].ToByteArray(), i + 4); var Value_Offset = BitConverter.ToUInt16(GeoKeyDirectoryTag[1].ToByteArray(), i + 6); log.InfoFormat("GeoKeyDirectoryTag ID={0} TagLoc={1} Count={2} Value/offset={3}", (GKID)KeyID, TIFFTagLocation, Count, Value_Offset); // save it if (TIFFTagLocation == 0) { GeoKeys[(GKID)KeyID] = Value_Offset; } else if (TIFFTagLocation == 34737) { GeoKeys[(GKID)KeyID] = Encoding.ASCII.GetString(tiff.GetField((TiffTag)TIFFTagLocation)[1].ToByteArray().Skip(Value_Offset).Take(Count).ToArray()); } else if (TIFFTagLocation == 34736) { GeoKeys[(GKID)KeyID] = BitConverter.ToDouble(tiff.GetField((TiffTag)TIFFTagLocation)[1].ToByteArray().Skip(Value_Offset * 8).Take(Count * 8).ToArray(), 0); } else { GeoKeys[(GKID)KeyID] = Value_Offset; } if (KeyID == (int)GKID.ProjectedCSTypeGeoKey) { ProjectedCSTypeGeoKey = Value_Offset; } if (KeyID == (int)GKID.GTRasterTypeGeoKey) { GTRasterTypeGeoKey = Value_Offset; } if (KeyID == (int)GKID.ProjCoordTransGeoKey) { ProjCoordTransGeoKey = Value_Offset; } if (TIFFTagLocation != 0) { if (TIFFTagLocation == 34737) //ascii { var value = tiff.GetField((TiffTag)TIFFTagLocation)[1].ToByteArray().Skip(Value_Offset) .Take(Count); log.InfoFormat("GeoKeyDirectoryTag ID={0} Value={1}", (GKID)KeyID, Encoding.ASCII.GetString(value.ToArray())); } if (TIFFTagLocation == 34736) // double { var value = tiff.GetField((TiffTag)TIFFTagLocation)[1].ToByteArray().Skip(Value_Offset * 8).Take(Count * 8); log.InfoFormat("GeoKeyDirectoryTag ID={0} Value={1}", (GKID)KeyID, BitConverter.ToDouble(value.ToArray(), 0)); } if (KeyID == (int)GKID.PCSCitationGeoKey) { var value = tiff.GetField((TiffTag)TIFFTagLocation)[1].ToByteArray().Skip(Value_Offset).Take(Count); PCSCitationGeoKey = Encoding.ASCII.GetString(value.ToArray()); log.InfoFormat("GeoKeyDirectoryTag ID={0} Value={1}", (GKID)KeyID, Encoding.ASCII.GetString(value.ToArray())); } } } GeoAsciiParamsTag = tiff.GetField((TiffTag)34737); if (GeoAsciiParamsTag != null && GeoAsciiParamsTag.Length == 2) { log.InfoFormat("GeoAsciiParamsTag 34737 {0}", GeoAsciiParamsTag[1]); } i = BitConverter.ToDouble(tiepoint[1].ToByteArray(), 0); j = BitConverter.ToDouble(tiepoint[1].ToByteArray(), 0 + 8); k = BitConverter.ToDouble(tiepoint[1].ToByteArray(), 0 + 16); x = BitConverter.ToDouble(tiepoint[1].ToByteArray(), 0 + 24); y = BitConverter.ToDouble(tiepoint[1].ToByteArray(), 0 + 32); z = BitConverter.ToDouble(tiepoint[1].ToByteArray(), 0 + 40); log.InfoFormat("Tie Point ({0},{1},{2}) --> ({3},{4},{5})", i, j, k, x, y, z); xscale = BitConverter.ToDouble(modelscale[1].ToByteArray(), 0); yscale = BitConverter.ToDouble(modelscale[1].ToByteArray(), 0 + 8); zscale = BitConverter.ToDouble(modelscale[1].ToByteArray(), 0 + 16); log.InfoFormat("Scale ({0},{1},{2})", xscale, yscale, zscale); if (GTRasterTypeGeoKey == 1) { // starts from top left so x + y - x += xscale / 2.0; y -= yscale / 2.0; } if (ProjectedCSTypeGeoKey == 32767 && ProjCoordTransGeoKey == 15) { // user-defined ProjectionInfo pStart = ProjectionInfo.FromProj4String($"+proj=stere +lat_ts={GeoKeys[GKID.ProjOriginLatGeoKey].ToString()} +lat_0=90 +lon_0={GeoKeys[GKID.ProjStraightVertPoleLongGeoKey].ToString()} +x_0=0 +y_0=0 +ellps={GeoKeys[GKID.GeogCitationGeoKey].ToString().Replace(" ", "").Replace("|", "")} +datum={GeoKeys[GKID.GeogCitationGeoKey].ToString().Replace(" ", "").Replace("|", "")} +units=m +no_defs "); ProjectionInfo pESRIEnd = KnownCoordinateSystems.Geographic.World.WGS1984; srcProjection = pStart; double[] xyarray = { x, y, x + width * xscale, y - height * yscale, x + width * xscale, y, x, y - height * yscale }; Reproject.ReprojectPoints(xyarray, null, pStart, pESRIEnd, 0, xyarray.Length / 2); ymin = Math.Min(Math.Min(Math.Min(xyarray[1], xyarray[3]), xyarray[5]), xyarray[7]); xmin = Math.Min(Math.Min(Math.Min(xyarray[0], xyarray[2]), xyarray[4]), xyarray[6]); ymax = Math.Max(Math.Max(Math.Max(xyarray[1], xyarray[3]), xyarray[5]), xyarray[7]); xmax = Math.Max(Math.Max(Math.Max(xyarray[0], xyarray[2]), xyarray[4]), xyarray[6]); } else if (ProjectedCSTypeGeoKey != 32767 && ProjectedCSTypeGeoKey != 0) { try { srcProjection = ProjectionInfo.FromEpsgCode(ProjectedCSTypeGeoKey); ProjectionInfo pESRIEnd = KnownCoordinateSystems.Geographic.World.WGS1984; double[] xyarray = { x, y, x + width * xscale, y - height * yscale, x + width * xscale, y, x, y - height * yscale }; Reproject.ReprojectPoints(xyarray, null, srcProjection, pESRIEnd, 0, xyarray.Length / 2); ymin = Math.Min(Math.Min(Math.Min(xyarray[1], xyarray[3]), xyarray[5]), xyarray[7]); xmin = Math.Min(Math.Min(Math.Min(xyarray[0], xyarray[2]), xyarray[4]), xyarray[6]); ymax = Math.Max(Math.Max(Math.Max(xyarray[1], xyarray[3]), xyarray[5]), xyarray[7]); xmax = Math.Max(Math.Max(Math.Max(xyarray[0], xyarray[2]), xyarray[4]), xyarray[6]); } catch (Exception ex) { log.Error(ex); srcProjection = null; } } else { try { srcProjection = ProjectionInfo.FromEsriString(GeoKeys[GKID.PCSCitationGeoKey].ToString()); ProjectionInfo pESRIEnd = KnownCoordinateSystems.Geographic.World.WGS1984; double[] xyarray = { x, y, x + width * xscale, y - height * yscale, x + width * xscale, y, x, y - height * yscale }; Reproject.ReprojectPoints(xyarray, null, srcProjection, pESRIEnd, 0, xyarray.Length / 2); ymin = Math.Min(Math.Min(Math.Min(xyarray[1], xyarray[3]), xyarray[5]), xyarray[7]); xmin = Math.Min(Math.Min(Math.Min(xyarray[0], xyarray[2]), xyarray[4]), xyarray[6]); ymax = Math.Max(Math.Max(Math.Max(xyarray[1], xyarray[3]), xyarray[5]), xyarray[7]); xmax = Math.Max(Math.Max(Math.Max(xyarray[0], xyarray[2]), xyarray[4]), xyarray[6]); } catch (Exception ex) { log.Error(ex); srcProjection = null; } } if (srcProjection != null) { } else // wgs84 utm if (ProjectedCSTypeGeoKey >= 32601 && ProjectedCSTypeGeoKey <= 32760) { if (ProjectedCSTypeGeoKey > 32700) { UTMZone = (ProjectedCSTypeGeoKey - 32700) * -1; srcProjection = ProjectionInfo.FromProj4String($"+proj=utm +zone={UTMZone} +ellps=WGS84 +datum=WGS84 +units=m +no_defs "); //tl var pnt = PointLatLngAlt.FromUTM(UTMZone, x, y); //br var pnt2 = PointLatLngAlt.FromUTM(UTMZone, x + width * xscale, y - height * yscale); //tr var pnt3 = PointLatLngAlt.FromUTM(UTMZone, x + width * xscale, y); //bl var pnt4 = PointLatLngAlt.FromUTM(UTMZone, x, y - height * yscale); ymin = Math.Min(Math.Min(Math.Min(pnt.Lat, pnt2.Lat), pnt3.Lat), pnt4.Lat); xmin = Math.Min(Math.Min(Math.Min(pnt.Lng, pnt2.Lng), pnt3.Lng), pnt4.Lng); ymax = Math.Max(Math.Max(Math.Max(pnt.Lat, pnt2.Lat), pnt3.Lat), pnt4.Lat); xmax = Math.Max(Math.Max(Math.Max(pnt.Lng, pnt2.Lng), pnt3.Lng), pnt4.Lng); } if (ProjectedCSTypeGeoKey < 32700) { UTMZone = ProjectedCSTypeGeoKey - 32600; srcProjection = ProjectionInfo.FromProj4String($"+proj=utm +zone={UTMZone} +ellps=WGS84 +datum=WGS84 +units=m +no_defs "); var pnt = PointLatLngAlt.FromUTM(UTMZone, x, y); var pnt2 = PointLatLngAlt.FromUTM(UTMZone, x + width * xscale, y - height * yscale); var pnt3 = PointLatLngAlt.FromUTM(UTMZone, x + width * xscale, y); var pnt4 = PointLatLngAlt.FromUTM(UTMZone, x, y - height * yscale); ymin = Math.Min(Math.Min(Math.Min(pnt.Lat, pnt2.Lat), pnt3.Lat), pnt4.Lat); xmin = Math.Min(Math.Min(Math.Min(pnt.Lng, pnt2.Lng), pnt3.Lng), pnt4.Lng); ymax = Math.Max(Math.Max(Math.Max(pnt.Lat, pnt2.Lat), pnt3.Lat), pnt4.Lat); xmax = Math.Max(Math.Max(Math.Max(pnt.Lng, pnt2.Lng), pnt3.Lng), pnt4.Lng); } } else // etrs89 utm if (ProjectedCSTypeGeoKey >= 3038 && ProjectedCSTypeGeoKey <= 3051) { UTMZone = ProjectedCSTypeGeoKey - 3012; srcProjection = ProjectionInfo.FromProj4String($"+proj=utm +zone={UTMZone} +ellps=GRS80 +units=m +no_defs "); // 3038 - 26 var pnt = PointLatLngAlt.FromUTM(UTMZone, x, y); var pnt2 = PointLatLngAlt.FromUTM(UTMZone, x + width * xscale, y - height * yscale); var pnt3 = PointLatLngAlt.FromUTM(UTMZone, x + width * xscale, y); var pnt4 = PointLatLngAlt.FromUTM(UTMZone, x, y - height * yscale); ymin = Math.Min(Math.Min(Math.Min(pnt.Lat, pnt2.Lat), pnt3.Lat), pnt4.Lat); xmin = Math.Min(Math.Min(Math.Min(pnt.Lng, pnt2.Lng), pnt3.Lng), pnt4.Lng); ymax = Math.Max(Math.Max(Math.Max(pnt.Lat, pnt2.Lat), pnt3.Lat), pnt4.Lat); xmax = Math.Max(Math.Max(Math.Max(pnt.Lng, pnt2.Lng), pnt3.Lng), pnt4.Lng); } else if (ProjectedCSTypeGeoKey >= 25828 && ProjectedCSTypeGeoKey <= 25838) { UTMZone = ProjectedCSTypeGeoKey - 25800; // 3038 - 26 var pnt = PointLatLngAlt.FromUTM(UTMZone, x, y); var pnt2 = PointLatLngAlt.FromUTM(UTMZone, x + width * xscale, y - height * yscale); var pnt3 = PointLatLngAlt.FromUTM(UTMZone, x + width * xscale, y); var pnt4 = PointLatLngAlt.FromUTM(UTMZone, x, y - height * yscale); ymin = Math.Min(Math.Min(Math.Min(pnt.Lat, pnt2.Lat), pnt3.Lat), pnt4.Lat); xmin = Math.Min(Math.Min(Math.Min(pnt.Lng, pnt2.Lng), pnt3.Lng), pnt4.Lng); ymax = Math.Max(Math.Max(Math.Max(pnt.Lat, pnt2.Lat), pnt3.Lat), pnt4.Lat); xmax = Math.Max(Math.Max(Math.Max(pnt.Lng, pnt2.Lng), pnt3.Lng), pnt4.Lng); } else /// gda94 if (ProjectedCSTypeGeoKey >= 28348 && ProjectedCSTypeGeoKey <= 28358) { UTMZone = (ProjectedCSTypeGeoKey - 28300) * -1; var pnt = PointLatLngAlt.FromUTM(UTMZone, x, y); var pnt2 = PointLatLngAlt.FromUTM(UTMZone, x + width * xscale, y - height * yscale); var pnt3 = PointLatLngAlt.FromUTM(UTMZone, x + width * xscale, y); var pnt4 = PointLatLngAlt.FromUTM(UTMZone, x, y - height * yscale); ymin = Math.Min(Math.Min(Math.Min(pnt.Lat, pnt2.Lat), pnt3.Lat), pnt4.Lat); xmin = Math.Min(Math.Min(Math.Min(pnt.Lng, pnt2.Lng), pnt3.Lng), pnt4.Lng); ymax = Math.Max(Math.Max(Math.Max(pnt.Lat, pnt2.Lat), pnt3.Lat), pnt4.Lat); xmax = Math.Max(Math.Max(Math.Max(pnt.Lng, pnt2.Lng), pnt3.Lng), pnt4.Lng); } else // geo lat/lng if (ProjectedCSTypeGeoKey == 0 || ProjectedCSTypeGeoKey == 4326) { var pnt = new PointLatLngAlt(y, x); var pnt2 = new PointLatLngAlt(y - height * yscale, x + width * xscale); var pnt3 = new PointLatLngAlt(y, x + width * xscale); var pnt4 = new PointLatLngAlt(y - height * yscale, x); ymin = Math.Min(Math.Min(Math.Min(pnt.Lat, pnt2.Lat), pnt3.Lat), pnt4.Lat); xmin = Math.Min(Math.Min(Math.Min(pnt.Lng, pnt2.Lng), pnt3.Lng), pnt4.Lng); ymax = Math.Max(Math.Max(Math.Max(pnt.Lat, pnt2.Lat), pnt3.Lat), pnt4.Lat); xmax = Math.Max(Math.Max(Math.Max(pnt.Lng, pnt2.Lng), pnt3.Lng), pnt4.Lng); } Area = new RectLatLng(ymax, xmin, xmax - xmin, ymax - ymin); log.InfoFormat("Coverage {0}", Area.ToString()); log.InfoFormat("Start Point ({0},{1},{2}) --> ({3},{4},{5})", i, j, k, x, y, z); lock (index) GeoTiff.index.Add(this); /* * * short numberOfDirectories = tiff.NumberOfDirectories(); * for (short d = 0; d < numberOfDirectories; ++d) * { * tiff.SetDirectory((short)d); * * for (ushort t = ushort.MinValue; t < ushort.MaxValue; ++t) * { * TiffTag tag = (TiffTag)t; * FieldValue[] value = tiff.GetField(tag); * if (value != null) * { * for (int j2 = 0; j2 < value.Length; j2++) * { * Console.WriteLine("{0} : {1} : {2}", tag.ToString(), value[j2].Value.GetType().ToString(), value[j2].ToString()); * } * } * } * } */ } return(true); }
public static async Task <List <PointLatLngAlt> > CreateGridAsync(List <PointLatLngAlt> polygon, double altitude, double distance, double spacing, double angle, double overshoot1, double overshoot2, StartPosition startpos, bool shutter, float minLaneSeparation, float leadin, PointLatLngAlt HomeLocation) { return(await Task.Run((() => CreateGrid(polygon, altitude, distance, spacing, angle, overshoot1, overshoot2, startpos, shutter, minLaneSeparation, leadin, HomeLocation)))); }
public bool setRallyPoint(byte index, PointLatLngAlt plla, short break_alt, UInt16 land_dir_cd, byte flags, byte rallypointcount) { mavlink_rally_point_t rp = new mavlink_rally_point_t(); rp.idx = index; rp.count = rallypointcount; rp.lat = (int) (plla.Lat*t7); rp.lng = (int) (plla.Lng*t7); rp.alt = (short) plla.Alt; rp.break_alt = break_alt; rp.land_dir = land_dir_cd; rp.flags = (byte) flags; rp.target_component = MAV.compid; rp.target_system = MAV.sysid; int retry = 3; while (retry > 0) { generatePacket((byte) MAVLINK_MSG_ID.RALLY_POINT, rp); int counttemp = 0; PointLatLngAlt newfp = getRallyPoint(rp.idx, ref counttemp); if (newfp.Lat == plla.Lat && newfp.Lng == rp.lng) { Console.WriteLine("Rally Set"); return true; } retry--; } return false; }
public static List <PointLatLngAlt> CreateGrid(List <PointLatLngAlt> polygon, double altitude, double distance, double spacing, double angle, double overshoot1, double overshoot2, StartPosition startpos, bool shutter, float minLaneSeparation, float leadin, PointLatLngAlt HomeLocation) { //DoDebug(); if (spacing < 0.1 && spacing != 0) { spacing = 0.1; } if (distance < 0.1) { distance = 0.1; } if (polygon.Count == 0) { return(new List <PointLatLngAlt>()); } // Make a non round number in case of corner cases if (minLaneSeparation != 0) { minLaneSeparation += 0.5F; } // Lane Separation in meters double minLaneSeparationINMeters = minLaneSeparation * distance; List <PointLatLngAlt> ans = new List <PointLatLngAlt>(); // utm zone distance calcs will be done in int utmzone = polygon[0].GetUTMZone(); // utm position list List <utmpos> utmpositions = utmpos.ToList(PointLatLngAlt.ToUTM(utmzone, polygon), utmzone); // close the loop if its not already if (utmpositions[0] != utmpositions[utmpositions.Count - 1]) { utmpositions.Add(utmpositions[0]); // make a full loop } // get mins/maxs of coverage area Rect area = getPolyMinMax(utmpositions); // get initial grid // used to determine the size of the outer grid area double diagdist = area.DiagDistance(); // somewhere to store out generated lines List <linelatlng> grid = new List <linelatlng>(); // number of lines we need int lines = 0; // get start point middle double x = area.MidWidth; double y = area.MidHeight; addtomap(new utmpos(x, y, utmzone), "Base"); // get left extent double xb1 = x; double yb1 = y; // to the left newpos(ref xb1, ref yb1, angle - 90, diagdist / 2 + distance); // backwards newpos(ref xb1, ref yb1, angle + 180, diagdist / 2 + distance); utmpos left = new utmpos(xb1, yb1, utmzone); addtomap(left, "left"); // get right extent double xb2 = x; double yb2 = y; // to the right newpos(ref xb2, ref yb2, angle + 90, diagdist / 2 + distance); // backwards newpos(ref xb2, ref yb2, angle + 180, diagdist / 2 + distance); utmpos right = new utmpos(xb2, yb2, utmzone); addtomap(right, "right"); // set start point to left hand side x = xb1; y = yb1; // draw the outergrid, this is a grid that cover the entire area of the rectangle plus more. while (lines < ((diagdist + distance * 2) / distance)) { // copy the start point to generate the end point double nx = x; double ny = y; newpos(ref nx, ref ny, angle, diagdist + distance * 2); linelatlng line = new linelatlng(); line.p1 = new utmpos(x, y, utmzone); line.p2 = new utmpos(nx, ny, utmzone); line.basepnt = new utmpos(x, y, utmzone); grid.Add(line); // addtomap(line); newpos(ref x, ref y, angle + 90, distance); lines++; } // find intersections with our polygon // store lines that dont have any intersections List <linelatlng> remove = new List <linelatlng>(); int gridno = grid.Count; // cycle through our grid for (int a = 0; a < gridno; a++) { double closestdistance = double.MaxValue; double farestdistance = double.MinValue; utmpos closestpoint = utmpos.Zero; utmpos farestpoint = utmpos.Zero; // somewhere to store our intersections List <utmpos> matchs = new List <utmpos>(); int b = -1; int crosses = 0; utmpos newutmpos = utmpos.Zero; foreach (utmpos pnt in utmpositions) { b++; if (b == 0) { continue; } newutmpos = FindLineIntersection(utmpositions[b - 1], utmpositions[b], grid[a].p1, grid[a].p2); if (!newutmpos.IsZero) { crosses++; matchs.Add(newutmpos); if (closestdistance > grid[a].p1.GetDistance(newutmpos)) { closestpoint.y = newutmpos.y; closestpoint.x = newutmpos.x; closestpoint.zone = newutmpos.zone; closestdistance = grid[a].p1.GetDistance(newutmpos); } if (farestdistance < grid[a].p1.GetDistance(newutmpos)) { farestpoint.y = newutmpos.y; farestpoint.x = newutmpos.x; farestpoint.zone = newutmpos.zone; farestdistance = grid[a].p1.GetDistance(newutmpos); } } } if (crosses == 0) // outside our polygon { if (!PointInPolygon(grid[a].p1, utmpositions) && !PointInPolygon(grid[a].p2, utmpositions)) { remove.Add(grid[a]); } } else if (crosses == 1) // bad - shouldnt happen { } else if (crosses == 2) // simple start and finish { linelatlng line = grid[a]; line.p1 = closestpoint; line.p2 = farestpoint; grid[a] = line; } else // multiple intersections { linelatlng line = grid[a]; remove.Add(line); while (matchs.Count > 1) { linelatlng newline = new linelatlng(); closestpoint = findClosestPoint(closestpoint, matchs); newline.p1 = closestpoint; matchs.Remove(closestpoint); closestpoint = findClosestPoint(closestpoint, matchs); newline.p2 = closestpoint; matchs.Remove(closestpoint); newline.basepnt = line.basepnt; grid.Add(newline); } } } // cleanup and keep only lines that pass though our polygon foreach (linelatlng line in remove) { grid.Remove(line); } // debug foreach (linelatlng line in grid) { addtomap(line); } if (grid.Count == 0) { return(ans); } // pick start positon based on initial point rectangle utmpos startposutm; switch (startpos) { default: case StartPosition.Home: startposutm = new utmpos(HomeLocation); break; case StartPosition.BottomLeft: startposutm = new utmpos(area.Left, area.Bottom, utmzone); break; case StartPosition.BottomRight: startposutm = new utmpos(area.Right, area.Bottom, utmzone); break; case StartPosition.TopLeft: startposutm = new utmpos(area.Left, area.Top, utmzone); break; case StartPosition.TopRight: startposutm = new utmpos(area.Right, area.Top, utmzone); break; case StartPosition.Point: startposutm = new utmpos(StartPointLatLngAlt); break; } // find the closes polygon point based from our startpos selection startposutm = findClosestPoint(startposutm, utmpositions); // find closest line point to startpos linelatlng closest = findClosestLine(startposutm, grid, 0 /*Lane separation does not apply to starting point*/, angle); utmpos lastpnt; // get the closes point from the line we picked if (closest.p1.GetDistance(startposutm) < closest.p2.GetDistance(startposutm)) { lastpnt = closest.p1; } else { lastpnt = closest.p2; } // S = start // E = end // ME = middle end // SM = start middle while (grid.Count > 0) { // for each line, check which end of the line is the next closest if (closest.p1.GetDistance(lastpnt) < closest.p2.GetDistance(lastpnt)) { utmpos newstart = newpos(closest.p1, angle, -leadin); newstart.Tag = "S"; addtomap(newstart, "S"); ans.Add(newstart); if (leadin < 0) { var p2 = new utmpos(newstart) { Tag = "SM" }; addtomap(p2, "SM"); ans.Add(p2); } else { closest.p1.Tag = "SM"; addtomap(closest.p1, "SM"); ans.Add(closest.p1); } if (spacing > 0) { for (double d = (spacing - ((closest.basepnt.GetDistance(closest.p1)) % spacing)); d < (closest.p1.GetDistance(closest.p2)); d += spacing) { double ax = closest.p1.x; double ay = closest.p1.y; newpos(ref ax, ref ay, angle, d); var utmpos1 = new utmpos(ax, ay, utmzone) { Tag = "M" }; addtomap(utmpos1, "M"); ans.Add(utmpos1); } } utmpos newend = newpos(closest.p2, angle, overshoot1); if (overshoot1 < 0) { var p2 = new utmpos(newend) { Tag = "ME" }; addtomap(p2, "ME"); ans.Add(p2); } else { closest.p2.Tag = "ME"; addtomap(closest.p2, "ME"); ans.Add(closest.p2); } newend.Tag = "E"; addtomap(newend, "E"); ans.Add(newend); lastpnt = closest.p2; grid.Remove(closest); if (grid.Count == 0) { break; } closest = findClosestLine(newend, grid, minLaneSeparationINMeters, angle); } else { utmpos newstart = newpos(closest.p2, angle, leadin); newstart.Tag = "S"; addtomap(newstart, "S"); ans.Add(newstart); if (leadin < 0) { var p2 = new utmpos(newstart) { Tag = "SM" }; addtomap(p2, "SM"); ans.Add(p2); } else { closest.p2.Tag = "SM"; addtomap(closest.p2, "SM"); ans.Add(closest.p2); } if (spacing > 0) { for (double d = ((closest.basepnt.GetDistance(closest.p2)) % spacing); d < (closest.p1.GetDistance(closest.p2)); d += spacing) { double ax = closest.p2.x; double ay = closest.p2.y; newpos(ref ax, ref ay, angle, -d); var utmpos2 = new utmpos(ax, ay, utmzone) { Tag = "M" }; addtomap(utmpos2, "M"); ans.Add(utmpos2); } } utmpos newend = newpos(closest.p1, angle, -overshoot2); if (overshoot2 < 0) { var p2 = new utmpos(newend) { Tag = "ME" }; addtomap(p2, "ME"); ans.Add(p2); } else { closest.p1.Tag = "ME"; addtomap(closest.p1, "ME"); ans.Add(closest.p1); } newend.Tag = "E"; addtomap(newend, "E"); ans.Add(newend); lastpnt = closest.p1; grid.Remove(closest); if (grid.Count == 0) { break; } closest = findClosestLine(newend, grid, minLaneSeparationINMeters, angle); } } // set the altitude on all points ans.ForEach(plla => { plla.Alt = altitude; }); return(ans); }
public void doConnect(MAVLinkInterface comPort, string portname, string baud) { bool skipconnectcheck = false; log.Info("We are connecting"); switch (portname) { case "preset": skipconnectcheck = true; break; case "TCP": comPort.BaseStream = new TcpSerial(); _connectionControl.CMB_serialport.Text = "TCP"; break; case "UDP": comPort.BaseStream = new UdpSerial(); _connectionControl.CMB_serialport.Text = "UDP"; break; case "UDPCl": comPort.BaseStream = new UdpSerialConnect(); _connectionControl.CMB_serialport.Text = "UDPCl"; break; case "AUTO": default: comPort.BaseStream = new SerialPort(); break; } // Tell the connection UI that we are now connected. _connectionControl.IsConnected(true); // Here we want to reset the connection stats counter etc. this.ResetConnectionStats(); comPort.MAV.cs.ResetInternals(); //cleanup any log being played comPort.logreadmode = false; if (comPort.logplaybackfile != null) comPort.logplaybackfile.Close(); comPort.logplaybackfile = null; try { // do autoscan if (portname == "AUTO") { Comms.CommsSerialScan.Scan(false); DateTime deadline = DateTime.Now.AddSeconds(50); while (Comms.CommsSerialScan.foundport == false) { System.Threading.Thread.Sleep(100); if (DateTime.Now > deadline) { CustomMessageBox.Show(Strings.Timeout); _connectionControl.IsConnected(false); return; } } _connectionControl.CMB_serialport.Text = portname = Comms.CommsSerialScan.portinterface.PortName; _connectionControl.CMB_baudrate.Text = baud = Comms.CommsSerialScan.portinterface.BaudRate.ToString(); } log.Info("Set Portname"); // set port, then options comPort.BaseStream.PortName = portname; log.Info("Set Baudrate"); try { comPort.BaseStream.BaudRate = int.Parse(baud); } catch (Exception exp) { log.Error(exp); } // prevent serialreader from doing anything comPort.giveComport = true; log.Info("About to do dtr if needed"); // reset on connect logic. if (config["CHK_resetapmonconnect"] == null || bool.Parse(config["CHK_resetapmonconnect"].ToString()) == true) { log.Info("set dtr rts to false"); comPort.BaseStream.DtrEnable = false; comPort.BaseStream.RtsEnable = false; comPort.BaseStream.toggleDTR(); } comPort.giveComport = false; // setup to record new logs try { Directory.CreateDirectory(MainV2.LogDir); comPort.logfile = new BufferedStream(File.Open(MainV2.LogDir + Path.DirectorySeparatorChar + DateTime.Now.ToString("yyyy-MM-dd HH-mm-ss") + ".tlog", FileMode.CreateNew, FileAccess.ReadWrite, FileShare.None)); comPort.rawlogfile = new BufferedStream(File.Open(MainV2.LogDir + Path.DirectorySeparatorChar + DateTime.Now.ToString("yyyy-MM-dd HH-mm-ss") + ".rlog", FileMode.CreateNew, FileAccess.ReadWrite, FileShare.None)); log.Info("creating logfile " + DateTime.Now.ToString("yyyy-MM-dd HH-mm-ss") + ".tlog"); } catch (Exception exp2) { log.Error(exp2); CustomMessageBox.Show(Strings.Failclog); } // soft fail // reset connect time - for timeout functions connecttime = DateTime.Now; // do the connect comPort.Open(false, skipconnectcheck); if (!comPort.BaseStream.IsOpen) { log.Info("comport is closed. existing connect"); try { _connectionControl.IsConnected(false); UpdateConnectIcon(); comPort.Close(); } catch { } return; } // 3dr radio is hidden as no hb packet is ever emitted if (comPort.sysidseen.Count > 1) { // we have more than one mav // user selection of sysid MissionPlanner.Controls.SysidSelector id = new SysidSelector(); id.Show(); } // create a copy int[] list = comPort.sysidseen.ToArray(); // get all the params foreach (var sysid in list) { comPort.sysidcurrent = sysid; comPort.getParamList(); } // set to first seen comPort.sysidcurrent = list[0]; // detect firmware we are conected to. if (comPort.MAV.cs.firmware == Firmwares.ArduCopter2) { _connectionControl.TOOL_APMFirmware.SelectedIndex = _connectionControl.TOOL_APMFirmware.Items.IndexOf(Firmwares.ArduCopter2); } else if (comPort.MAV.cs.firmware == Firmwares.Ateryx) { _connectionControl.TOOL_APMFirmware.SelectedIndex = _connectionControl.TOOL_APMFirmware.Items.IndexOf(Firmwares.Ateryx); } else if (comPort.MAV.cs.firmware == Firmwares.ArduRover) { _connectionControl.TOOL_APMFirmware.SelectedIndex = _connectionControl.TOOL_APMFirmware.Items.IndexOf(Firmwares.ArduRover); } else if (comPort.MAV.cs.firmware == Firmwares.ArduPlane) { _connectionControl.TOOL_APMFirmware.SelectedIndex = _connectionControl.TOOL_APMFirmware.Items.IndexOf(Firmwares.ArduPlane); } // check for newer firmware var softwares = Firmware.LoadSoftwares(); if (softwares.Count > 0) { try { string[] fields1 = comPort.MAV.VersionString.Split(' '); foreach (Firmware.software item in softwares) { string[] fields2 = item.name.Split(' '); // check primare firmware type. ie arudplane, arducopter if (fields1[0] == fields2[0]) { Version ver1 = VersionDetection.GetVersion(comPort.MAV.VersionString); Version ver2 = VersionDetection.GetVersion(item.name); if (ver2 > ver1) { Common.MessageShowAgain(Strings.NewFirmware, Strings.NewFirmwareA + item.name + Strings.Pleaseup); break; } // check the first hit only break; } } } catch (Exception ex) { log.Error(ex); } } FlightData.CheckBatteryShow(); MissionPlanner.Utilities.Tracking.AddEvent("Connect", "Connect", comPort.MAV.cs.firmware.ToString(), comPort.MAV.param.Count.ToString()); MissionPlanner.Utilities.Tracking.AddTiming("Connect", "Connect Time", (DateTime.Now - connecttime).TotalMilliseconds, ""); MissionPlanner.Utilities.Tracking.AddEvent("Connect", "Baud", comPort.BaseStream.BaudRate.ToString(), ""); // save the baudrate for this port config[_connectionControl.CMB_serialport.Text + "_BAUD"] = _connectionControl.CMB_baudrate.Text; this.Text = titlebar + " " + comPort.MAV.VersionString; // refresh config window if needed if (MyView.current != null) { if (MyView.current.Name == "HWConfig") MyView.ShowScreen("HWConfig"); if (MyView.current.Name == "SWConfig") MyView.ShowScreen("SWConfig"); } // load wps on connect option. if (config["loadwpsonconnect"] != null && bool.Parse(config["loadwpsonconnect"].ToString()) == true) { // only do it if we are connected. if (comPort.BaseStream.IsOpen) { MenuFlightPlanner_Click(null, null); FlightPlanner.BUT_read_Click(null, null); } } // get any rallypoints if (MainV2.comPort.MAV.param.ContainsKey("RALLY_TOTAL") && int.Parse(MainV2.comPort.MAV.param["RALLY_TOTAL"].ToString()) > 0) { FlightPlanner.getRallyPointsToolStripMenuItem_Click(null, null); double maxdist = 0; foreach (var rally in comPort.MAV.rallypoints) { foreach (var rally1 in comPort.MAV.rallypoints) { var pnt1 = new PointLatLngAlt(rally.Value.lat / 10000000.0f, rally.Value.lng / 10000000.0f); var pnt2 = new PointLatLngAlt(rally1.Value.lat / 10000000.0f, rally1.Value.lng / 10000000.0f); var dist = pnt1.GetDistance(pnt2); maxdist = Math.Max(maxdist, dist); } } if (comPort.MAV.param.ContainsKey("RALLY_LIMIT_KM") && (maxdist / 1000.0) > (float)comPort.MAV.param["RALLY_LIMIT_KM"]) { CustomMessageBox.Show(Strings.Warningrallypointdistance + " " + (maxdist / 1000.0).ToString("0.00") + " > " + (float)comPort.MAV.param["RALLY_LIMIT_KM"]); } } // set connected icon this.MenuConnect.Image = displayicons.disconnect; } catch (Exception ex) { log.Warn(ex); try { _connectionControl.IsConnected(false); UpdateConnectIcon(); comPort.Close(); } catch (Exception ex2) { log.Warn(ex2); } CustomMessageBox.Show("Can not establish a connection\n\n" + ex.Message); return; } }
public string SUBMITHOST; // Submitter Host “1.2.3.4” or “enduser5.faa.gov” public List <List <PointLatLng> > GetPaths() { //RLN27.576944W97.108611LN27.468056W96.961111LN27.322222W97.050000LN27.345833W97.088889LN27.439167W97.186944RLN27.672778W97.212222LN27.576944W97.108611LN27.533333W97.133333LN27.638333W97.237222RCN27.686333W97.294667R007.00 List <List <PointLatLng> > list = new List <List <PointLatLng> >(); List <PointLatLng> pointlist = new List <PointLatLng>(); var matches = all.Matches(BOUND); Console.WriteLine(BOUND); bool isarcterminate = false; bool iscircleterminate = false; int arcdir = 0; PointLatLngAlt pointcent = null; PointLatLngAlt pointstart = null; foreach (Match item in matches) { try { if (item.Groups[2].Value == "L") { var point = new PointLatLngAlt(double.Parse(item.Groups[4].Value, CultureInfo.InvariantCulture), double.Parse(item.Groups[6].Value, CultureInfo.InvariantCulture)); if (item.Groups[3].Value == "S") { point.Lat *= -1; } if (item.Groups[5].Value == "W") { point.Lng *= -1; } if (isarcterminate) { double radius = pointcent.GetDistance(pointstart); double startbearing = pointcent.GetBearing(pointstart); double endbearing = pointcent.GetBearing(point); if (arcdir > 0 && endbearing < startbearing) { endbearing += 360; } if (arcdir < 0) { for (double a = startbearing; a > endbearing; a += (10 * arcdir)) { pointlist.Add(pointcent.newpos(a, radius)); } } else { for (double a = startbearing; a < endbearing; a += (10 * arcdir)) { pointlist.Add(pointcent.newpos(a, radius)); } } pointlist.Add(point); list.Add(pointlist); pointlist = new List <PointLatLng>(); isarcterminate = false; iscircleterminate = false; continue; } if (iscircleterminate) { iscircleterminate = false; continue; } pointlist.Add(point); continue; } else if (item.Groups[7].Value == "A") { pointcent = new PointLatLngAlt(double.Parse(item.Groups[10].Value, CultureInfo.InvariantCulture), double.Parse(item.Groups[12].Value, CultureInfo.InvariantCulture)); if (item.Groups[9].Value == "S") { pointcent.Lat *= -1; } if (item.Groups[11].Value == "W") { pointcent.Lng *= -1; } pointstart = new PointLatLngAlt(double.Parse(item.Groups[14].Value, CultureInfo.InvariantCulture), double.Parse(item.Groups[16].Value, CultureInfo.InvariantCulture)); if (item.Groups[13].Value == "S") { pointstart.Lat *= -1; } if (item.Groups[15].Value == "W") { pointstart.Lng *= -1; } arcdir = item.Groups[8].Value == "+" ? 1 : -1; isarcterminate = true; continue; } else if (item.Groups[17].Value == "C") { var point = new PointLatLngAlt(double.Parse(item.Groups[19].Value, CultureInfo.InvariantCulture), double.Parse(item.Groups[21].Value, CultureInfo.InvariantCulture)); if (item.Groups[18].Value == "S") { point.Lat *= -1; } if (item.Groups[20].Value == "W") { point.Lng *= -1; } // radius in m from nautical miles double radius = double.Parse(item.Groups[22].Value, CultureInfo.InvariantCulture) * 1852; for (int a = 0; a <= 360; a += 10) { pointlist.Add(point.newpos(a, radius)); } list.Add(pointlist); pointlist = new List <PointLatLng>(); iscircleterminate = true; continue; } } catch { } } return(list); }
private void map_MouseUp(object sender, MouseEventArgs e) { MouseDownEnd = map.FromLocalToLatLng(e.X, e.Y); // Console.WriteLine("MainMap MU"); if (e.Button == MouseButtons.Right) // ignore right clicks { return; } if (isMouseDown) // mouse down on some other object and dragged to here. { if (e.Button == MouseButtons.Left) { isMouseDown = false; } if (!isMouseDraging) { if (CurrentGMapMarker != null) { // Redraw polygon //AddDrawPolygon(); } } } isMouseDraging = false; CurrentGMapMarker = null; CurrentGMapMarkerIndex = 0; CurrentGMapMarkerStartPos = null; }
PointLatLngAlt getSRTMAltPath(List <PointLatLngAlt> list, float triangle, ref bool carryon, ref bool up, ref bool down) { PointLatLngAlt answer = new PointLatLngAlt(); PointLatLngAlt last = list[0]; PointLatLngAlt loc = list[1]; double disttotal = 0; int a = 0; double elev = 0; double height = 0; double dist = last.GetDistance(loc); int points = (int)(dist / 10) + 1; double deltalat = (last.Lat - loc.Lat); double deltalng = (last.Lng - loc.Lng); double steplat = deltalat / points; double steplng = deltalng / points; PointLatLngAlt lastpnt = last; while (a <= points && elev < homealt + base_height + height && elev < drone_alt - clearance) { double lat = last.Lat - steplat * a; double lng = last.Lng - steplng * a; var newpoint = new PointLatLngAlt(lat, lng, srtm.getAltitude(lat, lng).alt, ""); double subdist = lastpnt.GetDistance(newpoint); disttotal += subdist; height = disttotal * Math.Tan(triangle); //Height of traingle formed at point with projected line // srtm alts answer = newpoint; elev = newpoint.Alt; lastpnt = newpoint; a++; } if (elev < homealt + base_height + height + 0.1 && elev > homealt + base_height + height - 0.1 && elev < drone_alt - clearance + 0.1 && elev > drone_alt - clearance - 0.1) //Terrain intercept found { carryon = false; } else { if (elev > homealt + base_height + height && elev > drone_alt - clearance) { up = true; down = false; } else { down = true; up = false; } } return(answer); }
private void map_OnMarkerEnter(GMapMarker item) { if (!isMouseDown) { if (item is GMapMarker) { CurrentGMapMarker = item as GMapMarker; CurrentGMapMarkerStartPos = CurrentGMapMarker.Position; } } }
public static async Task <List <PointLatLngAlt> > CreateRotaryAsync(List <PointLatLngAlt> polygon, double altitude, double distance, double spacing, double angle, double overshoot1, double overshoot2, StartPosition startpos, bool shutter, float minLaneSeparation, float leadin, PointLatLngAlt HomeLocation, int clockwise_laps, bool match_spiral_perimeter) { return(await Task.Run((() => CreateRotary(polygon, altitude, distance, spacing, angle, overshoot1, overshoot2, startpos, shutter, minLaneSeparation, leadin, HomeLocation, clockwise_laps, match_spiral_perimeter))).ConfigureAwait(false)); }