public GetTimeGPS ( string gpsline ) : System.DateTime | ||
gpsline | string | |
return | System.DateTime |
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 } }
public void processLine(string line) { try { if (doevent.Second != DateTime.Now.Second) { Application.DoEvents(); doevent = DateTime.Now; } 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; } if (line.ToLower().Contains("ArduRover")) { MainV2.comPort.MAV.cs.firmware = MainV2.Firmwares.ArduRover; } 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[0].Contains("GPS2")) { return; } if (items[DFLog.FindMessageOffset("GPS", "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.FindMessageOffset("GPS", "Alt")], new System.Globalization.CultureInfo("en-US")); position[positionindex].Add(new Point3D(double.Parse(items[DFLog.FindMessageOffset("GPS", "Lng")], new System.Globalization.CultureInfo("en-US")), double.Parse(items[DFLog.FindMessageOffset("GPS", "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("GRAW")) { 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")], new System.Globalization.CultureInfo("en-US")) / -1; runmodel.Orientation.tilt = double.Parse(items[DFLog.FindMessageOffset("ATT", "Pitch")], new System.Globalization.CultureInfo("en-US")) / -1; runmodel.Orientation.heading = double.Parse(items[DFLog.FindMessageOffset("ATT", "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 } }