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;
}
