public static Image <Rgba32> GenerateImage(DFLogBuffer cb, string type = "ACC1", string field = "AccX", string timeus = "TimeUS")
{
var bins = 10;
int N = 1 << bins;
var fft = new FFT2();
var acc1data = cb.GetEnumeratorType(type).ToArray();
log.Debug(type);
var firstsample = acc1data.Take(N);
var samplemin = double.Parse(firstsample.Min(a => a[timeus]));
var samplemax = double.Parse(firstsample.Max(a => a[timeus]));
log.Debug("samplemin " + samplemin + " samplemax " + samplemax);
var timedelta = samplemin - samplemax;
log.Debug(" timedelta " + timedelta);
double[] freqt = fft.FreqTable(N, (int)(1000 / (N / timedelta)));
// time , freq , [color] &freqcount
int totalsamples = acc1data.Count();
int count = totalsamples / N;
int done = 0;
// 50% overlap
int divisor = 2;
count *= divisor;
var img = new Image <Rgba32>(count, freqt.Length);
log.Debug("done and count ");
while (count > 1) // skip last part
{
var fftdata = acc1data.Skip((N * (done / divisor))).Take(N);
var time = fftdata.Skip(N / 2).First().time.ToString("o");
var fftanswerz = fft.rin(fftdata.Select(a => (double)(float)a.GetRaw(field)).ToArray(), (uint)bins);
//plotlydata.root.z.Add(fftanswerz.Select(a => a > 2 ? 0 : a).ToArray());
freqt.Select((y, i) => img[done, (freqt.Length - 1) - i] = GetColor(fftanswerz[i])).ToArray();
count--;
done++;
}
return(img);
}
public long GetLineNoFromTime(DFLogBuffer logdata, DateTime p1)
{
DateTime last = DateTime.MaxValue;
foreach (var dfItem in logdata.GetEnumeratorType(new string[] { "GPS", "GPS2" }))
{
// always forwards
if (dfItem.time >= p1)
{
return(dfItem.lineno);
}
last = dfItem.time;
}
if (last != DateTime.MaxValue)
{
return(long.MaxValue);
}
return(0);
}
public static void Create(string filein, string fileout, List <string> fmtList = null)
{
using (StreamReader tr = new StreamReader(filein))
using (DFLogBuffer logdata = new DFLogBuffer(tr.BaseStream))
{
List <string> colList = new List <string>();
Dictionary <string, int> colStart = new Dictionary <string, int>();
colStart.Add("GLOBAL", colList.Count);
colList.Add("GLOBAL_TimeMS");
foreach (var logformatValue in logdata.dflog.logformat.Values)
{
if (fmtList != null && !fmtList.Contains(logformatValue.Name))
{
continue;
}
colStart.Add(logformatValue.Name, colList.Count);
foreach (var field in logformatValue.FieldNames)
{
colList.Add(logformatValue.Name + "_" + field);
}
}
using (StreamWriter sr = new StreamWriter(fileout))
{
// header
foreach (var item in colList)
{
sr.Write(item + ",");
}
sr.WriteLine();
// lines
foreach (var dfitem in logdata.GetEnumeratorType(colStart.Keys.ToArray()))
{
if (dfitem.msgtype == "FMT")
{
continue;
}
var idx = 0;
StringBuilder sb = new StringBuilder();
sb.Append(dfitem.timems);
idx++;
sb.Append(',');
var start = colStart[dfitem.msgtype];
while (idx < start)
{
idx++;
sb.Append(',');
}
foreach (var item in dfitem.items.Skip(1))
{
sb.Append(item?.Trim());
idx++;
sb.Append(',');
}
sr.WriteLine(sb);
}
}
}
}
public static void getdata(string file)
{
DFLog.DFItem? IMU = null;
DFLog.DFItem? ATT = null;
List <Vector3> gaccel = new List <Vector3>();
List <Vector3> vel = new List <Vector3>();
List <DateTime> timestamps = new List <DateTime>();
List <int> accel_indexes = new List <int>();
var dtsum = 0.0;
var dtcount = 0;
var cb = new DFLogBuffer(File.OpenRead(file));
foreach (var item in cb.GetEnumeratorType(new string[] { "GPS", "GPS2", "IMU", "ATT" }))
{
switch (item.msgtype)
{
case "GPS":
case "GPS2":
if ((Byte)item.GetRaw("Status") >= 3)
{
var v = new Vector3(
(Single)item.GetRaw("Spd") * Math.Cos(radians((Single)item.GetRaw("GCrs"))),
(Single)item.GetRaw("Spd") * Math.Sin(radians((Single)item.GetRaw("GCrs"))),
(Single)item.GetRaw("VZ"));
vel.Add(v);
timestamps.Add(item.time);
accel_indexes.Add(Math.Max(gaccel.Count - 1, 0));
}
break;
case "IMU":
if (ATT.HasValue)
{
gaccel.Add(earth_accel_df(item, ATT.Value));
if (IMU.HasValue)
{
var dt = item.time - IMU.Value.time;
dtsum += dt.TotalSeconds;
dtcount += 1;
}
IMU = item;
}
break;
case "ATT":
ATT = item;
break;
}
}
var imu_dt = dtsum / dtcount;
Console.WriteLine("{0} samples at dt {1}", vel.Count, imu_dt);
var besti = -1;
var besterr = 0.0;
List <double> delays = new List <double>();
List <double> errors = new List <double>();
foreach (var i in Enumerable.Range(0, 100))
{
var err = velocity_error(timestamps, vel, gaccel, accel_indexes, imu_dt, i);
if (err == 0)
{
break;
}
errors.Add(err);
delays.Add(i * imu_dt);
if (besti == -1 || err < besterr)
{
besti = i;
besterr = err;
}
}
Console.WriteLine("Best {0} ({1}s) {2}", besti, besti * imu_dt, besterr);
}
