public Point3D ConvertToLatLong(double x, double y, double z) { Point3D ans = new Point3D(); ans.X = m_LatLongOrigin.X + ConvertFtToLong(x); ans.Y = m_LatLongOrigin.Y + ConvertFtToLat(y); ans.Z = z / (kmInFt/1000); return ans; }
private void processLine(string line) { try { Application.DoEvents(); line = line.Replace(", ", ","); line = line.Replace(": ", ":"); string[] items = line.Split(',', ':'); if (items[0].Contains("CMD")) { if (flightdata.Count == 0) { if (int.Parse(items[2]) <= (int)MAVLink.MAV_CMD.LAST) // wps { PointLatLngAlt temp = new PointLatLngAlt(double.Parse(items[5], new System.Globalization.CultureInfo("en-US")) / 10000000, double.Parse(items[6], new System.Globalization.CultureInfo("en-US")) / 10000000, double.Parse(items[4], new System.Globalization.CultureInfo("en-US")) / 100, items[1].ToString()); cmd.Add(temp); } } } if (items[0].Contains("MOD")) { positionindex++; modelist.Add(""); // i cant be bothered doing this properly modelist.Add(""); modelist[positionindex] = (items[1]); } if (items[0].Contains("GPS") && items[2] == "1" && items[4] != "0" && items[4] != "-1" && lastline != line) // check gps line and fixed status { if (position[positionindex] == null) position[positionindex] = new List<Point3D>(); double alt = double.Parse(items[6], new System.Globalization.CultureInfo("en-US")); if (items.Length == 11 && items[6] == "0.0000") alt = double.Parse(items[7], new System.Globalization.CultureInfo("en-US")); if (items.Length == 11 && items[6] == "0") alt = double.Parse(items[7], new System.Globalization.CultureInfo("en-US")); position[positionindex].Add(new Point3D(double.Parse(items[5], new System.Globalization.CultureInfo("en-US")), double.Parse(items[4], new System.Globalization.CultureInfo("en-US")), alt)); oldlastpos = lastpos; lastpos = (position[positionindex][position[positionindex].Count - 1]); lastline = line; } if (items[0].Contains("GPS") && items[4] != "0" && items[4] != "-1" && items.Length <= 9) { if (position[positionindex] == null) position[positionindex] = new List<Point3D>(); double alt = double.Parse(items[5], new System.Globalization.CultureInfo("en-US")); position[positionindex].Add(new Point3D(double.Parse(items[4], new System.Globalization.CultureInfo("en-US")), double.Parse(items[3], new System.Globalization.CultureInfo("en-US")), alt)); oldlastpos = lastpos; lastpos = (position[positionindex][position[positionindex].Count - 1]); lastline = line; } if (items[0].Contains("CTUN")) { ctunlast = items; } if (items[0].Contains("NTUN")) { ntunlast = items; line = "ATT:" + double.Parse(ctunlast[3], new System.Globalization.CultureInfo("en-US")) * 100 + "," + double.Parse(ctunlast[6], new System.Globalization.CultureInfo("en-US")) * 100 + "," + double.Parse(items[1], new System.Globalization.CultureInfo("en-US")) * 100; items = line.Split(',', ':'); } if (items[0].Contains("ATT")) { try { if (lastpos.X != 0 && oldlastpos.X != lastpos.X && oldlastpos.Y != lastpos.Y) { Data dat = new Data(); try { dat.datetime = int.Parse(lastline.Split(',', ':')[1]); } catch { } runmodel = new Model(); runmodel.Location.longitude = lastpos.X; runmodel.Location.latitude = lastpos.Y; runmodel.Location.altitude = lastpos.Z; oldlastpos = lastpos; runmodel.Orientation.roll = double.Parse(items[1], new System.Globalization.CultureInfo("en-US")) / -100; runmodel.Orientation.tilt = double.Parse(items[2], new System.Globalization.CultureInfo("en-US")) / -100; runmodel.Orientation.heading = double.Parse(items[3], new System.Globalization.CultureInfo("en-US")) / 100; dat.model = runmodel; dat.ctun = ctunlast; dat.ntun = ntunlast; flightdata.Add(dat); } } catch { } } } catch (Exception) { // if items is to short or parse fails.. ignore } }
public void processLine(string line) { try { doevent++; if ((doevent % 10) == 0) Application.DoEvents(); if (line.Length == 0) return; DateTime start = DateTime.Now; if (line.ToLower().Contains("ArduCopter")) { MainV2.comPort.MAV.cs.firmware = MainV2.Firmwares.ArduCopter2; } if (line.ToLower().Contains("ArduPlane")) { MainV2.comPort.MAV.cs.firmware = MainV2.Firmwares.ArduPlane; } line = line.Replace(", ", ","); line = line.Replace(": ", ":"); string[] items = line.Split(',', ':'); if (items[0].Contains("FMT")) { try { DFLog.FMTLine(line); } catch { } } else if (items[0].Contains("CMD")) { if (flightdata.Count == 0) { if (int.Parse(items[3]) <= (int)MAVLink.MAV_CMD.LAST) // wps { PointLatLngAlt temp = new PointLatLngAlt(double.Parse(items[7], new System.Globalization.CultureInfo("en-US")), double.Parse(items[8], new System.Globalization.CultureInfo("en-US")), double.Parse(items[6], new System.Globalization.CultureInfo("en-US")), items[2].ToString()); cmd.Add(temp); } } } else if (items[0].Contains("MOD")) { positionindex++; while (modelist.Count < positionindex + 1) modelist.Add(""); if (items.Length == 4) { modelist[positionindex] = (items[2]); } else { modelist[positionindex] = (items[1]); } } else if (items[0].Contains("GPS") && DFLog.logformat.ContainsKey("GPS")) { if (items[DFLog.FindInArray(DFLog.logformat["GPS"].FieldNames, "Status")] != "3") return; if (position[positionindex] == null) position[positionindex] = new List<Point3D>(); // if (double.Parse(items[4], new System.Globalization.CultureInfo("en-US")) == 0) // return; // 7 agl // 8 asl... double alt = double.Parse(items[DFLog.FindInArray(DFLog.logformat["GPS"].FieldNames, "Alt")], new System.Globalization.CultureInfo("en-US")); position[positionindex].Add(new Point3D(double.Parse(items[DFLog.FindInArray(DFLog.logformat["GPS"].FieldNames, "Lng")], new System.Globalization.CultureInfo("en-US")), double.Parse(items[DFLog.FindInArray(DFLog.logformat["GPS"].FieldNames, "Lat")], new System.Globalization.CultureInfo("en-US")), alt)); oldlastpos = lastpos; lastpos = (position[positionindex][position[positionindex].Count - 1]); lastline = line; } else if (items[0].Contains("GPS") && items[2] == "1" && items[4] != "0" && items[4] != "-1" && lastline != line) // check gps line and fixed status { MainV2.comPort.MAV.cs.firmware = MainV2.Firmwares.ArduPlane; if (position[positionindex] == null) position[positionindex] = new List<Point3D>(); if (double.Parse(items[4], new System.Globalization.CultureInfo("en-US")) == 0) return; double alt = double.Parse(items[6], new System.Globalization.CultureInfo("en-US")); if (items.Length == 11 && items[6] == "0.0000") alt = double.Parse(items[7], new System.Globalization.CultureInfo("en-US")); if (items.Length == 11 && items[6] == "0") alt = double.Parse(items[7], new System.Globalization.CultureInfo("en-US")); position[positionindex].Add(new Point3D(double.Parse(items[5], new System.Globalization.CultureInfo("en-US")), double.Parse(items[4], new System.Globalization.CultureInfo("en-US")), alt)); oldlastpos = lastpos; lastpos = (position[positionindex][position[positionindex].Count - 1]); lastline = line; } else if (items[0].Contains("GPS") && items[4] != "0" && items[4] != "-1" && items.Length <= 9) // AC { MainV2.comPort.MAV.cs.firmware = MainV2.Firmwares.ArduCopter2; if (position[positionindex] == null) position[positionindex] = new List<Point3D>(); if (double.Parse(items[4], new System.Globalization.CultureInfo("en-US")) == 0) return; double alt = double.Parse(items[5], new System.Globalization.CultureInfo("en-US")); position[positionindex].Add(new Point3D(double.Parse(items[4], new System.Globalization.CultureInfo("en-US")), double.Parse(items[3], new System.Globalization.CultureInfo("en-US")), alt)); oldlastpos = lastpos; lastpos = (position[positionindex][position[positionindex].Count - 1]); lastline = line; } else if ((items[0].Contains("GPS") && items[1] == "3" && items[6] != "0" && items[6] != "-1" && lastline != line && items.Length == 12) || (items[0].Contains("GPS") && items[1] == "3" && items[6] != "0" && items[6] != "-1" && lastline != line && items.Length == 14)) { if (position[positionindex] == null) position[positionindex] = new List<Point3D>(); // if (double.Parse(items[4], new System.Globalization.CultureInfo("en-US")) == 0) // return; // 8 agl // 9 asl... double alt = double.Parse(items[9], new System.Globalization.CultureInfo("en-US")); position[positionindex].Add(new Point3D(double.Parse(items[7], new System.Globalization.CultureInfo("en-US")), double.Parse(items[6], new System.Globalization.CultureInfo("en-US")), alt)); oldlastpos = lastpos; lastpos = (position[positionindex][position[positionindex].Count - 1]); lastline = line; } else if (items[0].Contains("GPS") && items[1] == "3" && items[4] != "0" && items[4] != "-1" && lastline != line && items.Length == 11) // check gps line and fixed status { if (position[positionindex] == null) position[positionindex] = new List<Point3D>(); // if (double.Parse(items[4], new System.Globalization.CultureInfo("en-US")) == 0) // return; // 7 agl // 8 asl... double alt = double.Parse(items[8], new System.Globalization.CultureInfo("en-US")); position[positionindex].Add(new Point3D(double.Parse(items[6], new System.Globalization.CultureInfo("en-US")), double.Parse(items[5], new System.Globalization.CultureInfo("en-US")), alt)); oldlastpos = lastpos; lastpos = (position[positionindex][position[positionindex].Count - 1]); lastline = line; } else if (items[0].Contains("CTUN")) { ctunlast = items; } else if (items[0].Contains("NTUN")) { ntunlast = items; try { // line = "ATT:" + double.Parse(ctunlast[3], new System.Globalization.CultureInfo("en-US")) * 100 + "," + double.Parse(ctunlast[6], new System.Globalization.CultureInfo("en-US")) * 100 + "," + double.Parse(items[1], new System.Globalization.CultureInfo("en-US")) * 100; // items = line.Split(',', ':'); } catch { } } else if (items[0].Contains("ATT")) { try { if (lastpos.X != 0 && oldlastpos.X != lastpos.X && oldlastpos.Y != lastpos.Y) { Data dat = new Data(); try { dat.datetime = DFLog.GetTimeGPS(lastline); } catch { } runmodel = new Model(); runmodel.Location.longitude = lastpos.X; runmodel.Location.latitude = lastpos.Y; runmodel.Location.altitude = lastpos.Z; oldlastpos = lastpos; runmodel.Orientation.roll = double.Parse(items[DFLog.FindInArray(DFLog.logformat["ATT"].FieldNames, "Roll")], new System.Globalization.CultureInfo("en-US")) / -1; runmodel.Orientation.tilt = double.Parse(items[DFLog.FindInArray(DFLog.logformat["ATT"].FieldNames, "Pitch")], new System.Globalization.CultureInfo("en-US")) / -1; runmodel.Orientation.heading = double.Parse(items[DFLog.FindInArray(DFLog.logformat["ATT"].FieldNames, "Yaw")], new System.Globalization.CultureInfo("en-US")) / 1; dat.model = runmodel; dat.ctun = ctunlast; dat.ntun = ntunlast; flightdata.Add(dat); } } catch (Exception ex) { log.Error(ex); } } if ((DateTime.Now - start).TotalMilliseconds > 5) { Console.WriteLine(line); } } catch (Exception) { // if items is to short or parse fails.. ignore } }
private void processLine(string line) { if (CHK_arducopter.Checked) { MainV2.comPort.MAV.cs.firmware = MainV2.Firmwares.ArduCopter2; } else { MainV2.comPort.MAV.cs.firmware = MainV2.Firmwares.ArduPlane; } try { Application.DoEvents(); line = line.Replace(", ", ","); line = line.Replace(": ", ":"); string[] items = line.Split(',', ':'); if (items[0].Contains("CMD")) { if (flightdata.Count == 0) { if (int.Parse(items[3]) <= (int)MAVLink.MAV_CMD.LAST) // wps { PointLatLngAlt temp = new PointLatLngAlt(double.Parse(items[7], new System.Globalization.CultureInfo("en-US")), double.Parse(items[8], new System.Globalization.CultureInfo("en-US")), double.Parse(items[6], new System.Globalization.CultureInfo("en-US")), items[2].ToString()); cmd.Add(temp); } } } if (items[0].Contains("MOD")) { positionindex++; modelist.Add(""); // i cant be bothered doing this properly modelist.Add(""); modelist[positionindex] = (items[1]); } //GPS, 1, 15691, 10, 0.00, -35.3629379, 149.1650850, -0.08, 585.41, 0.00, 126.89 if (items[0].Contains("GPS") && items[2] == "1" && items[4] != "0" && items[4] != "-1" && lastline != line) // check gps line and fixed status { MainV2.comPort.MAV.cs.firmware = MainV2.Firmwares.ArduPlane; if (position[positionindex] == null) position[positionindex] = new List<Point3D>(); if (double.Parse(items[4], new System.Globalization.CultureInfo("en-US")) == 0) return; double alt = double.Parse(items[6], new System.Globalization.CultureInfo("en-US")); if (items.Length == 11 && items[6] == "0.0000") alt = double.Parse(items[7], new System.Globalization.CultureInfo("en-US")); if (items.Length == 11 && items[6] == "0") alt = double.Parse(items[7], new System.Globalization.CultureInfo("en-US")); position[positionindex].Add(new Point3D(double.Parse(items[5], new System.Globalization.CultureInfo("en-US")), double.Parse(items[4], new System.Globalization.CultureInfo("en-US")), alt)); oldlastpos = lastpos; lastpos = (position[positionindex][position[positionindex].Count - 1]); lastline = line; } if (items[0].Contains("GPS") && items[4] != "0" && items[4] != "-1" && items.Length <= 9) // AC { MainV2.comPort.MAV.cs.firmware = MainV2.Firmwares.ArduCopter2; if (position[positionindex] == null) position[positionindex] = new List<Point3D>(); if (double.Parse(items[4], new System.Globalization.CultureInfo("en-US")) == 0) return; double alt = double.Parse(items[5], new System.Globalization.CultureInfo("en-US")); position[positionindex].Add(new Point3D(double.Parse(items[4], new System.Globalization.CultureInfo("en-US")), double.Parse(items[3], new System.Globalization.CultureInfo("en-US")), alt)); oldlastpos = lastpos; lastpos = (position[positionindex][position[positionindex].Count - 1]); lastline = line; } //FMT, 130, 37, GPS, BIHBcLLeeEe, Status,TimeMS,Week,NSats,HDop,Lat,Lng,RelAlt,Alt,Spd,GCrs //GPS, 3, 14716600, 1764, 9, 2.15, 36.3242566, 138.6393205, -0.04, 940.95, 0.02, 138.00 if (items[0].Contains("GPS") && items[1] == "3" && items[6] != "0" && items[6] != "-1" && lastline != line && items.Length == 12) { if (position[positionindex] == null) position[positionindex] = new List<Point3D>(); // if (double.Parse(items[4], new System.Globalization.CultureInfo("en-US")) == 0) // return; // 8 agl // 9 asl... double alt = double.Parse(items[9], new System.Globalization.CultureInfo("en-US")); position[positionindex].Add(new Point3D(double.Parse(items[7], new System.Globalization.CultureInfo("en-US")), double.Parse(items[6], new System.Globalization.CultureInfo("en-US")), alt)); oldlastpos = lastpos; lastpos = (position[positionindex][position[positionindex].Count - 1]); lastline = line; } //GPS, 1, 15691, 10, 0.00, -35.3629379, 149.1650850, -0.08, 585.41, 0.00, 126.89 if (items[0].Contains("GPS") && items[1] == "3" && items[4] != "0" && items[4] != "-1" && lastline != line && items.Length == 11) // check gps line and fixed status { if (position[positionindex] == null) position[positionindex] = new List<Point3D>(); // if (double.Parse(items[4], new System.Globalization.CultureInfo("en-US")) == 0) // return; // 7 agl // 8 asl... double alt = double.Parse(items[8], new System.Globalization.CultureInfo("en-US")); position[positionindex].Add(new Point3D(double.Parse(items[6], new System.Globalization.CultureInfo("en-US")), double.Parse(items[5], new System.Globalization.CultureInfo("en-US")), alt)); oldlastpos = lastpos; lastpos = (position[positionindex][position[positionindex].Count - 1]); lastline = line; } if (items[0].Contains("CTUN")) { ctunlast = items; } if (items[0].Contains("NTUN")) { ntunlast = items; try { // line = "ATT:" + double.Parse(ctunlast[3], new System.Globalization.CultureInfo("en-US")) * 100 + "," + double.Parse(ctunlast[6], new System.Globalization.CultureInfo("en-US")) * 100 + "," + double.Parse(items[1], new System.Globalization.CultureInfo("en-US")) * 100; // items = line.Split(',', ':'); } catch { } } if (items[0].Contains("ATT")) { try { if (lastpos.X != 0 && oldlastpos.X != lastpos.X && oldlastpos.Y != lastpos.Y) { Data dat = new Data(); try { dat.datetime = int.Parse(lastline.Split(',', ':')[2]); } catch { } runmodel = new Model(); runmodel.Location.longitude = lastpos.X; runmodel.Location.latitude = lastpos.Y; runmodel.Location.altitude = lastpos.Z; oldlastpos = lastpos; runmodel.Orientation.roll = double.Parse(items[1], new System.Globalization.CultureInfo("en-US")) / -1; runmodel.Orientation.tilt = double.Parse(items[2], new System.Globalization.CultureInfo("en-US")) / -1; runmodel.Orientation.heading = double.Parse(items[3], new System.Globalization.CultureInfo("en-US")) / 1; dat.model = runmodel; dat.ctun = ctunlast; dat.ntun = ntunlast; flightdata.Add(dat); } } catch { } } } catch (Exception) { // if items is to short or parse fails.. ignore } }
public void writeKML(string filename) { try { writeGPX(filename); writeRinex(filename); } catch { } Color[] colours = { Color.Red, Color.Orange, Color.Yellow, Color.Green, Color.Blue, Color.Indigo, Color.Violet, Color.Pink }; AltitudeMode altmode = AltitudeMode.absolute; KMLRoot kml = new KMLRoot(); Folder fldr = new Folder("Log"); Style style = new Style(); style.Id = "yellowLineGreenPoly"; style.Add(new LineStyle(HexStringToColor("7f00ffff"), 4)); Style style1 = new Style(); style1.Id = "spray"; style1.Add(new LineStyle(HexStringToColor("4c0000ff"), 0)); style1.Add(new PolyStyle() {Color = HexStringToColor("4c0000ff")}); PolyStyle pstyle = new PolyStyle(); pstyle.Color = HexStringToColor("7f00ff00"); style.Add(pstyle); kml.Document.AddStyle(style); kml.Document.AddStyle(style1); int stylecode = 0xff; int g = -1; foreach (List<Point3D> poslist in position) { g++; if (poslist == null) continue; /* List<PointLatLngAlt> pllalist = new List<PointLatLngAlt>(); foreach (var point in poslist) { pllalist.Add(new PointLatLngAlt(point.Y, point.X, point.Z, "")); } var ans = Utilities.LineOffset.GetPolygon(pllalist, 2); while (ans.Count > 0) { var first = ans[0]; var second = ans[1]; var secondlast = ans[ans.Count - 2]; var last = ans[ans.Count-1]; ans.Remove(first); ans.Remove(last); var polycoords = new BoundaryIs(); polycoords.LinearRing = new LinearRing(); polycoords.LinearRing.Coordinates.Add(new Point3D(first.Lng, first.Lat, 1)); polycoords.LinearRing.Coordinates.Add(new Point3D(second.Lng, second.Lat, 1)); polycoords.LinearRing.Coordinates.Add(new Point3D(secondlast.Lng, secondlast.Lat, 1)); polycoords.LinearRing.Coordinates.Add(new Point3D(last.Lng, last.Lat, 1)); polycoords.LinearRing.Coordinates.Add(new Point3D(first.Lng, first.Lat, 1)); //if (!IsClockwise(polycoords.LinearRing.Coordinates)) // polycoords.LinearRing.Coordinates.Reverse(); Polygon kmlpoly = new Polygon() { AltitudeMode = AltitudeMode.relativeToGround, Extrude = false, OuterBoundaryIs = polycoords }; Placemark pmpoly = new Placemark(); pmpoly.Polygon = kmlpoly; pmpoly.name = g + " test"; pmpoly.styleUrl = "#spray"; fldr.Add(pmpoly); } */ LineString ls = new LineString(); ls.AltitudeMode = altmode; ls.Extrude = true; //ls.Tessellate = true; Coordinates coords = new Coordinates(); coords.AddRange(poslist); ls.coordinates = coords; Placemark pm = new Placemark(); string mode = ""; if (g < modelist.Count) mode = modelist[g]; pm.name = g + " Flight Path " + mode; pm.styleUrl = "#yellowLineGreenPoly"; pm.LineString = ls; stylecode = colours[g%(colours.Length - 1)].ToArgb(); Style style2 = new Style(); Color color = Color.FromArgb(0xff, (stylecode >> 16) & 0xff, (stylecode >> 8) & 0xff, (stylecode >> 0) & 0xff); log.Info("colour " + color.ToArgb().ToString("X") + " " + color.ToKnownColor().ToString()); style2.Add(new LineStyle(color, 4)); pm.AddStyle(style2); fldr.Add(pm); } Placemark pmPOS = new Placemark(); pmPOS.name = "POS Message"; pmPOS.LineString = new LineString(); pmPOS.LineString.coordinates = new Coordinates(); Point3D lastPoint3D = new Point3D(); PointLatLngAlt lastplla = PointLatLngAlt.Zero; foreach (var item in PosLatLngAlts) { var newpoint = new Point3D(item.Lng, item.Lat, item.Alt); if (item.GetDistance(lastplla) < 0.1 && lastPoint3D.Z >= (newpoint.Z - 0.3) && lastPoint3D.Z <= (newpoint.Z + 0.3)) continue; pmPOS.LineString.coordinates.Add(newpoint); lastPoint3D = newpoint; lastplla = item; if (pmPOS.LineString.coordinates.Count > 20000) { //add current pmPOS.AddStyle(style); fldr.Add(pmPOS); // create new pmPOS = new Placemark(); pmPOS.name = "POS Message - extra"; pmPOS.LineString = new LineString(); pmPOS.LineString.coordinates = new Coordinates(); lastPoint3D = new Point3D(); lastplla = PointLatLngAlt.Zero; } } pmPOS.AddStyle(style); fldr.Add(pmPOS); Folder planes = new Folder(); planes.name = "Planes"; fldr.Add(planes); Folder waypoints = new Folder(); waypoints.name = "Waypoints"; fldr.Add(waypoints); LineString lswp = new LineString(); lswp.AltitudeMode = AltitudeMode.relativeToGround; lswp.Extrude = true; Coordinates coordswp = new Coordinates(); foreach (PointLatLngAlt p1 in cmd) { if (p1.Lng == 0 && p1.Lat == 0) continue; coordswp.Add(new Point3D(p1.Lng, p1.Lat, p1.Alt)); } lswp.coordinates = coordswp; Placemark pmwp = new Placemark(); pmwp.name = "Waypoints"; //pm.styleUrl = "#yellowLineGreenPoly"; pmwp.LineString = lswp; if (coordswp.Count > 0) waypoints.Add(pmwp); int a = 0; int l = -1; Model lastmodel = null; foreach (Data mod in flightdata) { l++; if (mod.model.Location.latitude == 0) continue; if (lastmodel != null) { if (lastmodel.Location.Equals(mod.model.Location)) { continue; } } Placemark pmplane = new Placemark(); pmplane.name = "Plane " + a; pmplane.visibility = false; Model model = mod.model; model.AltitudeMode = altmode; model.Scale.x = 2; model.Scale.y = 2; model.Scale.z = 2; try { pmplane.description = @"<![CDATA[ <table> <tr><td>Roll: " + model.Orientation.roll + @" </td></tr> <tr><td>Pitch: " + model.Orientation.tilt + @" </td></tr> <tr><td>Yaw: " + model.Orientation.heading + @" </td></tr> <tr><td>WP dist " + mod.ntun[2] + @" </td></tr> <tr><td>tar bear " + mod.ntun[3] + @" </td></tr> <tr><td>nav bear " + mod.ntun[4] + @" </td></tr> <tr><td>alt error " + mod.ntun[5] + @" </td></tr> </table> ]]>"; } catch { } try { pmplane.Point = new KmlPoint((float) model.Location.longitude, (float) model.Location.latitude, (float) model.Location.altitude); pmplane.Point.AltitudeMode = altmode; Link link = new Link(); link.href = "block_plane_0.dae"; model.Link = link; pmplane.Model = model; planes.Add(pmplane); } catch { } // bad lat long value lastmodel = mod.model; a++; } kml.Document.Add(fldr); kml.Save(filename); // create kmz - aka zip file FileStream fs = File.Open(filename.ToLower().Replace(".log.kml", ".kmz").Replace(".bin.kml", ".kmz"), FileMode.Create); ZipOutputStream zipStream = new ZipOutputStream(fs); zipStream.SetLevel(9); //0-9, 9 being the highest level of compression zipStream.UseZip64 = UseZip64.Off; // older zipfile // entry 1 string entryName = ZipEntry.CleanName(Path.GetFileName(filename)); // Removes drive from name and fixes slash direction ZipEntry newEntry = new ZipEntry(entryName); newEntry.DateTime = DateTime.Now; zipStream.PutNextEntry(newEntry); // Zip the file in buffered chunks // the "using" will close the stream even if an exception occurs byte[] buffer = new byte[4096]; using (FileStream streamReader = File.OpenRead(filename)) { StreamUtils.Copy(streamReader, zipStream, buffer); } zipStream.CloseEntry(); File.Delete(filename); filename = Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + "block_plane_0.dae"; // entry 2 entryName = ZipEntry.CleanName(Path.GetFileName(filename)); // Removes drive from name and fixes slash direction newEntry = new ZipEntry(entryName); newEntry.DateTime = DateTime.Now; zipStream.PutNextEntry(newEntry); // Zip the file in buffered chunks // the "using" will close the stream even if an exception occurs buffer = new byte[4096]; using (FileStream streamReader = File.OpenRead(filename)) { StreamUtils.Copy(streamReader, zipStream, buffer); } zipStream.CloseEntry(); zipStream.IsStreamOwner = true; // Makes the Close also Close the underlying stream zipStream.Close(); positionindex = 0; modelist.Clear(); flightdata.Clear(); position = new List<Core.Geometry.Point3D>[200]; cmd.Clear(); }
public void processLine(string line) { try { if (line.Length == 0) return; string[] items = line.Split(',', ':'); if (items[0].Contains("FMT")) { try { dflog.FMTLine(line); } catch { } } else if (items[0].Contains("CMD")) { if (flightdata.Count == 0) { if (int.Parse(items[3]) <= (int) MAVLink.MAV_CMD.LAST) // wps { PointLatLngAlt temp = new PointLatLngAlt( double.Parse(items[7], CultureInfo.InvariantCulture), double.Parse(items[8], CultureInfo.InvariantCulture), double.Parse(items[6], CultureInfo.InvariantCulture), items[2].ToString()); cmd.Add(temp); } } } else if (items[0].Contains("MOD")) { positionindex++; while (modelist.Count < positionindex + 1) modelist.Add(""); if (items.Length == 4) { modelist[positionindex] = (items[2]); } else { modelist[positionindex] = (items[1]); } } else if (items[0].Contains("GPS") && dflog.logformat.ContainsKey("GPS")) { if (items[0].Contains("GPS2")) return; if (int.Parse(items[dflog.FindMessageOffset("GPS", "Status")]) < 3) return; if (position[positionindex] == null) position[positionindex] = new List<Point3D>(); double alt = double.Parse(items[dflog.FindMessageOffset("GPS", "Alt")], CultureInfo.InvariantCulture); if (alt > 40000) return; double lng = double.Parse(items[dflog.FindMessageOffset("GPS", "Lng")], CultureInfo.InvariantCulture); double lat = double.Parse(items[dflog.FindMessageOffset("GPS", "Lat")], CultureInfo.InvariantCulture); if (lat < -90 || lat > 90) return; if (lng < -180 || lng > 180) return; position[positionindex].Add(new Point3D(lng, lat, alt)); oldlastpos = lastpos; lastpos = (position[positionindex][position[positionindex].Count - 1]); lastline = line; } else if (items[0].Contains("POS")) { if (dflog.logformat.ContainsKey("POS")) { int poslatindex = dflog.FindMessageOffset("POS", "Lat"); int poslngindex = dflog.FindMessageOffset("POS", "Lng"); int posaltindex = dflog.FindMessageOffset("POS", "Alt"); PosLatLngAlts.Add( new PointLatLngAlt( double.Parse(items[poslatindex], CultureInfo.InvariantCulture), double.Parse(items[poslngindex], CultureInfo.InvariantCulture), double.Parse(items[posaltindex], CultureInfo.InvariantCulture))); } } else if (items[0].Contains("GRAW")) { gpsrawdata.Add(line); } else if (items[0].Contains("GRXH")) { gpsrawdata.Add(line); } else if (items[0].Contains("GRXS")) { gpsrawdata.Add(line); } else if (items[0].Contains("CTUN")) { ctunlast = items; } else if (items[0].Contains("NTUN")) { ntunlast = items; try { // line = "ATT:" + double.Parse(ctunlast[3], new System.Globalization.CultureInfo("en-US")) * 100 + "," + double.Parse(ctunlast[6], new System.Globalization.CultureInfo("en-US")) * 100 + "," + double.Parse(items[1], new System.Globalization.CultureInfo("en-US")) * 100; // items = line.Split(',', ':'); } catch { } } else if (items[0].Contains("ATT")) { try { if (lastpos.X != 0 && oldlastpos.X != lastpos.X && oldlastpos.Y != lastpos.Y) { Data dat = new Data(); try { dat.datetime = dflog.GetTimeGPS(lastline); } catch { } runmodel = new Model(); runmodel.Location.longitude = lastpos.X; runmodel.Location.latitude = lastpos.Y; runmodel.Location.altitude = lastpos.Z; oldlastpos = lastpos; runmodel.Orientation.roll = double.Parse(items[dflog.FindMessageOffset("ATT", "Roll")],CultureInfo.InvariantCulture) / -1; runmodel.Orientation.tilt = double.Parse(items[dflog.FindMessageOffset("ATT", "Pitch")],CultureInfo.InvariantCulture) / -1; runmodel.Orientation.heading = double.Parse(items[dflog.FindMessageOffset("ATT", "Yaw")],CultureInfo.InvariantCulture) / 1; dat.model = runmodel; dat.ctun = ctunlast; dat.ntun = ntunlast; flightdata.Add(dat); } } catch (Exception ex) { log.Error(ex); } } } catch (Exception) { // if items is to short or parse fails.. ignore } }
public static Point3D MakePointFromStr(string str) { Point3D ans = new Point3D(); ans.Deserialize(str); return ans; }
public Point3D(Point3D xyz) { m_X = xyz.m_X; m_Y = xyz.m_Y; m_Z = xyz.m_Z; }
public bool SameCoordsAs(Point3D point3D) { return m_X == point3D.m_X && m_Y == point3D.m_Y && m_Z == point3D.m_Z; }