public void processLine(string line, bool arducopter)
{
if (arducopter)
{
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 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
}
}
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
}
}
