public static void ProcessFile(string logfile) { if (File.Exists(logfile + ".jpg")) { return; } double minx = 99999; double maxx = -99999; double miny = 99999; double maxy = -99999; bool sitl = false; Dictionary <int, List <PointLatLngAlt> > loc_list = new Dictionary <int, List <PointLatLngAlt> >(); try { if (logfile.ToLower().EndsWith(".tlog")) { Comms.CommsFile cf = new CommsFile(); cf.Open(logfile); using (CommsStream cs = new CommsStream(cf, cf.BytesToRead)) { MAVLink.MavlinkParse parse = new MAVLink.MavlinkParse(true); while (cs.Position < cs.Length) { MAVLink.MAVLinkMessage packet = parse.ReadPacket(cs); if (packet == null || packet.Length < 5) { continue; } if (packet.msgid == (byte)MAVLink.MAVLINK_MSG_ID.SIM_STATE || packet.msgid == (byte)MAVLink.MAVLINK_MSG_ID.SIMSTATE) { sitl = true; } if (packet.msgid == (byte)MAVLink.MAVLINK_MSG_ID.GLOBAL_POSITION_INT) { var loc = packet.ToStructure <MAVLink.mavlink_global_position_int_t>(); if (loc.lat == 0 || loc.lon == 0) { continue; } var id = packet.sysid * 256 + packet.compid; if (!loc_list.ContainsKey(id)) { loc_list[id] = new List <PointLatLngAlt>(); } loc_list[id].Add(new PointLatLngAlt(loc.lat / 10000000.0f, loc.lon / 10000000.0f)); minx = Math.Min(minx, loc.lon / 10000000.0f); maxx = Math.Max(maxx, loc.lon / 10000000.0f); miny = Math.Min(miny, loc.lat / 10000000.0f); maxy = Math.Max(maxy, loc.lat / 10000000.0f); } } } } else if (logfile.ToLower().EndsWith(".bin") || logfile.ToLower().EndsWith(".log")) { using ( CollectionBuffer colbuf = new CollectionBuffer(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.ReadWrite)) ) { loc_list[0] = new List <PointLatLngAlt>(); foreach (var item in colbuf.GetEnumeratorType("GPS")) { if (item.msgtype.StartsWith("GPS")) { if (!colbuf.dflog.logformat.ContainsKey("GPS")) { continue; } var status = double.Parse(item.items[colbuf.dflog.FindMessageOffset(item.msgtype, "Status")]); var lat = double.Parse(item.items[colbuf.dflog.FindMessageOffset(item.msgtype, "Lat")]); var lon = double.Parse(item.items[colbuf.dflog.FindMessageOffset(item.msgtype, "Lng")]); if (lat == 0 || lon == 0 || status < 3) { continue; } loc_list[0].Add(new PointLatLngAlt(lat, lon)); minx = Math.Min(minx, lon); maxx = Math.Max(maxx, lon); miny = Math.Min(miny, lat); maxy = Math.Max(maxy, lat); } } } } if (loc_list.Count > 0 && loc_list.First().Value.Count > 10) { // add a bit of buffer var area = RectLatLng.FromLTRB(minx - 0.001, maxy + 0.001, maxx + 0.001, miny - 0.001); using (var map = GetMap(area)) using (var grap = Graphics.FromImage(map)) { if (sitl) { AddTextToMap(grap, "SITL"); } Color[] colours = { Color.Red, Color.Orange, Color.Yellow, Color.Green, Color.Blue, Color.Indigo, Color.Violet, Color.Pink }; int a = 0; foreach (var locs in loc_list.Values) { PointF lastpoint = new PointF(); var pen = new Pen(colours[a % (colours.Length - 1)]); foreach (var loc in locs) { PointF newpoint = GetPixel(area, loc, map.Size); if (!lastpoint.IsEmpty) { grap.DrawLine(pen, lastpoint, newpoint); } lastpoint = newpoint; } a++; } map.Save(logfile + ".jpg", System.Drawing.Imaging.ImageFormat.Jpeg); File.SetLastWriteTime(logfile + ".jpg", new FileInfo(logfile).LastWriteTime); } } else { DoTextMap(logfile + ".jpg", "No gps data"); File.SetLastWriteTime(logfile + ".jpg", new FileInfo(logfile).LastWriteTime); } } catch (Exception ex) { if (ex.ToString().Contains("Mavlink 0.9")) { DoTextMap(logfile + ".jpg", "Old log\nMavlink 0.9"); } } }
void processbg(string file) { a++; Loading.ShowLoading(a + "/" + files.Count + " " + file, this); if (!File.Exists(file + ".jpg")) { LogMap.MapLogs(new string[] { file }); } var loginfo = new loginfo(); loginfo.fullname = file; try { // file not found exception even though it passes the exists check above. loginfo.Size = new FileInfo(file).Length; } catch { } if (File.Exists(file + ".jpg")) { loginfo.imgfile = file + ".jpg"; } if (file.ToLower().EndsWith(".tlog")) { using (MAVLinkInterface mine = new MAVLinkInterface()) { try { mine.logplaybackfile = new BinaryReader(File.Open(file, FileMode.Open, FileAccess.Read, FileShare.Read)); } catch (Exception ex) { log.Debug(ex.ToString()); CustomMessageBox.Show("Log Can not be opened. Are you still connected?"); return; } mine.logreadmode = true; mine.speechenabled = false; // file is to small if (mine.logplaybackfile.BaseStream.Length < 1024 * 4) { return; } mine.getHeartBeat(); loginfo.Date = mine.lastlogread; loginfo.Aircraft = mine.sysidcurrent; loginfo.Frame = mine.MAV.aptype.ToString(); var start = mine.lastlogread; try { mine.logplaybackfile.BaseStream.Seek(0, SeekOrigin.Begin); } catch { } var end = mine.lastlogread; var length = mine.logplaybackfile.BaseStream.Length; var a = 0; // abandon last 100 bytes while (mine.logplaybackfile.BaseStream.Position < (length - 100)) { var packet = mine.readPacket(); // gcs if (packet.sysid == 255) { continue; } if (packet.msgid == (uint)MAVLink.MAVLINK_MSG_ID.CAMERA_FEEDBACK) { loginfo.CamMSG++; } if (a % 10 == 0) { mine.MAV.cs.UpdateCurrentSettings(null, true, mine); } a++; if (mine.lastlogread > end) { end = mine.lastlogread; } } loginfo.Home = mine.MAV.cs.Location; loginfo.TimeInAir = mine.MAV.cs.timeInAir; loginfo.DistTraveled = mine.MAV.cs.distTraveled; loginfo.Duration = (end - start).ToString(); } } else if (file.ToLower().EndsWith(".bin") || file.ToLower().EndsWith(".log")) { using (CollectionBuffer colbuf = new CollectionBuffer(File.OpenRead(file))) { PointLatLngAlt lastpos = null; DateTime start = DateTime.MinValue; DateTime end = DateTime.MinValue; DateTime tia = DateTime.MinValue; // set time in air/home/distancetraveled foreach (var dfItem in colbuf.GetEnumeratorType("GPS")) { if (dfItem["Status"] != null) { var status = int.Parse(dfItem["Status"]); if (status >= 3) { var pos = new PointLatLngAlt( double.Parse(dfItem["Lat"], CultureInfo.InvariantCulture), double.Parse(dfItem["Lng"], CultureInfo.InvariantCulture), double.Parse(dfItem["Alt"], CultureInfo.InvariantCulture)); if (lastpos == null) { lastpos = pos; } if (start == DateTime.MinValue) { loginfo.Date = dfItem.time; start = dfItem.time; } end = dfItem.time; // add distance loginfo.DistTraveled += (float)lastpos.GetDistance(pos); // set home if (loginfo.Home == null) { loginfo.Home = pos; } if (dfItem.time > tia.AddSeconds(1)) { // ground speed > 0.2 or alt > homelat+2 if (double.Parse(dfItem["Spd"], CultureInfo.InvariantCulture) > 0.2 || pos.Alt > (loginfo.Home.Alt + 2)) { loginfo.TimeInAir++; } tia = dfItem.time; } } } } loginfo.Duration = (end - start).ToString(); loginfo.CamMSG = colbuf.GetEnumeratorType("CAM").Count(); loginfo.Aircraft = 0; //colbuf.dflog.param[""]; loginfo.Frame = "Unknown"; //mine.MAV.aptype.ToString(); } } lock (logs) logs.Add(loginfo); }
public static List <Tuple <DFLog.DFItem, double> > ProcessExpression(DFLog dflog, CollectionBuffer logdata, string expression) { List <Tuple <DFLog.DFItem, double> > answer = new List <Tuple <DFLog.DFItem, double> >(); Dictionary <string, List <string> > fieldsUsed = new Dictionary <string, List <string> >(); var fieldmatchs = Regex.Matches(expression, @"(([A-z0-9_]{2,20})\.([A-z0-9_]+))"); if (fieldmatchs.Count > 0) { foreach (Match match in fieldmatchs) { var type = match.Groups[2].Value.ToString(); var field = match.Groups[3].Value.ToString(); if (!fieldsUsed.ContainsKey(type)) { fieldsUsed[type] = new List <string>(); } fieldsUsed[type].Add(field); } } Function f = new Function("wrap_360(x) = (x+360) # 360"); Function f1 = new Function("degrees(x) = x*57.295779513"); Function f2 = new Function("atan2", new atan2()); Function f3 = new Function("lowpass", new lowpass()); Function f4 = new Function("delta", new deltaF()); // fix maths operators var filter1 = expression.Replace("%", "#").Replace("**", "^").Replace(":2", ""); //convert paramnames to remove . var filter2 = Regex.Replace(filter1, @"(([A-z0-9_]{2,20})\.([A-z0-9_]+))", match => match.Groups[2].ToString() + match.Groups[3]); // convert strings to long var filter3 = Regex.Replace(filter2, @"([""']{1}[^""]+[""']{1})", match => BitConverter.ToUInt64(match.Groups[0].Value.MakeBytesSize(8), 0).ToString()); Expression e = new Expression(filter3); e.addFunctions(f); e.addFunctions(f1); e.addFunctions(f2); e.addFunctions(f3); e.addFunctions(f4); foreach (var item in fieldsUsed) { foreach (var value in item.Value) { Argument x = new Argument(item.Key + "" + value); e.addArguments(x); } } bool bad = false; try { mXparser.consolePrintTokens(e.getCopyOfInitialTokens()); if (!e.checkSyntax()) { bad = true; } } catch { bad = true; } if (!bad) { foreach (var line in logdata.GetEnumeratorType(fieldsUsed.Keys.ToArray())) { foreach (var item in fieldsUsed) { foreach (var value in item.Value.Distinct()) { e.setArgumentValue(item.Key + "" + value, double.Parse(line.items[dflog.FindMessageOffset(item.Key, value)])); } } answer.Add(line, e.calculate()); } } if (answer.Count > 0) { return(answer); } //earth_accel_df(IMU2,ATT).x if (expression.Contains("earth_accel_df")) { var matchs = Regex.Matches(expression, @"([A-z0-9_]+),([A-z0-9_]+)"); List <string> msglist = new List <string>(); foreach (Match match in matchs) { foreach (var item in match.Groups) { msglist.Add(item.ToString()); } } IMU_t IMU = new IMU_t(); ATT_t ATT = new ATT_t(); foreach (var item in logdata.GetEnumeratorType(msglist.ToArray())) { if (item.msgtype == "ATT") { ATT.Roll = double.Parse(item.items[dflog.FindMessageOffset("ATT", "Roll")]); ATT.Pitch = double.Parse(item.items[dflog.FindMessageOffset("ATT", "Pitch")]); ATT.Yaw = double.Parse(item.items[dflog.FindMessageOffset("ATT", "Yaw")]); } else if (item.msgtype == "IMU") { IMU.AccX = double.Parse(item.items[dflog.FindMessageOffset("IMU", "AccX")]); IMU.AccY = double.Parse(item.items[dflog.FindMessageOffset("IMU", "AccY")]); IMU.AccZ = double.Parse(item.items[dflog.FindMessageOffset("IMU", "AccZ")]); } else if (item.msgtype == "IMU2") { IMU.AccX = double.Parse(item.items[dflog.FindMessageOffset("IMU2", "AccX")]); IMU.AccY = double.Parse(item.items[dflog.FindMessageOffset("IMU2", "AccY")]); IMU.AccZ = double.Parse(item.items[dflog.FindMessageOffset("IMU2", "AccZ")]); } if (expression.Contains(".x")) { answer.Add(item, earth_accel_df(IMU, ATT).x); } if (expression.Contains(".y")) { answer.Add(item, earth_accel_df(IMU, ATT).y); } if (expression.Contains(".z")) { answer.Add(item, earth_accel_df(IMU, ATT).z); } } } // delta(gps_velocity_df(GPS).x,'x',GPS.TimeUS) else if (expression.Contains("delta(gps_velocity_df(GPS)")) { foreach (var item in logdata.GetEnumeratorType("GPS")) { var GPS = new GPS_t(); if (item.msgtype == "GPS") { GPS.Spd = double.Parse(item.items[dflog.FindMessageOffset("GPS", "Spd")]); GPS.GCrs = double.Parse(item.items[dflog.FindMessageOffset("GPS", "GCrs")]); GPS.VZ = double.Parse(item.items[dflog.FindMessageOffset("GPS", "VZ")]); } if (expression.Contains(".x")) { answer.Add(item, delta(gps_velocity_df(GPS).x, "x", item.timems * 1000)); } else if (expression.Contains(".y")) { answer.Add(item, delta(gps_velocity_df(GPS).y, "y", item.timems * 1000)); } else if (expression.Contains(".z")) { answer.Add(item, delta(gps_velocity_df(GPS).z, "z", item.timems * 1000) - 9.8); } } } else if (expression.Contains("delta(gps_velocity_df(GPS2)")) { foreach (var item in logdata.GetEnumeratorType("GPS2")) { var GPS = new GPS_t(); if (item.msgtype == "GPS2") { GPS.Spd = double.Parse(item.items[dflog.FindMessageOffset("GPS2", "Spd")]); GPS.GCrs = double.Parse(item.items[dflog.FindMessageOffset("GPS2", "GCrs")]); GPS.VZ = double.Parse(item.items[dflog.FindMessageOffset("GPS2", "VZ")]); } if (expression.Contains(".x")) { answer.Add(item, delta(gps_velocity_df(GPS).x, "x", item.timems * 1000)); } else if (expression.Contains(".y")) { answer.Add(item, delta(gps_velocity_df(GPS).y, "y", item.timems * 1000)); } else if (expression.Contains(".z")) { answer.Add(item, delta(gps_velocity_df(GPS).z, "z", item.timems * 1000) - 9.8); } } } else if (expression.StartsWith("degrees")) { var matchs = Regex.Matches(expression, @"([A-z0-9_]+)\.([A-z0-9_]+)"); if (matchs.Count > 0) { var type = matchs[0].Groups[1].Value.ToString(); var field = matchs[0].Groups[2].Value.ToString(); foreach (var item in logdata.GetEnumeratorType(type)) { answer.Add(item, degrees(double.Parse(item.items[dflog.FindMessageOffset(type, field)]))); } } } else if (expression.StartsWith("sqrt")) { // there are alot of assumptions made in this code Dictionary <int, double> work = new Dictionary <int, double>(); List <KeyValuePair <string, string> > types = new List <KeyValuePair <string, string> >(); var matchs = Regex.Matches(expression, @"(([A-z0-9_]+)\.([A-z0-9_]+)\*\*2)"); if (matchs.Count > 0) { foreach (Match match in matchs) { var type = match.Groups[2].Value.ToString(); var field = match.Groups[3].Value.ToString(); types.Add(new KeyValuePair <string, string>(type, field)); } List <string> keyarray = new List <string>(); types.ForEach(g => { keyarray.Add(g.Key); }); List <string> valuearray = new List <string>(); types.ForEach(g => { valuearray.Add(g.Value); }); foreach (var item in logdata.GetEnumeratorType(keyarray.ToArray())) { for (int a = 0; a < types.Count; a++) { var key = keyarray[a]; var value = valuearray[a]; var offset = dflog.FindMessageOffset(key, value); if (offset == -1) { continue; } var ans = logdata.GetUnit(key, value); string unit = ans.Item1; double multiplier = ans.Item2; work[a] = double.Parse(item.items[offset]) * multiplier; } double workanswer = 0; foreach (var value in work.Values) { workanswer += Math.Pow(value, 2); } answer.Add(item, Math.Sqrt(workanswer)); } } } else if (expression.Contains("*")) // ATT.DesRoll*ATT.Roll { var matchs = Regex.Matches(expression, @"([A-z0-9_]+)\.([A-z0-9_]+)\*([A-z0-9_]+)\.([A-z0-9_]+)"); if (matchs.Count > 0) { var type = matchs[0].Groups[1].Value.ToString(); var field = matchs[0].Groups[2].Value.ToString(); var type2 = matchs[0].Groups[3].Value.ToString(); var field2 = matchs[0].Groups[4].Value.ToString(); foreach (var item in logdata.GetEnumeratorType(new[] { type, type2 })) { if (type == type2) { var idx1 = dflog.FindMessageOffset(type, field); var idx2 = dflog.FindMessageOffset(type2, field2); if (idx1 == -1 || idx2 == -1) { break; } answer.Add(item, double.Parse(item.items[idx1]) * double.Parse(item.items[idx2])); } } } } else if (expression.Contains("-")) // ATT.DesRoll-ATT.Roll { var matchs = Regex.Matches(expression, @"([A-z0-9_]+)\.([A-z0-9_]+)-([A-z0-9_]+)\.([A-z0-9_]+)"); if (matchs.Count > 0) { var type = matchs[0].Groups[1].Value.ToString(); var field = matchs[0].Groups[2].Value.ToString(); var type2 = matchs[0].Groups[3].Value.ToString(); var field2 = matchs[0].Groups[4].Value.ToString(); foreach (var item in logdata.GetEnumeratorType(new[] { type, type2 })) { if (type == type2) { var idx1 = dflog.FindMessageOffset(type, field); var idx2 = dflog.FindMessageOffset(type2, field2); if (idx1 == -1 || idx2 == -1) { break; } answer.Add(item, double.Parse(item.items[idx1]) - double.Parse(item.items[idx2])); } } } } else if (expression.Contains("mag_heading_df")) { var matchs = Regex.Matches(expression, @"([A-z0-9_]+),([A-z0-9_]+)"); List <string> msglist = new List <string>(); foreach (Match match in matchs) { foreach (var item in match.Groups) { msglist.Add(item.ToString()); } } var MAG = new MAG_t(); var ATT = new ATT_t(); foreach (var item in logdata.GetEnumeratorType(msglist.ToArray())) { if (item.msgtype.StartsWith("MAG")) { MAG.MagX = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "MagX")]); MAG.MagY = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "MagY")]); MAG.MagZ = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "MagZ")]); MAG.OfsX = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "OfsX")]); MAG.OfsY = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "OfsY")]); MAG.OfsZ = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "OfsZ")]); } else if (item.msgtype == "ATT") { ATT.Roll = double.Parse(item.items[dflog.FindMessageOffset("ATT", "Roll")]); ATT.Pitch = double.Parse(item.items[dflog.FindMessageOffset("ATT", "Pitch")]); ATT.Yaw = double.Parse(item.items[dflog.FindMessageOffset("ATT", "Yaw")]); } answer.Add(item, mag_heading_df(MAG, ATT)); } } return(answer); }
public static PointPairList ProcessExpression(ref DFLog dflog, ref CollectionBuffer logdata, string expression) { PointPairList answer = new PointPairList(); //earth_accel_df(IMU2,ATT).x if (expression.Contains("earth_accel_df")) { var matchs = Regex.Matches(expression, @"([A-z0-9_]+),([A-z0-9_]+)"); List <string> msglist = new List <string>(); foreach (Match match in matchs) { foreach (var item in match.Groups) { msglist.Add(item.ToString()); } } foreach (var item in logdata.GetEnumeratorType(msglist.ToArray())) { IMU imu = new IMU(); ATT att = new ATT(); if (item.msgtype == "ATT") { ATT.Roll = double.Parse(item.items[dflog.FindMessageOffset("ATT", "Roll")]); ATT.Pitch = double.Parse(item.items[dflog.FindMessageOffset("ATT", "Pitch")]); ATT.Yaw = double.Parse(item.items[dflog.FindMessageOffset("ATT", "Yaw")]); } else if (item.msgtype == "IMU") { IMU.AccX = double.Parse(item.items[dflog.FindMessageOffset("IMU", "AccX")]); IMU.AccY = double.Parse(item.items[dflog.FindMessageOffset("IMU", "AccY")]); IMU.AccZ = double.Parse(item.items[dflog.FindMessageOffset("IMU", "AccZ")]); } else if (item.msgtype == "IMU2") { IMU.AccX = double.Parse(item.items[dflog.FindMessageOffset("IMU2", "AccX")]); IMU.AccY = double.Parse(item.items[dflog.FindMessageOffset("IMU2", "AccY")]); IMU.AccZ = double.Parse(item.items[dflog.FindMessageOffset("IMU2", "AccZ")]); } if (expression.Contains(".x")) { answer.Add(item.lineno, earth_accel_df(imu, att).x); } if (expression.Contains(".y")) { answer.Add(item.lineno, earth_accel_df(imu, att).y); } if (expression.Contains(".z")) { answer.Add(item.lineno, earth_accel_df(imu, att).z); } } } // delta(gps_velocity_df(GPS).x,'x',GPS.TimeUS) else if (expression.Contains("delta(gps_velocity_df(GPS)")) { foreach (var item in logdata.GetEnumeratorType("GPS")) { var gps = new GPS(); if (item.msgtype == "GPS") { GPS.Spd = double.Parse(item.items[dflog.FindMessageOffset("GPS", "Spd")]); GPS.GCrs = double.Parse(item.items[dflog.FindMessageOffset("GPS", "GCrs")]); GPS.VZ = double.Parse(item.items[dflog.FindMessageOffset("GPS", "VZ")]); } if (expression.Contains(".x")) { answer.Add(item.lineno, delta(gps_velocity_df(gps).x, "x", item.timems * 1000)); } else if (expression.Contains(".y")) { answer.Add(item.lineno, delta(gps_velocity_df(gps).y, "y", item.timems * 1000)); } else if (expression.Contains(".z")) { answer.Add(item.lineno, delta(gps_velocity_df(gps).z, "z", item.timems * 1000) - 9.8); } } } else if (expression.StartsWith("degrees")) { var matchs = Regex.Matches(expression, @"([A-z0-9_]+)\.([A-z0-9_]+)"); if (matchs.Count > 0) { var type = matchs[0].Groups[1].Value.ToString(); var field = matchs[0].Groups[2].Value.ToString(); foreach (var item in logdata.GetEnumeratorType(type)) { answer.Add(item.lineno, degrees(double.Parse(item.items[dflog.FindMessageOffset(type, field)]))); } } } else if (expression.StartsWith("sqrt")) { // there are alot of assumptions made in this code Dictionary <int, double> work = new Dictionary <int, double>(); List <KeyValuePair <string, string> > types = new EventList <KeyValuePair <string, string> >(); var matchs = Regex.Matches(expression, @"(([A-z0-9_]+)\.([A-z0-9_]+)\*\*2)"); if (matchs.Count > 0) { foreach (Match match in matchs) { var type = match.Groups[2].Value.ToString(); var field = match.Groups[3].Value.ToString(); types.Add(new KeyValuePair <string, string>(type, field)); } List <string> keyarray = new List <string>(); types.ForEach(g => { keyarray.Add(g.Key); }); List <string> valuearray = new List <string>(); types.ForEach(g => { valuearray.Add(g.Value); }); foreach (var item in logdata.GetEnumeratorType(keyarray.ToArray())) { for (int a = 0; a < types.Count; a++) { var key = keyarray[a]; var value = valuearray[a]; var offset = dflog.FindMessageOffset(key, value); if (offset == -1) { continue; } work[a] = double.Parse(item.items[offset]); } double workanswer = 0; foreach (var value in work.Values) { workanswer += Math.Pow(value, 2); } answer.Add(item.lineno, Math.Sqrt(workanswer)); } } } return(answer); }
public static void Create(string filein, string fileout, List <string> fmtList = null) { using (StreamReader tr = new StreamReader(filein)) using (CollectionBuffer logdata = new CollectionBuffer(tr.BaseStream)) { List <string> colList = new List <string>(); Dictionary <string, int> colStart = new Dictionary <string, int>(); colStart.Add("GLOBAL", colList.Count); colList.Add("GLOBAL_TimeMS"); foreach (var logformatValue in logdata.dflog.logformat.Values) { if (fmtList != null && !fmtList.Contains(logformatValue.Name)) { continue; } colStart.Add(logformatValue.Name, colList.Count); foreach (var field in logformatValue.FieldNames) { colList.Add(logformatValue.Name + "_" + field); } } using (StreamWriter sr = new StreamWriter(fileout)) { // header foreach (var item in colList) { sr.Write(item + ","); } sr.WriteLine(); // lines foreach (var dfitem in logdata.GetEnumeratorType(colStart.Keys.ToArray())) { if (dfitem.msgtype == "FMT") { continue; } var idx = 0; StringBuilder sb = new StringBuilder(); sb.Append(dfitem.timems); idx++; sb.Append(','); var start = colStart[dfitem.msgtype]; while (idx < start) { idx++; sb.Append(','); } foreach (var item in dfitem.items.Skip(1)) { sb.Append(item?.Trim()); idx++; sb.Append(','); } sr.WriteLine(sb); } } } }
public static List <Tuple <DFLog.DFItem, double> > ProcessExpression(ref DFLog dflog, ref CollectionBuffer logdata, string expression) { List <Tuple <DFLog.DFItem, double> > answer = new List <Tuple <DFLog.DFItem, double> >(); //earth_accel_df(IMU2,ATT).x if (expression.Contains("earth_accel_df")) { var matchs = Regex.Matches(expression, @"([A-z0-9_]+),([A-z0-9_]+)"); List <string> msglist = new List <string>(); foreach (Match match in matchs) { foreach (var item in match.Groups) { msglist.Add(item.ToString()); } } IMU_t IMU = new IMU_t(); ATT_t ATT = new ATT_t(); foreach (var item in logdata.GetEnumeratorType(msglist.ToArray())) { if (item.msgtype == "ATT") { ATT.Roll = double.Parse(item.items[dflog.FindMessageOffset("ATT", "Roll")]); ATT.Pitch = double.Parse(item.items[dflog.FindMessageOffset("ATT", "Pitch")]); ATT.Yaw = double.Parse(item.items[dflog.FindMessageOffset("ATT", "Yaw")]); } else if (item.msgtype == "IMU") { IMU.AccX = double.Parse(item.items[dflog.FindMessageOffset("IMU", "AccX")]); IMU.AccY = double.Parse(item.items[dflog.FindMessageOffset("IMU", "AccY")]); IMU.AccZ = double.Parse(item.items[dflog.FindMessageOffset("IMU", "AccZ")]); } else if (item.msgtype == "IMU2") { IMU.AccX = double.Parse(item.items[dflog.FindMessageOffset("IMU2", "AccX")]); IMU.AccY = double.Parse(item.items[dflog.FindMessageOffset("IMU2", "AccY")]); IMU.AccZ = double.Parse(item.items[dflog.FindMessageOffset("IMU2", "AccZ")]); } if (expression.Contains(".x")) { answer.Add(item, earth_accel_df(IMU, ATT).x); } if (expression.Contains(".y")) { answer.Add(item, earth_accel_df(IMU, ATT).y); } if (expression.Contains(".z")) { answer.Add(item, earth_accel_df(IMU, ATT).z); } } } // delta(gps_velocity_df(GPS).x,'x',GPS.TimeUS) else if (expression.Contains("delta(gps_velocity_df(GPS)")) { foreach (var item in logdata.GetEnumeratorType("GPS")) { var GPS = new GPS_t(); if (item.msgtype == "GPS") { GPS.Spd = double.Parse(item.items[dflog.FindMessageOffset("GPS", "Spd")]); GPS.GCrs = double.Parse(item.items[dflog.FindMessageOffset("GPS", "GCrs")]); GPS.VZ = double.Parse(item.items[dflog.FindMessageOffset("GPS", "VZ")]); } if (expression.Contains(".x")) { answer.Add(item, delta(gps_velocity_df(GPS).x, "x", item.timems * 1000)); } else if (expression.Contains(".y")) { answer.Add(item, delta(gps_velocity_df(GPS).y, "y", item.timems * 1000)); } else if (expression.Contains(".z")) { answer.Add(item, delta(gps_velocity_df(GPS).z, "z", item.timems * 1000) - 9.8); } } } else if (expression.Contains("delta(gps_velocity_df(GPS2)")) { foreach (var item in logdata.GetEnumeratorType("GPS2")) { var GPS = new GPS_t(); if (item.msgtype == "GPS2") { GPS.Spd = double.Parse(item.items[dflog.FindMessageOffset("GPS2", "Spd")]); GPS.GCrs = double.Parse(item.items[dflog.FindMessageOffset("GPS2", "GCrs")]); GPS.VZ = double.Parse(item.items[dflog.FindMessageOffset("GPS2", "VZ")]); } if (expression.Contains(".x")) { answer.Add(item, delta(gps_velocity_df(GPS).x, "x", item.timems * 1000)); } else if (expression.Contains(".y")) { answer.Add(item, delta(gps_velocity_df(GPS).y, "y", item.timems * 1000)); } else if (expression.Contains(".z")) { answer.Add(item, delta(gps_velocity_df(GPS).z, "z", item.timems * 1000) - 9.8); } } } else if (expression.StartsWith("degrees")) { var matchs = Regex.Matches(expression, @"([A-z0-9_]+)\.([A-z0-9_]+)"); if (matchs.Count > 0) { var type = matchs[0].Groups[1].Value.ToString(); var field = matchs[0].Groups[2].Value.ToString(); foreach (var item in logdata.GetEnumeratorType(type)) { answer.Add(item, degrees(double.Parse(item.items[dflog.FindMessageOffset(type, field)]))); } } } else if (expression.StartsWith("sqrt")) { // there are alot of assumptions made in this code Dictionary <int, double> work = new Dictionary <int, double>(); List <KeyValuePair <string, string> > types = new List <KeyValuePair <string, string> >(); var matchs = Regex.Matches(expression, @"(([A-z0-9_]+)\.([A-z0-9_]+)\*\*2)"); if (matchs.Count > 0) { foreach (Match match in matchs) { var type = match.Groups[2].Value.ToString(); var field = match.Groups[3].Value.ToString(); types.Add(new KeyValuePair <string, string>(type, field)); } List <string> keyarray = new List <string>(); types.ForEach(g => { keyarray.Add(g.Key); }); List <string> valuearray = new List <string>(); types.ForEach(g => { valuearray.Add(g.Value); }); foreach (var item in logdata.GetEnumeratorType(keyarray.ToArray())) { for (int a = 0; a < types.Count; a++) { var key = keyarray[a]; var value = valuearray[a]; var offset = dflog.FindMessageOffset(key, value); if (offset == -1) { continue; } work[a] = double.Parse(item.items[offset]); } double workanswer = 0; foreach (var value in work.Values) { workanswer += Math.Pow(value, 2); } answer.Add(item, Math.Sqrt(workanswer)); } } } else if (expression.Contains("-")) // ATT.DesRoll-ATT.Roll { var matchs = Regex.Matches(expression, @"([A-z0-9_]+)\.([A-z0-9_]+)-([A-z0-9_]+)\.([A-z0-9_]+)"); if (matchs.Count > 0) { var type = matchs[0].Groups[1].Value.ToString(); var field = matchs[0].Groups[2].Value.ToString(); var type2 = matchs[0].Groups[3].Value.ToString(); var field2 = matchs[0].Groups[4].Value.ToString(); foreach (var item in logdata.GetEnumeratorType(new[] { type, type2 })) { if (type == type2) { var idx1 = dflog.FindMessageOffset(type, field); var idx2 = dflog.FindMessageOffset(type2, field2); if (idx1 == -1 || idx2 == -1) { break; } answer.Add(item, double.Parse(item.items[idx1]) - double.Parse(item.items[idx2])); } } } } else if (expression.Contains("mag_heading_df")) { var matchs = Regex.Matches(expression, @"([A-z0-9_]+),([A-z0-9_]+)"); List <string> msglist = new List <string>(); foreach (Match match in matchs) { foreach (var item in match.Groups) { msglist.Add(item.ToString()); } } var MAG = new MAG_t(); var ATT = new ATT_t(); foreach (var item in logdata.GetEnumeratorType(msglist.ToArray())) { if (item.msgtype.StartsWith("MAG")) { MAG.MagX = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "MagX")]); MAG.MagY = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "MagY")]); MAG.MagZ = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "MagZ")]); MAG.OfsX = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "OfsX")]); MAG.OfsY = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "OfsY")]); MAG.OfsZ = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "OfsZ")]); } else if (item.msgtype == "ATT") { ATT.Roll = double.Parse(item.items[dflog.FindMessageOffset("ATT", "Roll")]); ATT.Pitch = double.Parse(item.items[dflog.FindMessageOffset("ATT", "Pitch")]); ATT.Yaw = double.Parse(item.items[dflog.FindMessageOffset("ATT", "Yaw")]); } answer.Add(item, mag_heading_df(MAG, ATT)); } } return(answer); }