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