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