string GetLog(ushort no) { MainV2.comPort.Progress += comPort_Progress; status = serialstatus.Reading; // get df log from mav var ms = MainV2.comPort.GetLog(no); status = serialstatus.Done; MainV2.comPort.Progress -= comPort_Progress; // set log fn byte[] hbpacket = MainV2.comPort.getHeartBeat(); MAVLink.mavlink_heartbeat_t hb = (MAVLink.mavlink_heartbeat_t)MainV2.comPort.DebugPacket(hbpacket); logfile = MainV2.LogDir + Path.DirectorySeparatorChar + MainV2.comPort.MAV.aptype.ToString() + Path.DirectorySeparatorChar + hbpacket[3] + Path.DirectorySeparatorChar + DateTime.Now.ToString("yyyy-MM-dd HH-mm") + " " + no + ".bin"; // make log dir Directory.CreateDirectory(Path.GetDirectoryName(logfile)); // save memorystream to file using (BinaryWriter bw = new BinaryWriter(File.OpenWrite(logfile))) { bw.Write(ms.ToArray()); } // read binary log to assci log var temp1 = Log.BinaryLog.ReadLog(logfile); // delete binary log file //File.Delete(logfile); logfile = logfile + ".log"; // write assci log File.WriteAllLines(logfile, temp1); // get gps time of assci log DateTime logtime = DFLog.GetFirstGpsTime(logfile); // rename log is we have a valid gps time if (logtime != DateTime.MinValue) { string newlogfilename = MainV2.LogDir + Path.DirectorySeparatorChar + MainV2.comPort.MAV.aptype.ToString() + Path.DirectorySeparatorChar + hbpacket[3] + Path.DirectorySeparatorChar + logtime.ToString("yyyy-MM-dd HH-mm") + ".log"; File.Move(logfile, newlogfilename); // rename bin as well File.Move(logfile.Replace(".log", ""), newlogfilename.Replace(".log", ".bin")); logfile = newlogfilename; } return(logfile); }
string GetLog(ushort no) { MainV2.comPort.Progress += comPort_Progress; status = serialstatus.Reading; // get df log from mav var ms = MainV2.comPort.GetLog(no); status = serialstatus.Done; updateDisplay(); MainV2.comPort.Progress -= comPort_Progress; // set log fn byte[] hbpacket = MainV2.comPort.getHeartBeat(); MAVLink.mavlink_heartbeat_t hb = (MAVLink.mavlink_heartbeat_t)MainV2.comPort.DebugPacket(hbpacket); logfile = MainV2.LogDir + Path.DirectorySeparatorChar + MainV2.comPort.MAV.aptype.ToString() + Path.DirectorySeparatorChar + hbpacket[3] + Path.DirectorySeparatorChar + DateTime.Now.ToString("yyyy-MM-dd HH-mm-ss") + " " + no + ".bin"; // make log dir Directory.CreateDirectory(Path.GetDirectoryName(logfile)); // save memorystream to file using (BinaryWriter bw = new BinaryWriter(File.OpenWrite(logfile))) { bw.Write(ms.ToArray()); } // create ascii log BinaryLog.ConvertBin(logfile, logfile + ".log"); //update the new filename logfile = logfile + ".log"; // get gps time of assci log DateTime logtime = DFLog.GetFirstGpsTime(logfile); // rename log is we have a valid gps time if (logtime != DateTime.MinValue) { string newlogfilename = MainV2.LogDir + Path.DirectorySeparatorChar + MainV2.comPort.MAV.aptype.ToString() + Path.DirectorySeparatorChar + hbpacket[3] + Path.DirectorySeparatorChar + logtime.ToString("yyyy-MM-dd HH-mm-ss") + ".log"; try { File.Move(logfile, newlogfilename); // rename bin as well File.Move(logfile.Replace(".log", ""), newlogfilename.Replace(".log", ".bin")); logfile = newlogfilename; } catch { CustomMessageBox.Show("Failed to rename file " + logfile + "\nto " + newlogfilename, "Error"); } } return(logfile); }
void DrawTime() { int a = 0; DateTime starttime = DateTime.MinValue; int startdelta = 0; DateTime workingtime = starttime; DateTime lastdrawn = DateTime.MinValue; //zg1.GraphPane.GraphObjList.Clear(); foreach (var item in logdata) { if (item.msgtype == "GPS") { if (!DFLog.logformat.ContainsKey("GPS")) { break; } int index = DFLog.FindMessageOffset("GPS", "TimeMS"); if (index == -1) { a++; continue; } string time = item.items[index].ToString(); int temp; if (int.TryParse(time, out temp)) { if (startdelta == 0) { startdelta = temp; } workingtime = starttime.AddMilliseconds(temp - startdelta); TimeSpan span = workingtime - starttime; if (workingtime.Minute != lastdrawn.Minute) { zg1.GraphPane.GraphObjList.Add(new TextObj(span.TotalMinutes.ToString("0") + " min", a, zg1.GraphPane.YAxis.Scale.Max, CoordType.AxisXYScale, AlignH.Left, AlignV.Top)); lastdrawn = workingtime; } } } a++; } }
PointLatLng?getPointLatLng(DFLog.DFItem item) { if (item.msgtype == "GPS") { if (!DFLog.logformat.ContainsKey("GPS")) { return(null); } int index = DFLog.FindMessageOffset("GPS", "Lat"); if (index == -1) { return(null); } int index2 = DFLog.FindMessageOffset("GPS", "Lng"); if (index2 == -1) { return(null); } int index3 = DFLog.FindMessageOffset("GPS", "Status"); if (index3 == -1) { return(null); } try { if (double.Parse(item.items[index3].ToString(), System.Globalization.CultureInfo.InvariantCulture) != 3) { return(null); } string lat = item.items[index].ToString(); string lng = item.items[index2].ToString(); PointLatLng pnt = new PointLatLng() { }; pnt.Lat = double.Parse(lat, System.Globalization.CultureInfo.InvariantCulture); pnt.Lng = double.Parse(lng, System.Globalization.CultureInfo.InvariantCulture); return(pnt); } catch { } } return(null); }
public void LoadLog(string FileName) { while (!this.IsHandleCreated) { Thread.Sleep(100); } Loading.ShowLoading(Strings.Scanning_File, this); try { log.Info("before read " + (GC.GetTotalMemory(false) / 1024.0 / 1024.0)); logdata = new DFLogBuffer(FileName); dflog = logdata.dflog; log.Info("got log lines " + (GC.GetTotalMemory(false) / 1024.0 / 1024.0)); log.Info("process to datagrid " + (GC.GetTotalMemory(false) / 1024.0 / 1024.0)); Loading.ShowLoading("Scanning coloum widths", this); colcount = 0; foreach (var msgid in logdata.FMT) { if (msgid.Value.Item4 == null) { continue; } var colsplit = msgid.Value.Item4.FirstOrDefault().ToString().Split(',').Length; colcount = Math.Max(colcount, (msgid.Value.Item4.Length + typecoloum + colsplit)); } log.Info("Done " + (GC.GetTotalMemory(false) / 1024.0 / 1024.0)); //this.BeginInvokeIfRequired(() => { LoadLog2(FileName, logdata, colcount); }); } catch (Exception ex) { MsgBox.CustomMessageBox.Show("Failed to read File: " + ex.ToString()); return; } log.Info("LoadLog Done"); }
void DrawErrors() { bool top = false; int a = 0; foreach (var item in logdata) { if (item.msgtype == "ERR") { if (!DFLog.logformat.ContainsKey("ERR")) { return; } int index = DFLog.FindMessageOffset("ERR", "Subsys"); if (index == -1) { continue; } int index2 = DFLog.FindMessageOffset("ERR", "ECode"); if (index2 == -1) { continue; } string mode = "Err: " + ((DFLog.error_subsystem) int.Parse(item.items[index].ToString())) + "-" + item.items[index2].ToString().Trim(); if (top) { var temp = new TextObj(mode, a, zg1.GraphPane.YAxis.Scale.Max, CoordType.AxisXYScale, AlignH.Left, AlignV.Top); temp.FontSpec.Fill.Color = Color.Red; zg1.GraphPane.GraphObjList.Add(temp); } else { var temp = new TextObj(mode, a, zg1.GraphPane.YAxis.Scale.Max, CoordType.AxisXYScale, AlignH.Left, AlignV.Bottom); temp.FontSpec.Fill.Color = Color.Red; zg1.GraphPane.GraphObjList.Add(temp); } top = !top; } a++; } }
void DrawModes() { bool top = false; int a = 0; zg1.GraphPane.GraphObjList.Clear(); foreach (var item in logdata) { if (item.msgtype == "MODE") { if (!DFLog.logformat.ContainsKey("MODE")) { return; } int index = DFLog.FindMessageOffset("MODE", "Mode"); if (index == -1) { continue; } string mode = item.items[index].ToString().Trim(); if (top) { zg1.GraphPane.GraphObjList.Add(new TextObj(mode, a, zg1.GraphPane.YAxis.Scale.Min, CoordType.AxisXYScale, AlignH.Left, AlignV.Top)); } else { zg1.GraphPane.GraphObjList.Add(new TextObj(mode, a, zg1.GraphPane.YAxis.Scale.Min, CoordType.AxisXYScale, AlignH.Left, AlignV.Bottom)); } top = !top; } a++; } }
void GraphItem_GetList(string fieldname, string type, DFLog dflog, DataModifer dataModifier, bool left) { int col = dflog.FindMessageOffset(type, fieldname); // field does not exist if (col == -1) return; PointPairList list1 = new PointPairList(); int error = 0; double a = 0; // row counter double b = 0; DateTime screenupdate = DateTime.MinValue; double value_prev = 0; foreach (var item in logdata.GetEnumeratorType(type)) { b = item.lineno; if (screenupdate.Second != DateTime.Now.Second) { Console.Write(b + " of " + logdata.Count + " \r"); screenupdate = DateTime.Now; } if (item.msgtype == type) { try { double value = double.Parse(item.items[col], System.Globalization.CultureInfo.InvariantCulture); // abandon realy bad data if (Math.Abs(value) > 9.15e8) { a++; continue; } if (dataModifier.IsValid()) { if ((a != 0) && Math.Abs(value - value_prev) > 1e5) { // there is a glitch in the data, reject it by replacing it with the previous value value = value_prev; } value_prev = value; if (dataModifier.doOffsetFirst) { value += dataModifier.offset; value *= dataModifier.scalar; } else { value *= dataModifier.scalar; value += dataModifier.offset; } } if (chk_time.Checked) { var e = new DataGridViewCellValueEventArgs(1, (int) b); dataGridView1_CellValueNeeded(dataGridView1, e); XDate time = new XDate(DateTime.Parse(e.Value.ToString())); list1.Add(time, value); } else { list1.Add(b, value); } } catch { error++; log.Info("Bad Data : " + type + " " + col + " " + a); if (error >= 500) { CustomMessageBox.Show("There is to much bad data - failing"); break; } } } a++; } Invoke((Action) delegate { GraphItem_AddCurve(list1, type, fieldname, left); }); }
public static void MapLogs(string[] logs) { foreach (var logfile in logs) { if (File.Exists(logfile + ".jpg")) { continue; } double minx = 99999; double maxx = -99999; double miny = 99999; double maxy = -99999; List <PointLatLngAlt> locs = new List <PointLatLngAlt>(); try { if (logfile.ToLower().EndsWith(".tlog")) { MAVLinkInterface mine = new MAVLinkInterface(); using (mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read))) { mine.logreadmode = true; CurrentState cs = new CurrentState(); while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length) { byte[] packet = mine.readPacket(); if (packet.Length < 5) { continue; } try { if (MainV2.speechEngine != null) { MainV2.speechEngine.SpeakAsyncCancelAll(); } } catch { } if (packet[5] == (byte)MAVLink.MAVLINK_MSG_ID.GLOBAL_POSITION_INT) { var loc = packet.ByteArrayToStructure <MAVLink.mavlink_global_position_int_t>(6); if (loc.lat == 0 || loc.lon == 0) { continue; } locs.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")) { bool bin = logfile.ToLower().EndsWith(".bin"); using (var st = File.OpenRead(logfile)) { using (StreamReader sr = new StreamReader(st)) { while (sr.BaseStream.Position < sr.BaseStream.Length) { string line = ""; if (bin) { line = BinaryLog.ReadMessage(sr.BaseStream); } else { line = sr.ReadLine(); } if (line.StartsWith("FMT")) { DFLog.FMTLine(line); } else if (line.StartsWith("GPS")) { var item = DFLog.GetDFItemFromLine(line, 0); var lat = double.Parse(item.items[DFLog.FindMessageOffset(item.msgtype, "Lat")]); var lon = double.Parse(item.items[DFLog.FindMessageOffset(item.msgtype, "Lng")]); if (lat == 0 || lon == 0) { continue; } locs.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 (locs.Count > 10) { // add a bit of buffer var area = RectLatLng.FromLTRB(minx - 0.001, maxy + 0.001, maxx + 0.001, miny - 0.001); var map = GetMap(area); var grap = Graphics.FromImage(map); PointF lastpoint = new PointF(); foreach (var loc in locs) { PointF newpoint = GetPixel(area, loc, map.Size); if (!lastpoint.IsEmpty) { grap.DrawLine(Pens.Red, lastpoint, newpoint); } lastpoint = newpoint; } map.Save(logfile + ".jpg", System.Drawing.Imaging.ImageFormat.Jpeg); map.Dispose(); map = null; } else { DoTextMap(logfile + ".jpg", "No gps data"); } } catch (Exception ex) { if (ex.ToString().Contains("Mavlink 0.9")) { DoTextMap(logfile + ".jpg", "Old log\nMavlink 0.9"); } continue; } } }
void comPort_DataReceived(object sender, SerialDataReceivedEventArgs e) { try { while (comPort.BytesToRead > 0 && threadrun) { updateDisplay(); string line = ""; comPort.ReadTimeout = 500; try { line = comPort.ReadLine(); //readline(comPort); if (!line.Contains("\n")) { line = line + "\n"; } } catch { line = comPort.ReadExisting(); //byte[] data = readline(comPort); //line = Encoding.ASCII.GetString(data, 0, data.Length); } receivedbytes += line.Length; //string line = Encoding.ASCII.GetString(data, 0, data.Length); switch (status) { case serialstatus.Connecting: if (line.Contains("ENTER") || line.Contains("GROUND START") || line.Contains("reset to FLY") || line.Contains("interactive setup") || line.Contains("CLI") || line.Contains("Ardu")) { try { //System.Threading.Thread.Sleep(200); //comPort.Write("\n\n\n\n"); } catch { } System.Threading.Thread.Sleep(500); // clear history this.Invoke((System.Windows.Forms.MethodInvoker) delegate() { TXT_seriallog.Clear(); }); // comPort.Write("logs\r"); status = serialstatus.Done; } break; case serialstatus.Closefile: sw.Close(); DateTime logtime = DFLog.GetFirstGpsTime(logfile); if (logtime != DateTime.MinValue) { string newlogfilename = MainV2.LogDir + Path.DirectorySeparatorChar + logtime.ToString("yyyy-MM-dd HH-mm-ss") + ".log"; try { File.Move(logfile, newlogfilename); logfile = newlogfilename; } catch (Exception ex) { log.Error(ex); CustomMessageBox.Show("Failed to rename file " + logfile + "\nto " + newlogfilename, Strings.ERROR); } } TextReader tr = new StreamReader(logfile); // this.Invoke((System.Windows.Forms.MethodInvoker) delegate() { TXT_seriallog.AppendText("Creating KML for " + logfile); }); LogOutput lo = new LogOutput(); while (tr.Peek() != -1) { lo.processLine(tr.ReadLine()); } tr.Close(); try { lo.writeKML(logfile + ".kml"); } catch { } // usualy invalid lat long error status = serialstatus.Done; comPort.DiscardInBuffer(); break; case serialstatus.Createfile: receivedbytes = 0; Directory.CreateDirectory(MainV2.LogDir); logfile = MainV2.LogDir + Path.DirectorySeparatorChar + DateTime.Now.ToString("yyyy-MM-dd HH-mm-ss") + " " + currentlog + ".log"; sw = new StreamWriter(logfile); status = serialstatus.Waiting; lock (thisLock) { this.Invoke((System.Windows.Forms.MethodInvoker) delegate() { TXT_seriallog.Clear(); }); } //if (line.Contains("Dumping Log")) { status = serialstatus.Reading; } break; case serialstatus.Done: // // if (line.Contains("start") && line.Contains("end")) { Regex regex2 = new Regex(@"^Log ([0-9]+)[,\s]", RegexOptions.IgnoreCase); if (regex2.IsMatch(line)) { MatchCollection matchs = regex2.Matches(line); logcount = int.Parse(matchs[0].Groups[1].Value); genchkcombo(logcount); //status = serialstatus.Done; } } if (line.Contains("No logs")) { status = serialstatus.Done; } break; case serialstatus.Reading: if (line.Contains("packets read") || line.Contains("Done") || line.Contains("logs enabled")) { status = serialstatus.Closefile; Console.Write("CloseFile: " + line); break; } sw.Write(line); continue; case serialstatus.Waiting: if (line.Contains("Dumping Log") || line.Contains("GPS:") || line.Contains("NTUN:") || line.Contains("CTUN:") || line.Contains("PM:")) { status = serialstatus.Reading; Console.Write("Reading: " + line); } break; } lock (thisLock) { this.BeginInvoke((MethodInvoker) delegate() { Console.Write(line); TXT_seriallog.AppendText(line.Replace((char)0x0, ' ')); // auto scroll if (TXT_seriallog.TextLength >= 10000) { TXT_seriallog.Text = TXT_seriallog.Text.Substring(TXT_seriallog.TextLength / 2); } TXT_seriallog.SelectionStart = TXT_seriallog.Text.Length; TXT_seriallog.ScrollToCaret(); TXT_seriallog.Refresh(); }); } } //log.Info("exit while"); } catch (Exception ex) { CustomMessageBox.Show("Error reading data" + ex.ToString()); } }
public static double[] getOffsetsLog(string fn) { // this is for a dxf Polyline3dVertex vertex; List<Polyline3dVertex> vertexes = new List<Polyline3dVertex>(); List<Tuple<float, float, float>> data = new List<Tuple<float, float, float>>(); List<Tuple<float, float, float>> data2 = new List<Tuple<float, float, float>>(); Log.DFLog dflog = new Log.DFLog(); var logfile = dflog.ReadLog(fn); foreach (var line in logfile) { if (line.msgtype == "MAG" || line.msgtype == "MAG2") { int indexmagx = dflog.FindMessageOffset(line.msgtype, "MagX"); int indexmagy = dflog.FindMessageOffset(line.msgtype, "MagY"); int indexmagz = dflog.FindMessageOffset(line.msgtype, "MagZ"); int indexoffsetx = dflog.FindMessageOffset(line.msgtype, "OfsX"); int indexoffsety = dflog.FindMessageOffset(line.msgtype, "OfsY"); int indexoffsetz = dflog.FindMessageOffset(line.msgtype, "OfsZ"); if (indexmagx != -1 && indexoffsetx != -1) { float magx = float.Parse(line.items[indexmagx]); float magy = float.Parse(line.items[indexmagy]); float magz = float.Parse(line.items[indexmagz]); float offsetx = float.Parse(line.items[indexoffsetx]); float offsety = float.Parse(line.items[indexoffsety]); float offsetz = float.Parse(line.items[indexoffsetz]); //offsetx = offsety = offsetz = 0; if (line.msgtype == "MAG") { data.Add(new Tuple<float, float, float>( magx - offsetx, magy - offsety, magz - offsetz)); // fox dxf vertex = new Polyline3dVertex(new Vector3f(magx - offsetx, magy - offsety, magz - offsetz) ); vertexes.Add(vertex); } else if (line.msgtype == "MAG2") { data2.Add(new Tuple<float, float, float>( magx - offsetx, magy - offsety, magz - offsetz)); } } } } double[] x = LeastSq(data); if (data2.Count > 0) { double[] x2 = LeastSq(data2); } log.Info("Least Sq Done " + DateTime.Now); doDXF(vertexes, x); Array.Resize<double>(ref x, 3); return x; }
public static void MapLogs(string[] logs) { foreach (var logfile in logs) { if (File.Exists(logfile + ".jpg")) { continue; } double minx = 99999; double maxx = -99999; double miny = 99999; double maxy = -99999; Dictionary <int, List <PointLatLngAlt> > loc_list = new Dictionary <int, List <PointLatLngAlt> >(); try { if (logfile.ToLower().EndsWith(".tlog")) { using (MAVLinkInterface mine = new MAVLinkInterface()) using ( mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.ReadWrite)) ) { mine.logreadmode = true; while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length) { MAVLink.MAVLinkMessage packet = mine.readPacket(); if (packet.Length < 5) { continue; } try { if (MainV2.speechEngine != null) { MainV2.speechEngine.SpeakAsyncCancelAll(); } } catch { } 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 = MAVList.GetID(packet.sysid, 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")) { bool bin = logfile.ToLower().EndsWith(".bin"); BinaryLog binlog = new BinaryLog(); DFLog dflog = new DFLog(); using (var st = File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.ReadWrite)) { using (StreamReader sr = new StreamReader(st)) { loc_list[0] = new List <PointLatLngAlt>(); while (sr.BaseStream.Position < sr.BaseStream.Length) { string line = ""; if (bin) { line = binlog.ReadMessage(sr.BaseStream); } else { line = sr.ReadLine(); } if (line.StartsWith("FMT")) { dflog.FMTLine(line); } else if (line.StartsWith("GPS")) { var item = dflog.GetDFItemFromLine(line, 0); var lat = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "Lat")]); var lon = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "Lng")]); if (lat == 0 || lon == 0) { 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.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); var map = GetMap(area); var grap = Graphics.FromImage(map); 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); map.Dispose(); map = null; } else { DoTextMap(logfile + ".jpg", "No gps data"); } } catch (Exception ex) { if (ex.ToString().Contains("Mavlink 0.9")) { DoTextMap(logfile + ".jpg", "Old log\nMavlink 0.9"); } continue; } } }
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); }
void comPort_DataReceived(object sender, SerialDataReceivedEventArgs e) { try { while (comPort.BytesToRead > 0 && threadrun) { updateDisplay(); string line = ""; comPort.ReadTimeout = 500; try { line = comPort.ReadLine(); //readline(comPort); if (!line.Contains("\n")) line = line + "\n"; } catch { line = comPort.ReadExisting(); //byte[] data = readline(comPort); //line = Encoding.ASCII.GetString(data, 0, data.Length); } receivedbytes += line.Length; //string line = Encoding.ASCII.GetString(data, 0, data.Length); switch (status) { case serialstatus.Connecting: if (line.Contains("ENTER") || line.Contains("GROUND START") || line.Contains("reset to FLY") || line.Contains("interactive setup") || line.Contains("CLI") || line.Contains("Ardu")) { try { //System.Threading.Thread.Sleep(200); //comPort.Write("\n\n\n\n"); } catch { } System.Threading.Thread.Sleep(500); // clear history this.Invoke((System.Windows.Forms.MethodInvoker) delegate() { TXT_seriallog.Clear(); }); // comPort.Write("logs\r"); status = serialstatus.Done; } break; case serialstatus.Closefile: sw.Close(); DateTime logtime = new DFLog().GetFirstGpsTime(logfile); if (logtime != DateTime.MinValue) { string newlogfilename = MainV2.LogDir + Path.DirectorySeparatorChar + logtime.ToString("yyyy-MM-dd HH-mm-ss") + ".log"; try { File.Move(logfile, newlogfilename); logfile = newlogfilename; } catch (Exception ex) { log.Error(ex); CustomMessageBox.Show( "Failed to rename file " + logfile + "\nto " + newlogfilename, Strings.ERROR); } } TextReader tr = new StreamReader(logfile); // this.Invoke( (System.Windows.Forms.MethodInvoker) delegate() { TXT_seriallog.AppendText("Creating KML for " + logfile); }); LogOutput lo = new LogOutput(); while (tr.Peek() != -1) { lo.processLine(tr.ReadLine()); } tr.Close(); try { lo.writeKML(logfile + ".kml"); } catch { } // usualy invalid lat long error status = serialstatus.Done; comPort.DiscardInBuffer(); break; case serialstatus.Createfile: receivedbytes = 0; Directory.CreateDirectory(MainV2.LogDir); logfile = MainV2.LogDir + Path.DirectorySeparatorChar + DateTime.Now.ToString("yyyy-MM-dd HH-mm-ss") + " " + currentlog + ".log"; sw = new StreamWriter(logfile); status = serialstatus.Waiting; lock (thisLock) { this.Invoke((System.Windows.Forms.MethodInvoker) delegate() { TXT_seriallog.Clear(); }); } //if (line.Contains("Dumping Log")) { status = serialstatus.Reading; } break; case serialstatus.Done: // // if (line.Contains("start") && line.Contains("end")) { Regex regex2 = new Regex(@"^Log ([0-9]+)[,\s]", RegexOptions.IgnoreCase); if (regex2.IsMatch(line)) { MatchCollection matchs = regex2.Matches(line); logcount = int.Parse(matchs[0].Groups[1].Value); genchkcombo(logcount); //status = serialstatus.Done; } } if (line.Contains("No logs")) { status = serialstatus.Done; } break; case serialstatus.Reading: if (line.Contains("packets read") || line.Contains("Done") || line.Contains("logs enabled")) { status = serialstatus.Closefile; Console.Write("CloseFile: " + line); break; } sw.Write(line); continue; case serialstatus.Waiting: if (line.Contains("Dumping Log") || line.Contains("GPS:") || line.Contains("NTUN:") || line.Contains("CTUN:") || line.Contains("PM:")) { status = serialstatus.Reading; Console.Write("Reading: " + line); } break; } lock (thisLock) { this.BeginInvoke((MethodInvoker) delegate() { Console.Write(line); TXT_seriallog.AppendText(line.Replace((char) 0x0, ' ')); // auto scroll if (TXT_seriallog.TextLength >= 10000) { TXT_seriallog.Text = TXT_seriallog.Text.Substring(TXT_seriallog.TextLength/2); } TXT_seriallog.SelectionStart = TXT_seriallog.Text.Length; TXT_seriallog.ScrollToCaret(); TXT_seriallog.Refresh(); }); } } //log.Info("exit while"); } catch (Exception ex) { CustomMessageBox.Show("Error reading data" + ex.ToString()); } }
void GraphItem(string type, string fieldname, bool left = true) { double a = 0; // row counter int error = 0; // ensure we tick the treeview foreach (TreeNode node in treeView1.Nodes) { if (node.Text == type) { foreach (TreeNode subnode in node.Nodes) { if (subnode.Text == fieldname && subnode.Checked != true) { subnode.Checked = true; break; } } } } if (!DFLog.logformat.ContainsKey(type)) { CustomMessageBox.Show("No FMT message for " + type + " - " + fieldname, "Error"); return; } int col = DFLog.FindMessageOffset(type, fieldname); // field does not exist if (col == -1) { return; } PointPairList list1 = new PointPairList(); string header = fieldname; foreach (var item in logdata) { if (item.msgtype == type) { try { double value = double.Parse(item.items[col], System.Globalization.CultureInfo.InvariantCulture); // XDate time = new XDate(DateTime.Parse(datarow.Cells[1].Value.ToString())); list1.Add(a, value); } catch { error++; log.Info("Bad Data : " + type + " " + col + " " + a); if (error >= 500) { CustomMessageBox.Show("There is to much bad data - failing"); break; } } } a++; } if (list1.Count < 1) { return; } LineItem myCurve; myCurve = zg1.GraphPane.AddCurve(header, list1, colours[zg1.GraphPane.CurveList.Count % colours.Length], SymbolType.None); leftorrightaxis(left, myCurve); // Make sure the Y axis is rescaled to accommodate actual data try { zg1.AxisChange(); } catch { } // Zoom all zg1.ZoomOutAll(zg1.GraphPane); try { DrawModes(); DrawErrors(); DrawTime(); } catch { } // Force a redraw zg1.Invalidate(); }
string GetLog(ushort no, string fileName) { log.Info("GetLog " + no); MainV2.comPort.Progress += comPort_Progress; status = SerialStatus.Reading; // used for log fn MAVLink.MAVLinkMessage hbpacket = MainV2.comPort.getHeartBeat(); if (hbpacket != null) { log.Info("Got hbpacket length: " + hbpacket.Length); } // get df log from mav using (var ms = MainV2.comPort.GetLog(no)) { if (ms != null) { log.Info("Got Log length: " + ms.Length); } ms.Seek(0, SeekOrigin.Begin); status = SerialStatus.Done; MAVLink.mavlink_heartbeat_t hb = (MAVLink.mavlink_heartbeat_t)MainV2.comPort.DebugPacket(hbpacket); logfile = Settings.Instance.LogDir + Path.DirectorySeparatorChar + MainV2.comPort.MAV.aptype.ToString() + Path.DirectorySeparatorChar + hbpacket.sysid + Path.DirectorySeparatorChar + no + " " + MakeValidFileName(fileName) + ".bin"; // make log dir Directory.CreateDirectory(Path.GetDirectoryName(logfile)); log.Info("about to write: " + logfile); // save memorystream to file using (BinaryWriter bw = new BinaryWriter(File.OpenWrite(logfile))) { byte[] buffer = new byte[256 * 1024]; while (ms.Position < ms.Length) { int read = ms.Read(buffer, 0, buffer.Length); bw.Write(buffer, 0, read); } } } log.Info("about to convertbin: " + logfile); // create ascii log BinaryLog.ConvertBin(logfile, logfile + ".log"); //update the new filename logfile = logfile + ".log"; // rename file if needed log.Info("about to GetFirstGpsTime: " + logfile); // get gps time of assci log DateTime logtime = new DFLog().GetFirstGpsTime(logfile); // rename log is we have a valid gps time if (logtime != DateTime.MinValue) { string newlogfilename = Settings.Instance.LogDir + Path.DirectorySeparatorChar + MainV2.comPort.MAV.aptype.ToString() + Path.DirectorySeparatorChar + hbpacket.sysid + Path.DirectorySeparatorChar + logtime.ToString("yyyy-MM-dd HH-mm-ss") + ".log"; try { File.Move(logfile, newlogfilename); // rename bin as well File.Move(logfile.Replace(".log", ""), newlogfilename.Replace(".log", ".bin")); logfile = newlogfilename; } catch { CustomMessageBox.Show(Strings.ErrorRenameFile + " " + logfile + "\nto " + newlogfilename, Strings.ERROR); } } MainV2.comPort.Progress -= comPort_Progress; return(logfile); }
public static void MapLogs(string[] logs) { foreach (var logfile in logs) { if (File.Exists(logfile + ".jpg")) continue; double minx = 99999; double maxx = -99999; double miny = 99999; double maxy = -99999; List<PointLatLngAlt> locs = new List<PointLatLngAlt>(); try { if (logfile.ToLower().EndsWith(".tlog")) { using (MAVLinkInterface mine = new MAVLinkInterface()) using (mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.ReadWrite))) { mine.logreadmode = true; CurrentState cs = new CurrentState(); while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length) { byte[] packet = mine.readPacket(); if (packet.Length < 5) continue; try { if (MainV2.speechEngine != null) MainV2.speechEngine.SpeakAsyncCancelAll(); } catch { } if (packet[5] == (byte)MAVLink.MAVLINK_MSG_ID.GLOBAL_POSITION_INT) { var loc = packet.ByteArrayToStructure<MAVLink.mavlink_global_position_int_t>(6); if (loc.lat == 0 || loc.lon == 0) continue; locs.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")) { bool bin = logfile.ToLower().EndsWith(".bin"); BinaryLog binlog = new BinaryLog(); DFLog dflog = new DFLog(); using (var st = File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.ReadWrite)) { using (StreamReader sr = new StreamReader(st)) { while (sr.BaseStream.Position < sr.BaseStream.Length) { string line = ""; if (bin) { line = binlog.ReadMessage(sr.BaseStream); } else { line = sr.ReadLine(); } if (line.StartsWith("FMT")) { dflog.FMTLine(line); } else if (line.StartsWith("GPS")) { var item = dflog.GetDFItemFromLine(line, 0); var lat = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "Lat")]); var lon = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "Lng")]); if (lat == 0 || lon == 0) continue; locs.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 (locs.Count > 10) { // add a bit of buffer var area = RectLatLng.FromLTRB(minx - 0.001, maxy + 0.001, maxx + 0.001, miny - 0.001); var map = GetMap(area); var grap = Graphics.FromImage(map); PointF lastpoint = new PointF(); foreach (var loc in locs) { PointF newpoint = GetPixel(area, loc, map.Size); if (!lastpoint.IsEmpty) grap.DrawLine(Pens.Red, lastpoint, newpoint); lastpoint = newpoint; } map.Save(logfile + ".jpg", System.Drawing.Imaging.ImageFormat.Jpeg); map.Dispose(); map = null; } else { DoTextMap(logfile + ".jpg", "No gps data"); } } catch (Exception ex) { if (ex.ToString().Contains("Mavlink 0.9")) DoTextMap(logfile + ".jpg", "Old log\nMavlink 0.9"); continue; } } }
private void Form1_Load(object sender, EventArgs e) { Hashtable seenmessagetypes = new Hashtable(); OpenFileDialog openFileDialog1 = new OpenFileDialog(); openFileDialog1.Filter = "Log Files|*.log;*.bin"; openFileDialog1.FilterIndex = 2; openFileDialog1.RestoreDirectory = true; openFileDialog1.InitialDirectory = MainV2.LogDir; if (openFileDialog1.ShowDialog() == DialogResult.OK) { try { Stream stream; if (openFileDialog1.FileName.ToLower().EndsWith(".bin")) { log.Info("before " + (GC.GetTotalMemory(false) / 1024.0 / 1024.0)); // extract log - converts to assci lines var loglines = BinaryLog.ReadLog(openFileDialog1.FileName); log.Info("loglines " + (GC.GetTotalMemory(false) / 1024.0 / 1024.0)); GC.Collect(); log.Info("loglines2 " + (GC.GetTotalMemory(false) / 1024.0 / 1024.0)); // create temp file string tempfile = Path.GetTempFileName(); log.Info("temp file " + tempfile); stream = File.Open(tempfile, FileMode.Create, FileAccess.ReadWrite, FileShare.Delete); // save ascii lines to file foreach (string line in loglines) { stream.Write(ASCIIEncoding.ASCII.GetBytes(line), 0, line.Length); } stream.Flush(); // back to stream start stream.Seek(0, SeekOrigin.Begin); loglines.Clear(); loglines = null; // force memory reclaim after loglines clear GC.Collect(); log.Info("loglines.clear " + (GC.GetTotalMemory(false) / 1024.0 / 1024.0)); } else { stream = File.Open(openFileDialog1.FileName, FileMode.Open, FileAccess.Read, FileShare.Read); } logdata = DFLog.ReadLog(stream); this.Text = "Log Browser - " + Path.GetFileName(openFileDialog1.FileName); log.Info("about to create DataTable " + (GC.GetTotalMemory(false) / 1024.0 / 1024.0)); m_dtCSV = new DataTable(); log.Info("process to datagrid " + (GC.GetTotalMemory(false) / 1024.0 / 1024.0)); bool largelog = logdata.Count > 500000 ? true : false; foreach (var item in logdata) { if (item.items != null) { while (m_dtCSV.Columns.Count < (item.items.Length + typecoloum)) { m_dtCSV.Columns.Add(); } seenmessagetypes[item.msgtype] = ""; if (largelog) { continue; } DataRow dr = m_dtCSV.NewRow(); dr[0] = item.lineno; dr[1] = item.time.ToString("yyyy-MM-dd HH:mm:ss.fff"); for (int a = 0; a < item.items.Length; a++) { dr[a + typecoloum] = item.items[a]; } m_dtCSV.Rows.Add(dr); } } log.Info("Done " + (GC.GetTotalMemory(false) / 1024.0 / 1024.0)); //PopulateDataTableFromUploadedFile(stream); stream.Close(); log.Info("set dgv datasourse " + (GC.GetTotalMemory(false) / 1024.0 / 1024.0)); if (MainV2.MONO) { //if (m_dtCSV.Rows.Count > 5000) // { // CustomMessageBox.Show("This log apears to be a large log, the grid view will be disabled.\nAll graphing will still work however", "Large Log"); // dataGridView1.Visible = false; // } // else { BindingSource bs = new BindingSource(); bs.DataSource = m_dtCSV; dataGridView1.DataSource = bs; } } else { dataGridView1.VirtualMode = true; dataGridView1.RowCount = logdata.Count; dataGridView1.ColumnCount = m_dtCSV.Columns.Count; } dataGridView1.Columns[0].Visible = false; log.Info("datasource set " + (GC.GetTotalMemory(false) / 1024.0 / 1024.0)); } catch (Exception ex) { CustomMessageBox.Show("Failed to read File: " + ex.ToString()); } foreach (DataGridViewColumn column in dataGridView1.Columns) { column.SortMode = DataGridViewColumnSortMode.NotSortable; } CreateChart(zg1); ResetTreeView(seenmessagetypes); if (DFLog.logformat.Count == 0) { CustomMessageBox.Show("Log Browse will not function correctly without FMT messages in your log.\nThese appear to be missing from your log.", "Error"); this.Close(); return; } } else { this.Close(); return; } }
PointLatLng? getPointLatLng(DFLog.DFItem item) { if (item.msgtype == "GPS") { if (!dflog.logformat.ContainsKey("GPS")) return null; int index = dflog.FindMessageOffset("GPS", "Lat"); if (index == -1) { return null; } int index2 = dflog.FindMessageOffset("GPS", "Lng"); if (index2 == -1) { return null; } int index3 = dflog.FindMessageOffset("GPS", "Status"); if (index3 == -1) { return null; } try { if (double.Parse(item.items[index3].ToString(), System.Globalization.CultureInfo.InvariantCulture) < 3) { return null; } string lat = item.items[index].ToString(); string lng = item.items[index2].ToString(); PointLatLng pnt = new PointLatLng() {}; pnt.Lat = double.Parse(lat, System.Globalization.CultureInfo.InvariantCulture); pnt.Lng = double.Parse(lng, System.Globalization.CultureInfo.InvariantCulture); return pnt; } catch { } } if (item.msgtype == "POS") { if (!dflog.logformat.ContainsKey("POS")) return null; int index = dflog.FindMessageOffset("POS", "Lat"); if (index == -1) { return null; } int index2 = dflog.FindMessageOffset("POS", "Lng"); if (index2 == -1) { return null; } try { string lat = item.items[index].ToString(); string lng = item.items[index2].ToString(); PointLatLng pnt = new PointLatLng() {}; pnt.Lat = double.Parse(lat, System.Globalization.CultureInfo.InvariantCulture); pnt.Lng = double.Parse(lng, System.Globalization.CultureInfo.InvariantCulture); if (Math.Abs(pnt.Lat) > 90 || Math.Abs(pnt.Lng) > 180) return null; return pnt; } catch { } } return null; }
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 } }
public void writeRinex(string filename) { if (gpsrawdata.Count == 0) { return; } string file = Path.GetDirectoryName(filename) + Path.DirectorySeparatorChar + Path.GetFileNameWithoutExtension(filename) + ".obs"; var rinexoutput = new StreamWriter(file); // 60 chars string header = @" 3.02 OBSERVATION DATA M: Mixed RINEX VERSION / TYPE MARKER NAME MARKER NUMBER MARKER TYPE OBSERVER / AGENCY REC # / TYPE / VERS ANT # / TYPE 0.0000 0.0000 0.0000 APPROX POSITION XYZ 0.0000 0.0000 0.0000 ANTENNA: DELTA H/E/N G 4 C1C L1C D1C S1C SYS / # / OBS TYPES S 4 C1C L1C D1C S1C SYS / # / OBS TYPES G SYS / PHASE SHIFT END OF HEADER "; rinexoutput.WriteLine(header); DateTime lastgpstime = DateTime.MinValue; foreach (string line in gpsrawdata) { string[] items = line.Split(',', ':'); double weekms = double.Parse(items[DFLog.FindMessageOffset("GRAW", "WkMS")]); int week = int.Parse(items[DFLog.FindMessageOffset("GRAW", "Week")]); double NSats = double.Parse(items[DFLog.FindMessageOffset("GRAW", "numSV")]); double sv = double.Parse(items[DFLog.FindMessageOffset("GRAW", "sv")]); double cpMes = double.Parse(items[DFLog.FindMessageOffset("GRAW", "cpMes")]); double prMes = double.Parse(items[DFLog.FindMessageOffset("GRAW", "prMes")]); double doMes = double.Parse(items[DFLog.FindMessageOffset("GRAW", "doMes")]); double mesQI = double.Parse(items[DFLog.FindMessageOffset("GRAW", "mesQI")]); double cno = double.Parse(items[DFLog.FindMessageOffset("GRAW", "cno")]); double lli = double.Parse(items[DFLog.FindMessageOffset("GRAW", "lli")]); DateTime gpstime = GetFromGps(week, weekms / 1000.0); if (week == 0) { continue; } if (lastgpstime != gpstime) { rinexoutput.WriteLine("> {0,4} {1,2} {2,2} {3,2} {4,2}{5,11} {6,1}{7,3}", gpstime.Year, gpstime.Month, gpstime.Day, gpstime.Hour, gpstime.Minute, (gpstime.Second + (gpstime.Millisecond / 1000.0)).ToString("0.0000000", System.Globalization.CultureInfo.InvariantCulture), 0, NSats); lastgpstime = gpstime; } string sattype = "G"; // exclude sbas sats if (sv > 32) { sattype = "S"; sv -= 100; } // a1,i2.2,satcount*(f14.3,i1,i1) rinexoutput.WriteLine("{0}{1,2}{2,14} {3,14}{4,1} {5,14} {6,14} ", sattype, (sv).ToString("00"), (prMes).ToString("0.000", System.Globalization.CultureInfo.InvariantCulture), (cpMes).ToString("0.000", System.Globalization.CultureInfo.InvariantCulture), lli, doMes, (cno).ToString("0.000", System.Globalization.CultureInfo.InvariantCulture)); } }
public void writeRinex(string filename) { if (gpsrawdata.Count == 0) { return; } string file = Path.GetDirectoryName(filename) + Path.DirectorySeparatorChar + Path.GetFileNameWithoutExtension(filename) + ".obs"; var rinexoutput = new StreamWriter(file); // 60 chars string header = @" 3.02 OBSERVATION DATA M: Mixed RINEX VERSION / TYPE MARKER NAME MARKER NUMBER MARKER TYPE OBSERVER / AGENCY REC # / TYPE / VERS ANT # / TYPE 0.0000 0.0000 0.0000 APPROX POSITION XYZ 0.0000 0.0000 0.0000 ANTENNA: DELTA H/E/N G 4 C1C L1C D1C S1C SYS / # / OBS TYPES S 4 C1C L1C D1C S1C SYS / # / OBS TYPES R 4 C1C L1C D1C S1C SYS / # / OBS TYPES E 4 C1C L1C D1C S1C SYS / # / OBS TYPES G SYS / PHASE SHIFT END OF HEADER "; rinexoutput.WriteLine(header); /* * { LOG_GPS_RAW_MSG, sizeof(log_GPS_RAW), \ * 690 "GRAW", "QIHBBddfBbB", "TimeUS,WkMS,Week,numSV,sv,cpMes,prMes,doMes,mesQI,cno,lli" }, \ + { LOG_GPS_RAWH_MSG, sizeof(log_GPS_RAWH), \ + 692 + "GRXH", "QdHbBB", "TimeUS,rcvTime,week,leapS,numMeas,recStat" }, \ + 693 + { LOG_GPS_RAWS_MSG, sizeof(log_GPS_RAWS), \ + 694 + "GRXS", "QddfBBBHBBBBB", "TimeUS,prMes,cpMes,doMes,gnss,sv,freq,lock,cno,prD,cpD,doD,trk" }, \ + */ /* * GNSS Identifiers * gnssId GNSS Type * 0 GPS * 1 SBAS * 2 Galileo * 3 BeiDou * 4 IMES * 5 QZSS * 6 GLONASS */ //ftp://igs.org/pub/data/format/rinex302.pdf //https://www.u-blox.com/images/downloads/Product_Docs/u-bloxM8_ReceiverDescriptionProtocolSpec_(UBX-13003221)_Public.pdf DateTime lastgpstime = DateTime.MinValue; double weekms = 0, NSats = 0; int week = 0; foreach (string line in gpsrawdata) { string sattype = "G"; string[] items = line.Split(',', ':'); double sv = -1, cpMes = -1, prMes = -1, doMes = -1, mesQI = -1, cno = -1, lli = -1; int gnss = 0; if (items[0].StartsWith("GRAW")) { weekms = double.Parse(items[DFLog.FindMessageOffset("GRAW", "WkMS")]); week = int.Parse(items[DFLog.FindMessageOffset("GRAW", "Week")]); NSats = double.Parse(items[DFLog.FindMessageOffset("GRAW", "numSV")]); sv = double.Parse(items[DFLog.FindMessageOffset("GRAW", "sv")]); cpMes = double.Parse(items[DFLog.FindMessageOffset("GRAW", "cpMes")]); prMes = double.Parse(items[DFLog.FindMessageOffset("GRAW", "prMes")]); doMes = double.Parse(items[DFLog.FindMessageOffset("GRAW", "doMes")]); mesQI = double.Parse(items[DFLog.FindMessageOffset("GRAW", "mesQI")]); cno = double.Parse(items[DFLog.FindMessageOffset("GRAW", "cno")]); lli = double.Parse(items[DFLog.FindMessageOffset("GRAW", "lli")]); if (sv > 32) { gnss = 1; } } else if (items[0].StartsWith("GRXH")) { weekms = double.Parse(items[DFLog.FindMessageOffset("GRXH", "rcvTime")]) * 1000.0; week = int.Parse(items[DFLog.FindMessageOffset("GRXH", "week")]); NSats = double.Parse(items[DFLog.FindMessageOffset("GRXH", "numMeas")]); continue; } else if (items[0].StartsWith("GRXS")) { sv = double.Parse(items[DFLog.FindMessageOffset("GRXS", "sv")]); cpMes = double.Parse(items[DFLog.FindMessageOffset("GRXS", "cpMes")]); prMes = double.Parse(items[DFLog.FindMessageOffset("GRXS", "prMes")]); doMes = double.Parse(items[DFLog.FindMessageOffset("GRXS", "doMes")]); gnss = int.Parse(items[DFLog.FindMessageOffset("GRXS", "gnss")]); cno = double.Parse(items[DFLog.FindMessageOffset("GRXS", "cno")]); double locktime = double.Parse(items[DFLog.FindMessageOffset("GRXS", "lock")]); lli = 0; // OK or not known } DateTime gpstime = GetFromGps(week, weekms / 1000.0); if (week == 0) { continue; } if (lastgpstime != gpstime) { rinexoutput.WriteLine("> {0,4} {1,2} {2,2} {3,2} {4,2}{5,11} {6,1}{7,3}", gpstime.Year, gpstime.Month, gpstime.Day, gpstime.Hour, gpstime.Minute, (gpstime.Second + (gpstime.Millisecond / 1000.0)).ToString("0.0000000", System.Globalization.CultureInfo.InvariantCulture), 0, NSats); lastgpstime = gpstime; } if (gnss == 0) { //GPS sattype = "G"; } if (gnss == 1) { //sbas sattype = "S"; sv -= 100; } if (gnss == 2) { //Galileo sattype = "E"; } if (gnss == 3) { //BEIDOU sattype = "C"; } if (gnss == 4) { //IMES sattype = "I"; } if (gnss == 5) { //QZSS sattype = "J"; sv -= 192; // check this (100 or 192) } if (gnss == 6) { //GLONASS sattype = "R"; } // a1,i2.2,satcount*(f14.3,i1,i1) rinexoutput.WriteLine("{0}{1,2}{2,14} {3,14}{4,1} {5,14} {6,14} ", sattype, (sv).ToString("00"), (prMes).ToString("0.000", System.Globalization.CultureInfo.InvariantCulture), (cpMes).ToString("0.000", System.Globalization.CultureInfo.InvariantCulture), lli, doMes, (cno).ToString("0.000", System.Globalization.CultureInfo.InvariantCulture)); } }
private void Form1_Load(object sender, EventArgs e) { rowno = 1; OpenFileDialog openFileDialog1 = new OpenFileDialog(); openFileDialog1.Filter = "Log Files|*.log;*.bin"; openFileDialog1.FilterIndex = 2; openFileDialog1.RestoreDirectory = true; openFileDialog1.InitialDirectory = MainV2.LogDir; if (openFileDialog1.ShowDialog() == DialogResult.OK) { try { Stream stream; if (openFileDialog1.FileName.ToLower().EndsWith(".bin")) { // extract log List <string> loglines = BinaryLog.ReadLog(openFileDialog1.FileName); // convert log to memory stream; stream = new MemoryStream(); // create single string with entire log foreach (string line in loglines) { stream.Write(ASCIIEncoding.ASCII.GetBytes(line), 0, line.Length); } // back to stream start stream.Seek(0, SeekOrigin.Begin); } else { stream = File.Open(openFileDialog1.FileName, FileMode.Open, FileAccess.Read, FileShare.Read); } var logdata = DFLog.ReadLog(stream); this.Text = "Log Browser - " + Path.GetFileName(openFileDialog1.FileName); m_dtCSV = new DataTable(); log.Info("process to datagrid"); foreach (var item in logdata) { if (item.items != null) { while (m_dtCSV.Columns.Count < (item.items.Length + typecoloum)) { m_dtCSV.Columns.Add(); } DataRow dr = m_dtCSV.NewRow(); dr[0] = item.lineno; dr[1] = item.time.ToString("yyyy-MM-dd HH:mm:ss.fff"); for (int a = 0; a < item.items.Length; a++) { dr[a + typecoloum] = item.items[a]; } m_dtCSV.Rows.Add(dr); } } log.Info("Done"); //PopulateDataTableFromUploadedFile(stream); stream.Close(); log.Info("set dgv datasourse"); dataGridView1.DataSource = m_dtCSV; dataGridView1.Columns[0].Visible = false; log.Info("datasource set"); } catch (Exception ex) { CustomMessageBox.Show("Failed to read File: " + ex.ToString()); } foreach (DataGridViewColumn column in dataGridView1.Columns) { column.SortMode = DataGridViewColumnSortMode.NotSortable; } log.Info("Get map"); DrawMap(); log.Info("map done"); CreateChart(zg1); } else { this.Close(); return; } }
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); }
string GetLog(ushort no) { log.Info("GetLog " + no); MainV2.comPort.Progress += comPort_Progress; status = serialstatus.Reading; // used for log fn byte[] hbpacket = MainV2.comPort.getHeartBeat(); if (hbpacket != null) log.Info("Got hbpacket length: " + hbpacket.Length); // get df log from mav using (var ms = MainV2.comPort.GetLog(no)) { ms.Seek(0, SeekOrigin.Begin); if (ms != null) log.Info("Got Log length: " + ms.Length); status = serialstatus.Done; updateDisplay(); MainV2.comPort.Progress -= comPort_Progress; MAVLink.mavlink_heartbeat_t hb = (MAVLink.mavlink_heartbeat_t) MainV2.comPort.DebugPacket(hbpacket); logfile = MainV2.LogDir + Path.DirectorySeparatorChar + MainV2.comPort.MAV.aptype.ToString() + Path.DirectorySeparatorChar + hbpacket[3] + Path.DirectorySeparatorChar + DateTime.Now.ToString("yyyy-MM-dd HH-mm-ss") + " " + no + ".bin"; // make log dir Directory.CreateDirectory(Path.GetDirectoryName(logfile)); log.Info("about to write: " + logfile); // save memorystream to file using (BinaryWriter bw = new BinaryWriter(File.OpenWrite(logfile))) { byte[] buffer = new byte[256*1024]; while (ms.Position < ms.Length) { int read = ms.Read(buffer, 0, buffer.Length); bw.Write(buffer, 0, read); } } } log.Info("about to convertbin: " + logfile); // create ascii log BinaryLog.ConvertBin(logfile, logfile + ".log"); //update the new filename logfile = logfile + ".log"; log.Info("about to GetFirstGpsTime: " + logfile); // get gps time of assci log DateTime logtime = new DFLog().GetFirstGpsTime(logfile); // rename log is we have a valid gps time if (logtime != DateTime.MinValue) { string newlogfilename = MainV2.LogDir + Path.DirectorySeparatorChar + MainV2.comPort.MAV.aptype.ToString() + Path.DirectorySeparatorChar + hbpacket[3] + Path.DirectorySeparatorChar + logtime.ToString("yyyy-MM-dd HH-mm-ss") + ".log"; try { File.Move(logfile, newlogfilename); // rename bin as well File.Move(logfile.Replace(".log", ""), newlogfilename.Replace(".log", ".bin")); logfile = newlogfilename; } catch { CustomMessageBox.Show(Strings.ErrorRenameFile + " " + logfile + "\nto " + newlogfilename, Strings.ERROR); } } return logfile; }
public static double[] getOffsetsLog(string fn) { // this is for a dxf Polyline3dVertex vertex; List <Polyline3dVertex> vertexes = new List <Polyline3dVertex>(); List <Tuple <float, float, float> > data = new List <Tuple <float, float, float> >(); List <Tuple <float, float, float> > data2 = new List <Tuple <float, float, float> >(); Log.DFLog dflog = new Log.DFLog(); var logfile = dflog.ReadLog(fn); foreach (var line in logfile) { if (line.msgtype == "MAG" || line.msgtype == "MAG2") { int indexmagx = dflog.FindMessageOffset(line.msgtype, "MagX"); int indexmagy = dflog.FindMessageOffset(line.msgtype, "MagY"); int indexmagz = dflog.FindMessageOffset(line.msgtype, "MagZ"); int indexoffsetx = dflog.FindMessageOffset(line.msgtype, "OfsX"); int indexoffsety = dflog.FindMessageOffset(line.msgtype, "OfsY"); int indexoffsetz = dflog.FindMessageOffset(line.msgtype, "OfsZ"); if (indexmagx != -1 && indexoffsetx != -1) { float magx = float.Parse(line.items[indexmagx]); float magy = float.Parse(line.items[indexmagy]); float magz = float.Parse(line.items[indexmagz]); float offsetx = float.Parse(line.items[indexoffsetx]); float offsety = float.Parse(line.items[indexoffsety]); float offsetz = float.Parse(line.items[indexoffsetz]); //offsetx = offsety = offsetz = 0; if (line.msgtype == "MAG") { data.Add(new Tuple <float, float, float>( magx - offsetx, magy - offsety, magz - offsetz)); // fox dxf vertex = new Polyline3dVertex(new Vector3f(magx - offsetx, magy - offsety, magz - offsetz) ); vertexes.Add(vertex); } else if (line.msgtype == "MAG2") { data2.Add(new Tuple <float, float, float>( magx - offsetx, magy - offsety, magz - offsetz)); } } } } double[] x = LeastSq(data); if (data2.Count > 0) { double[] x2 = LeastSq(data2); } log.Info("Least Sq Done " + DateTime.Now); doDXF(vertexes, x); Array.Resize <double>(ref x, 3); return(x); }
public static void MapLogs(string[] logs) { foreach (var logfile in logs) { if (File.Exists(logfile + ".jpg")) continue; 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")) { using (MAVLinkInterface mine = new MAVLinkInterface()) using ( mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.ReadWrite)) ) { mine.logreadmode = true; mine.speechenabled = false; while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length) { MAVLink.MAVLinkMessage packet = mine.readPacket(); if (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 = MAVList.GetID(packet.sysid, 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")) { bool bin = logfile.ToLower().EndsWith(".bin"); BinaryLog binlog = new BinaryLog(); DFLog dflog = new DFLog(); using (var st = File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.ReadWrite)) { using (StreamReader sr = new StreamReader(st)) { loc_list[0] = new List<PointLatLngAlt>(); while (sr.BaseStream.Position < sr.BaseStream.Length) { string line = ""; if (bin) { line = binlog.ReadMessage(sr.BaseStream); } else { line = sr.ReadLine(); } if (line.StartsWith("FMT")) { dflog.FMTLine(line); } else if (line.StartsWith("GPS")) { var item = dflog.GetDFItemFromLine(line, 0); if (!dflog.logformat.ContainsKey("GPS")) continue; var status = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "Status")]); var lat = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "Lat")]); var lon = double.Parse(item.items[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); var map = GetMap(area); 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); map.Dispose(); map = null; } else { DoTextMap(logfile + ".jpg", "No gps data"); } } catch (Exception ex) { if (ex.ToString().Contains("Mavlink 0.9")) DoTextMap(logfile + ".jpg", "Old log\nMavlink 0.9"); continue; } } }
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); }
string GetLog(string no, string fileName) { log.Info("GetLog " + no); status = SerialStatus.Reading; logfile = Settings.Instance.LogDir + Path.DirectorySeparatorChar + MakeValidFileName(fileName) + ".bin"; // make log dir Directory.CreateDirectory(Path.GetDirectoryName(logfile)); log.Info("about to write: " + logfile); // save memorystream to file SftpClient client = new SftpClient(_connectionInfo); client.Connect(); using (var logstream = File.Open(logfile, FileMode.Create, FileAccess.Write)) { client.DownloadFile(no, logstream, downloadCallback); } client.Disconnect(); log.Info("about to convertbin: " + logfile); // create ascii log BinaryLog.ConvertBin(logfile, logfile + ".log"); //update the new filename logfile = logfile + ".log"; // rename file if needed log.Info("about to GetFirstGpsTime: " + logfile); // get gps time of assci log DateTime logtime = new DFLog().GetFirstGpsTime(logfile); // rename log is we have a valid gps time if (logtime != DateTime.MinValue) { string newlogfilename = Settings.Instance.LogDir + Path.DirectorySeparatorChar + logtime.ToString("yyyy-MM-dd HH-mm-ss") + ".log"; try { File.Move(logfile, newlogfilename); // rename bin as well File.Move(logfile.Replace(".log", ""), newlogfilename.Replace(".log", ".bin")); logfile = newlogfilename; } catch { CustomMessageBox.Show(Strings.ErrorRenameFile + " " + logfile + "\nto " + newlogfilename, Strings.ERROR); } } MainV2.comPort.Progress -= comPort_Progress; return(logfile); }
string GetLog(ushort no) { MainV2.comPort.Progress += comPort_Progress; status = serialstatus.Reading; // get df log from mav var ms = MainV2.comPort.GetLog(no); status = serialstatus.Done; updateDisplay(); MainV2.comPort.Progress -= comPort_Progress; // set log fn byte[] hbpacket = MainV2.comPort.getHeartBeat(); MAVLink.mavlink_heartbeat_t hb = (MAVLink.mavlink_heartbeat_t)MainV2.comPort.DebugPacket(hbpacket); logfile = MainV2.LogDir + Path.DirectorySeparatorChar + MainV2.comPort.MAV.aptype.ToString() + Path.DirectorySeparatorChar + hbpacket[3] + Path.DirectorySeparatorChar + DateTime.Now.ToString("yyyy-MM-dd HH-mm-ss") + " " + no + ".bin"; // make log dir Directory.CreateDirectory(Path.GetDirectoryName(logfile)); // save memorystream to file using (BinaryWriter bw = new BinaryWriter(File.OpenWrite(logfile))) { bw.Write(ms.ToArray()); } // create ascii log BinaryLog.ConvertBin(logfile, logfile + ".log"); //update the new filename logfile = logfile + ".log"; // get gps time of assci log DateTime logtime = new DFLog().GetFirstGpsTime(logfile); // rename log is we have a valid gps time if (logtime != DateTime.MinValue) { string newlogfilename = MainV2.LogDir + Path.DirectorySeparatorChar + MainV2.comPort.MAV.aptype.ToString() + Path.DirectorySeparatorChar + hbpacket[3] + Path.DirectorySeparatorChar + logtime.ToString("yyyy-MM-dd HH-mm-ss") + ".log"; try { File.Move(logfile, newlogfilename); // rename bin as well File.Move(logfile.Replace(".log", ""), newlogfilename.Replace(".log", ".bin")); logfile = newlogfilename; } catch { CustomMessageBox.Show(Strings.ErrorRenameFile+ " " + logfile + "\nto " + newlogfilename, Strings.ERROR); } } return logfile; }
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; }
PointLatLng? getPointLatLng(DFLog.DFItem item) { if (item.msgtype == "GPS") { if (!DFLog.logformat.ContainsKey("GPS")) return null; int index = FindInArray(DFLog.logformat["GPS"].FieldNames, "Lat") + 1; if (index == -1) { return null; } int index2 = FindInArray(DFLog.logformat["GPS"].FieldNames, "Lng") + 1; if (index2 == -1) { return null; } int index3 = FindInArray(DFLog.logformat["GPS"].FieldNames, "Status") + 1; if (index3 == -1) { return null; } try { if (double.Parse(item.items[index3].ToString(), System.Globalization.CultureInfo.InvariantCulture) != 3) { return null; } string lat = item.items[index].ToString(); string lng = item.items[index2].ToString(); PointLatLng pnt = new PointLatLng() { }; pnt.Lat = double.Parse(lat, System.Globalization.CultureInfo.InvariantCulture); pnt.Lng = double.Parse(lng, System.Globalization.CultureInfo.InvariantCulture); return pnt; } catch { } } return null; }
private void PopulateDataTableFromUploadedFile(System.IO.Stream strm) { System.IO.StreamReader srdr = new System.IO.StreamReader(strm); String strLine = String.Empty; Int32 iLineCount = 0; DFLog.Clear(); do { strLine = srdr.ReadLine(); if (strLine == null) { break; } else if (strLine == "") { // continue; } if (0 == iLineCount++) { m_dtCSV = new DataTable("CSVTable"); } this.AddDataRowToTable(strLine, m_dtCSV); strLine = strLine.Replace(", ", ","); strLine = strLine.Replace(": ", ":"); string[] items = strLine.Split(',', ':'); // populate fw type if (items[0].Contains("SYSID_SW_TYPE")) { switch (items[1].ToString()) { case "10": MainV2.comPort.MAV.cs.firmware = MainV2.Firmwares.ArduCopter2; break; case "7": MainV2.comPort.MAV.cs.firmware = MainV2.Firmwares.Ateryx; break; case "20": MainV2.comPort.MAV.cs.firmware = MainV2.Firmwares.ArduRover; break; case "0": MainV2.comPort.MAV.cs.firmware = MainV2.Firmwares.ArduPlane; break; } } if (items[0].Contains("FMT")) { try { DFLog.FMTLine(strLine); } catch { } } } while (true); }