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 (DFLogBuffer colbuf = new DFLogBuffer(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); }
private void but_ISBH_Click(object sender, EventArgs e) { Utilities.FFT2 fft = new FFT2(); using ( OpenFileDialog ofd = new OpenFileDialog()) { ofd.Filter = "*.log;*.bin|*.log;*.bin;*.BIN;*.LOG"; ofd.ShowDialog(); if (!File.Exists(ofd.FileName)) { return; } var file = new DFLogBuffer(File.OpenRead(ofd.FileName)); int bins = (int)NUM_bins.Value; int N = 1 << bins; Color[] color = new Color[] { Color.Red, Color.Green, Color.Blue, Color.Black, Color.Violet, Color.Orange }; ZedGraphControl[] ctls = new ZedGraphControl[] { zedGraphControl1, zedGraphControl2, zedGraphControl3, zedGraphControl4, zedGraphControl5, zedGraphControl6 }; // 3 imus * 2 sets of measurements(gyr/acc) FFT2.datastate[] alldata = new FFT2.datastate[3 * 2]; for (int a = 0; a < alldata.Length; a++) { alldata[a] = new FFT2.datastate(); } // state cache int Ns = 0; int type = 0; int instance = 0; int sensorno = 0; foreach (var item in file.GetEnumeratorType(new string[] { "ISBH", "ISBD" })) { if (item.msgtype == null) { continue; } if (item.msgtype.StartsWith("ISBH")) { Ns = int.Parse(item.items[file.dflog.FindMessageOffset(item.msgtype, "N")], CultureInfo.InvariantCulture); type = int.Parse(item.items[file.dflog.FindMessageOffset(item.msgtype, "type")], CultureInfo.InvariantCulture); instance = int.Parse(item.items[file.dflog.FindMessageOffset(item.msgtype, "instance")], CultureInfo.InvariantCulture); sensorno = type * 3 + instance; alldata[sensorno].sample_rate = double.Parse(item.items[file.dflog.FindMessageOffset(item.msgtype, "smp_rate")], CultureInfo.InvariantCulture); if (type == 0) { alldata[sensorno].type = "ACC" + instance.ToString(); } if (type == 1) { alldata[sensorno].type = "GYR" + instance.ToString(); } } else if (item.msgtype.StartsWith("ISBD")) { var Nsdata = int.Parse(item.items[file.dflog.FindMessageOffset(item.msgtype, "N")], CultureInfo.InvariantCulture); if (Ns != Nsdata) { continue; } int offsetX = file.dflog.FindMessageOffset(item.msgtype, "x"); int offsetY = file.dflog.FindMessageOffset(item.msgtype, "y"); int offsetZ = file.dflog.FindMessageOffset(item.msgtype, "z"); int offsetTime = file.dflog.FindMessageOffset(item.msgtype, "TimeUS"); double time = double.Parse(item.items[offsetTime], CultureInfo.InvariantCulture) / 1000.0; if (time < alldata[sensorno].lasttime) { continue; } if (time != alldata[sensorno].lasttime) { alldata[sensorno].timedelta = alldata[sensorno].timedelta * 0.99 + (time - alldata[sensorno].lasttime) * 0.01; } alldata[sensorno].lasttime = time; item.items[offsetX].Split(new[] { ' ', '[', ']' }, StringSplitOptions.RemoveEmptyEntries).ForEach(aa => { alldata[sensorno].datax.Add(double.Parse(aa, CultureInfo.InvariantCulture)); }); item.items[offsetY].Split(new[] { ' ', '[', ']' }, StringSplitOptions.RemoveEmptyEntries).ForEach(aa => { alldata[sensorno].datay.Add(double.Parse(aa, CultureInfo.InvariantCulture)); }); item.items[offsetZ].Split(new[] { ' ', '[', ']' }, StringSplitOptions.RemoveEmptyEntries).ForEach(aa => { alldata[sensorno].dataz.Add(double.Parse(aa, CultureInfo.InvariantCulture)); }); } } int controlindex = 0; foreach (var sensordata in alldata) { if (sensordata.datax.Count <= N) { continue; } double samplerate = 0; samplerate = sensordata.sample_rate;// Math.Round(1000 / sensordata.timedelta, 1); double[] freqt = fft.FreqTable(N, (int)samplerate); double[] avgx = new double[N / 2]; double[] avgy = new double[N / 2]; double[] avgz = new double[N / 2]; int totalsamples = sensordata.datax.Count; int count = totalsamples / N; int done = 0; while (count > 1) // skip last part { var fftanswerx = fft.rin(sensordata.datax.Skip(N * done).Take(N).ToArray(), (uint)bins); var fftanswery = fft.rin(sensordata.datay.Skip(N * done).Take(N).ToArray(), (uint)bins); var fftanswerz = fft.rin(sensordata.dataz.Skip(N * done).Take(N).ToArray(), (uint)bins); for (int b = 0; b < N / 2; b++) { if (freqt[b] < (double)NUM_startfreq.Value) { continue; } avgx[b] += fftanswerx[b] / (done + count); avgy[b] += fftanswery[b] / (done + count); avgz[b] += fftanswerz[b] / (done + count); } count--; done++; } ZedGraph.PointPairList pplx = new ZedGraph.PointPairList(freqt, avgx); ZedGraph.PointPairList pply = new ZedGraph.PointPairList(freqt, avgy); ZedGraph.PointPairList pplz = new ZedGraph.PointPairList(freqt, avgz); var curvex = new LineItem(sensordata.type + " x", pplx, color[0], SymbolType.None); var curvey = new LineItem(sensordata.type + " y", pply, color[1], SymbolType.None); var curvez = new LineItem(sensordata.type + " z", pplz, color[2], SymbolType.None); ctls[controlindex].GraphPane.Legend.IsVisible = true; ctls[controlindex].GraphPane.XAxis.Title.Text = "Freq Hz"; ctls[controlindex].GraphPane.YAxis.Title.Text = "Amplitude"; ctls[controlindex].GraphPane.Title.Text = "FFT " + sensordata.type + " - " + Path.GetFileName(ofd.FileName) + " - " + samplerate + "hz input"; ctls[controlindex].GraphPane.CurveList.Clear(); ctls[controlindex].GraphPane.CurveList.Add(curvex); ctls[controlindex].GraphPane.CurveList.Add(curvey); ctls[controlindex].GraphPane.CurveList.Add(curvez); ctls[controlindex].Invalidate(); ctls[controlindex].AxisChange(); ctls[controlindex].GraphPane.XAxis.Scale.Max = samplerate / 2; ctls[controlindex].Refresh(); controlindex++; } SetScale(ctls); } }
//FMT, 131, 43, IMU, IffffffIIf, TimeMS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ,ErrG,ErrA,Temp //FMT, 135, 43, IMU2, IffffffIIf, TimeMS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ,ErrG,ErrA,Temp //FMT, 149, 43, IMU3, IffffffIIf, TimeMS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ,ErrG,ErrA,Temp //FMT, 172, 23, ACC1, IIfff, TimeMS,TimeUS,AccX,AccY,AccZ //FMT, 173, 23, ACC2, IIfff, TimeMS,TimeUS,AccX,AccY,AccZ //FMT, 174, 23, ACC3, IIfff, TimeMS,TimeUS,AccX,AccY,AccZ //FMT, 175, 23, GYR1, IIfff, TimeMS,TimeUS,GyrX,GyrY,GyrZ //FMT, 176, 23, GYR2, IIfff, TimeMS,TimeUS,GyrX,GyrY,GyrZ //FMT, 177, 23, GYR3, IIfff, TimeMS,TimeUS,GyrX,GyrY,GyrZ private void myButton1_Click(object sender, EventArgs e) { Utilities.FFT2 fft = new FFT2(); using (OpenFileDialog ofd = new OpenFileDialog()) { ofd.Filter = "*.log;*.bin|*.log;*.bin;*.BIN;*.LOG"; ofd.ShowDialog(); if (!File.Exists(ofd.FileName)) { return; } var file = new DFLogBuffer(File.OpenRead(ofd.FileName)); int bins = (int)NUM_bins.Value; int N = 1 << bins; double[] datainGX = new double[N]; double[] datainGY = new double[N]; double[] datainGZ = new double[N]; double[] datainAX = new double[N]; double[] datainAY = new double[N]; double[] datainAZ = new double[N]; List <double[]> avg = new List <double[]> { // 6 new double[N / 2], new double[N / 2], new double[N / 2], new double[N / 2], new double[N / 2], new double[N / 2] }; object[] datas = new object[] { datainGX, datainGY, datainGZ, datainAX, datainAY, datainAZ }; string[] datashead = new string[] { "GYR1-GyrX", "GYR1-GyrY", "GYR1-GyrZ", "ACC1-AccX", "ACC1-AccY", "ACC1-AccZ" }; Color[] color = new Color[] { Color.Red, Color.Green, Color.Black, Color.Violet, Color.Blue, Color.Orange }; ZedGraphControl[] ctls = new ZedGraphControl[] { zedGraphControl1, zedGraphControl2, zedGraphControl3, zedGraphControl4, zedGraphControl5, zedGraphControl6 }; int samplecounta = 0; int samplecountg = 0; double lasttime = 0; double timedelta = 0; double[] freqt = null; double samplerate = 0; foreach (var item in file.GetEnumeratorType(new string[] { "ACC1", "GYR1" })) { if (item.msgtype == "ACC1") { int offsetAX = file.dflog.FindMessageOffset("ACC1", "AccX"); int offsetAY = file.dflog.FindMessageOffset("ACC1", "AccY"); int offsetAZ = file.dflog.FindMessageOffset("ACC1", "AccZ"); int offsetTime = file.dflog.FindMessageOffset("ACC1", "TimeUS"); double time = double.Parse(item.items[offsetTime], CultureInfo.InvariantCulture) / 1000.0; timedelta = timedelta * 0.99 + (time - lasttime) * 0.01; // we missed gyro data if (samplecounta >= N) { continue; } datainAX[samplecounta] = double.Parse(item.items[offsetAX], CultureInfo.InvariantCulture); datainAY[samplecounta] = double.Parse(item.items[offsetAY], CultureInfo.InvariantCulture); datainAZ[samplecounta] = double.Parse(item.items[offsetAZ], CultureInfo.InvariantCulture); samplecounta++; lasttime = time; } else if (item.msgtype == "GYR1") { int offsetGX = file.dflog.FindMessageOffset("GYR1", "GyrX"); int offsetGY = file.dflog.FindMessageOffset("GYR1", "GyrY"); int offsetGZ = file.dflog.FindMessageOffset("GYR1", "GyrZ"); int offsetTime = file.dflog.FindMessageOffset("ACC1", "TimeUS"); double time = double.Parse(item.items[offsetTime], CultureInfo.InvariantCulture) / 1000.0; // we missed accel data if (samplecountg >= N) { continue; } datainGX[samplecountg] = double.Parse(item.items[offsetGX], CultureInfo.InvariantCulture); datainGY[samplecountg] = double.Parse(item.items[offsetGY], CultureInfo.InvariantCulture); datainGZ[samplecountg] = double.Parse(item.items[offsetGZ], CultureInfo.InvariantCulture); samplecountg++; } if (samplecounta >= N && samplecountg >= N) { int inputdataindex = 0; foreach (var itemlist in datas) { var fftanswer = fft.rin((double[])itemlist, (uint)bins); for (int b = 0; b < N / 2; b++) { avg[inputdataindex][b] += fftanswer[b] * (1.0 / (N / 2.0)); } samplecounta = 0; samplecountg = 0; inputdataindex++; } } } if (freqt == null) { samplerate = Math.Round(1000 / timedelta, 1); freqt = fft.FreqTable(N, (int)samplerate); } // 0 out all data befor cutoff for (int inputdataindex = 0; inputdataindex < 6; inputdataindex++) { for (int b = 0; b < N / 2; b++) { if (freqt[b] < (double)NUM_startfreq.Value) { avg[inputdataindex][b] = 0; continue; } break; } } int controlindex = 0; foreach (var item in avg) { ZedGraph.PointPairList ppl = new ZedGraph.PointPairList(freqt, item); //double xMin, xMax, yMin, yMax; var curve = new LineItem(datashead[controlindex], ppl, color[controlindex], SymbolType.None); //curve.GetRange(out xMin, out xMax, out yMin, out yMax, true, false, ctls[c].GraphPane); ctls[controlindex].GraphPane.Legend.IsVisible = false; ctls[controlindex].GraphPane.XAxis.Title.Text = "Freq Hz"; ctls[controlindex].GraphPane.YAxis.Title.Text = "Amplitude"; ctls[controlindex].GraphPane.Title.Text = "FFT " + datashead[controlindex] + " - " + Path.GetFileName(ofd.FileName) + " - " + samplerate + "hz input"; ctls[controlindex].GraphPane.CurveList.Clear(); ctls[controlindex].GraphPane.CurveList.Add(curve); ctls[controlindex].Invalidate(); ctls[controlindex].AxisChange(); ctls[controlindex].Refresh(); controlindex++; } SetScale(ctls); } }
public static List <Tuple <DFLog.DFItem, double> > ProcessExpression(DFLog dflog, DFLogBuffer 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_func()); 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") && (item.instance == "" || item.instance == "0")) { 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" && (item.instance == "" || item.instance == "0")) { 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); }
private void but_fftimu13_Click(object sender, EventArgs e) { Utilities.FFT2 fft = new FFT2(); using ( OpenFileDialog ofd = new OpenFileDialog()) { ofd.Filter = "*.log;*.bin|*.log;*.bin;*.BIN;*.LOG"; ofd.ShowDialog(); if (!File.Exists(ofd.FileName)) { return; } var file = new DFLogBuffer(File.OpenRead(ofd.FileName)); int bins = (int)NUM_bins.Value; int N = 1 << bins; Color[] color = new Color[] { Color.Red, Color.Green, Color.Blue, Color.Black, Color.Violet, Color.Orange }; ZedGraphControl[] ctls = new ZedGraphControl[] { zedGraphControl1, zedGraphControl2, zedGraphControl3, zedGraphControl4, zedGraphControl5, zedGraphControl6 }; // 3 imus * 2 sets of measurements(gyr/acc) FFT2.datastate[] alldata = new FFT2.datastate[3 * 2]; for (int a = 0; a < alldata.Length; a++) { alldata[a] = new FFT2.datastate(); } foreach (var item in file.GetEnumeratorType(new string[] { "IMU", "IMU2", "IMU3" })) { if (item.msgtype == null) { continue; } if (item.msgtype.StartsWith("IMU")) { int sensorno = 0; if (item.msgtype == "IMU") { sensorno = 0; } if (item.msgtype == "IMU2") { sensorno = 1; } if (item.msgtype == "IMU3") { sensorno = 2; } alldata[sensorno + 3].type = item.msgtype + " ACC"; int offsetAX = file.dflog.FindMessageOffset(item.msgtype, "AccX"); int offsetAY = file.dflog.FindMessageOffset(item.msgtype, "AccY"); int offsetAZ = file.dflog.FindMessageOffset(item.msgtype, "AccZ"); int offsetTime = file.dflog.FindMessageOffset(item.msgtype, "TimeUS"); double time = double.Parse(item.items[offsetTime], CultureInfo.InvariantCulture) / 1000.0; if (time != alldata[sensorno + 3].lasttime) { alldata[sensorno + 3].timedelta = alldata[sensorno + 3].timedelta * 0.99 + (time - alldata[sensorno + 3].lasttime) * 0.01; } alldata[sensorno + 3].lasttime = time; alldata[sensorno + 3].datax.Add(double.Parse(item.items[offsetAX], CultureInfo.InvariantCulture)); alldata[sensorno + 3].datay.Add(double.Parse(item.items[offsetAY], CultureInfo.InvariantCulture)); alldata[sensorno + 3].dataz.Add(double.Parse(item.items[offsetAZ], CultureInfo.InvariantCulture)); //gyro alldata[sensorno].type = item.msgtype + " GYR"; int offsetGX = file.dflog.FindMessageOffset(item.msgtype, "GyrX"); int offsetGY = file.dflog.FindMessageOffset(item.msgtype, "GyrY"); int offsetGZ = file.dflog.FindMessageOffset(item.msgtype, "GyrZ"); if (time != alldata[sensorno].lasttime) { alldata[sensorno].timedelta = alldata[sensorno].timedelta * 0.99 + (time - alldata[sensorno].lasttime) * 0.01; } alldata[sensorno].lasttime = time; alldata[sensorno].datax.Add(double.Parse(item.items[offsetGX], CultureInfo.InvariantCulture)); alldata[sensorno].datay.Add(double.Parse(item.items[offsetGY], CultureInfo.InvariantCulture)); alldata[sensorno].dataz.Add(double.Parse(item.items[offsetGZ], CultureInfo.InvariantCulture)); } } int controlindex = 0; foreach (var sensordata in alldata) { if (sensordata.datax.Count <= N) { continue; } double samplerate = 0; samplerate = Math.Round(1000 / sensordata.timedelta, 1); double[] freqt = fft.FreqTable(N, (int)samplerate); double[] avgx = new double[N / 2]; double[] avgy = new double[N / 2]; double[] avgz = new double[N / 2]; int totalsamples = sensordata.datax.Count; int count = totalsamples / N; int done = 0; while (count > 1) // skip last part { var fftanswerx = fft.rin(sensordata.datax.AsSpan().Slice(N * done, N), (uint)bins, indB); var fftanswery = fft.rin(sensordata.datay.AsSpan().Slice(N * done, N), (uint)bins, indB); var fftanswerz = fft.rin(sensordata.dataz.AsSpan().Slice(N * done, N), (uint)bins, indB); for (int b = 0; b < N / 2; b++) { if (freqt[b] < (double)NUM_startfreq.Value) { continue; } avgx[b] += fftanswerx[b] / (done + count); avgy[b] += fftanswery[b] / (done + count); avgz[b] += fftanswerz[b] / (done + count); } count--; done++; } ZedGraph.PointPairList pplx = new ZedGraph.PointPairList(freqt, avgx); ZedGraph.PointPairList pply = new ZedGraph.PointPairList(freqt, avgy); ZedGraph.PointPairList pplz = new ZedGraph.PointPairList(freqt, avgz); var curvex = new LineItem(sensordata.type + " x", pplx, color[0], SymbolType.None); var curvey = new LineItem(sensordata.type + " y", pply, color[1], SymbolType.None); var curvez = new LineItem(sensordata.type + " z", pplz, color[2], SymbolType.None); ctls[controlindex].GraphPane.Legend.IsVisible = true; ctls[controlindex].GraphPane.XAxis.Title.Text = "Freq Hz"; ctls[controlindex].GraphPane.YAxis.Title.Text = "Amplitude"; ctls[controlindex].GraphPane.Title.Text = "FFT " + sensordata.type + " - " + Path.GetFileName(ofd.FileName) + " - " + samplerate + "hz input"; ctls[controlindex].GraphPane.CurveList.Clear(); ctls[controlindex].GraphPane.CurveList.Add(curvex); ctls[controlindex].GraphPane.CurveList.Add(curvey); ctls[controlindex].GraphPane.CurveList.Add(curvez); ctls[controlindex].Invalidate(); ctls[controlindex].AxisChange(); ctls[controlindex].GraphPane.XAxis.Scale.Max = samplerate / 2; ctls[controlindex].Refresh(); controlindex++; } SetScale(ctls); } }
public static void SortLogs(string[] logs, string masterdestdir = "") { Parallel.ForEach(logs, logfile => //foreach (var logfile in logs) { if (masterdestdir == "") { masterdestdir = Path.GetDirectoryName(logfile); } try { FileInfo info; info = new FileInfo(logfile); // delete 0 size files if (info.Length == 0) { try { File.Delete(logfile); } catch { } return; } // move small logs - most likerly invalid if (info.Length <= 1024) { try { string destdir = masterdestdir + Path.DirectorySeparatorChar + "SMALL" + Path.DirectorySeparatorChar; if (!Directory.Exists(destdir)) { Directory.CreateDirectory(destdir); } log.Info("Move log small " + logfile + " to " + destdir + Path.GetFileName(logfile)); MoveFileUsingMask(logfile, destdir); } catch { } return; } bool issitl = false; var sysid = 0; var compid = 0; var aptype = MAVLink.MAV_TYPE.GENERIC; var packetsseen = 0; if (logfile.ToLower().EndsWith(".bin") || logfile.ToLower().EndsWith(".log")) { var logBuffer = new DFLogBuffer(File.OpenRead(logfile)); //PARM, 68613507, SYSID_THISMAV, 1 var sysidlist = logBuffer.GetEnumeratorType("PARM").Where(a => a["Name"] == "SYSID_THISMAV"); sysid = int.Parse(sysidlist.First()["Value"].ToString()); //logBuffer.dflog if (logBuffer.SeenMessageTypes.Contains("SIM")) { logBuffer.Dispose(); var destdir = masterdestdir + Path.DirectorySeparatorChar + "SITL" + Path.DirectorySeparatorChar + aptype.ToString() + Path.DirectorySeparatorChar + sysid + Path.DirectorySeparatorChar; if (!Directory.Exists(destdir)) { Directory.CreateDirectory(destdir); } MoveFileUsingMask(logfile, destdir); } return; } else { } var parse = new MAVLink.MavlinkParse(true); using (var binfile = File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read)) { var midpoint = binfile.Length / 8; binfile.Seek(midpoint, SeekOrigin.Begin); MAVLink.MAVLinkMessage packet; List <MAVLink.MAVLinkMessage> hblist = new List <MAVLink.MAVLinkMessage>(); try { var length = binfile.Length; while (binfile.Position < length) { packet = parse.ReadPacket(binfile); // no gcs packets if (packet == null || packet.compid == 190) { continue; } if (packet.msgid == (uint)MAVLink.MAVLINK_MSG_ID.SIMSTATE) { packetsseen++; issitl = true; } else if (packet.msgid == (uint)MAVLink.MAVLINK_MSG_ID.HEARTBEAT) { packetsseen++; hblist.Add(packet); var hb = (MAVLink.mavlink_heartbeat_t)packet.data; sysid = packet.sysid; compid = packet.compid; aptype = (MAVLink.MAV_TYPE)hb.type; if (hblist.Count > 10) { break; } } else if (packet != MAVLink.MAVLinkMessage.Invalid) { packetsseen++; if (sysid == 0) { sysid = packet.sysid; } if (compid == 0) { compid = packet.compid; } } } } catch (EndOfStreamException) { } catch { } if (hblist.Count == 0) { binfile.Close(); if (!Directory.Exists(masterdestdir + Path.DirectorySeparatorChar + "BAD")) { Directory.CreateDirectory(masterdestdir + Path.DirectorySeparatorChar + "BAD"); } log.Info("Move log bad " + logfile + " to " + masterdestdir + Path.DirectorySeparatorChar + "BAD" + Path.DirectorySeparatorChar + Path.GetFileName(logfile) + " " + info.Length); MoveFileUsingMask(logfile, masterdestdir + Path.DirectorySeparatorChar + "BAD" + Path.DirectorySeparatorChar); return; } // find most appropriate if (hblist.GroupBy(a => a.sysid * 256 + a.compid).ToArray().Length > 1) { foreach (var mav in hblist) { var hb = (MAVLink.mavlink_heartbeat_t)mav.data; if (hb.type == (byte)MAVLink.MAV_TYPE.ANTENNA_TRACKER) { continue; } if (hb.type == (byte)MAVLink.MAV_TYPE.GCS) { continue; } sysid = mav.sysid; compid = mav.compid; aptype = (MAVLink.MAV_TYPE)hb.type; } } binfile.Close(); string destdir = masterdestdir + Path.DirectorySeparatorChar + aptype.ToString() + Path.DirectorySeparatorChar + sysid + Path.DirectorySeparatorChar; if (issitl) { destdir = masterdestdir + Path.DirectorySeparatorChar + "SITL" + Path.DirectorySeparatorChar + aptype.ToString() + Path.DirectorySeparatorChar + sysid + Path.DirectorySeparatorChar; } if (!Directory.Exists(destdir)) { Directory.CreateDirectory(destdir); } MoveFileUsingMask(logfile, destdir); } } catch { return; } }); }
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) { try { 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); } } catch { } } } cf.Close(); } else if (logfile.ToLower().EndsWith(".bin") || logfile.ToLower().EndsWith(".log")) { using ( DFLogBuffer colbuf = new DFLogBuffer(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", SKEncodedImageFormat.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"); } } }