| public GetTimeGPS ( string gpsline ) : System.DateTime | ||
| gpsline | string | |
| return | System.DateTime | |
public DateTime GetTimeGPS(string gpsline)
{
if (gpsline.StartsWith("GPS") && logformat.Count > 0)
{
string strLine = gpsline.Replace(", ", ",");
strLine = strLine.Replace(": ", ":");
string[] items = strLine.Split(',', ':');
// check its a valid lock
int indexstatus = FindMessageOffset("GPS", "Status");
if (indexstatus != -1)
{
// 3d lock or better
if (items[indexstatus].Trim() == "0" || items[indexstatus].Trim() == "1" ||
items[indexstatus].Trim() == "2")
return DateTime.MinValue;
}
// get time since start of week
int indextimems = FindMessageOffset("GPS", "TimeMS");
if (indextimems == -1)
{
indextimems = FindMessageOffset("GPS", "GMS");
}
// get week number
int indexweek = FindMessageOffset("GPS", "Week");
if (indexweek == -1)
indexweek = FindMessageOffset("GPS", "GWk");
if (indextimems == -1 || indexweek == -1)
return DateTime.MinValue;
try
{
return gpsTimeToTime(int.Parse(items[indexweek]), long.Parse(items[indextimems]) / 1000.0);
}
catch
{
return DateTime.MinValue;
}
}
return DateTime.MinValue;
}
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
{
}
}
if (items[0].Contains("PARM"))
{
try
{
int nameindex = dflog.FindMessageOffset("PARM", "Name");
int valueindex = dflog.FindMessageOffset("PARM", "Value");
paramlist[items[nameindex].Trim()] = items[valueindex].Trim();
} catch
{
}
}
else if (items[0].Contains("CMD")) // "CMD", "QHHHfffffff","TimeUS,CTot,CNum,CId,Prm1,Prm2,Prm3,Prm4,Lat,Lng,Alt" }, \
{
cmdraw.Add(line);
int cidindex = dflog.FindMessageOffset("CMD", "CId");
if (int.Parse(items[cidindex]) <= (int)MAVLink.MAV_CMD.LAST) // wps
{
int cmdflatindex = dflog.FindMessageOffset("CMD", "Lat");
int cmdlngindex = dflog.FindMessageOffset("CMD", "Lng");
int cmdaltindex = dflog.FindMessageOffset("CMD", "Alt");
int cmdnumindex = dflog.FindMessageOffset("CMD", "CNum");
cmd.Add(new PointLatLngAlt(
double.Parse(items[cmdflatindex], CultureInfo.InvariantCulture),
double.Parse(items[cmdlngindex], CultureInfo.InvariantCulture),
double.Parse(items[cmdaltindex], CultureInfo.InvariantCulture), items[cmdnumindex]));
}
}
else if (items[0].Contains("MOD"))
{
positionindex++;
while (modelist.Count < positionindex + 1)
{
modelist.Add("");
}
int nameindex = dflog.FindMessageOffset("MODE", "Mode");
if (nameindex > 0)
{
modelist[positionindex] = items[nameindex];
}
else
{
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
}
}
