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");
}
}
}