public LinkInterface()
{
// init fields
MAVlist = new MAVList(this);
this.BaseStream = new SerialPort();
this.packetcount = 0;
this._bytesReceivedSubj = new Subject <int>();
this._bytesSentSubj = new Subject <int>();
this.WhenPacketLost = new Subject <int>();
this.WhenPacketReceived = new Subject <int>();
this.readlock = new object();
//this.mavlinkversion = 0;
//this.debugmavlink = false;
this.logreadmode = false;
this.lastlogread = DateTime.MinValue;
this._logplaybackfile = null;
this.logfile = null;
this.rawlogfile = null;
this.connection = null;
}
public static void ProcessFile(string logfile)
{
if (File.Exists(logfile + ".jpg"))
{
return;
}
double minx = 99999;
double maxx = -99999;
double miny = 99999;
double maxy = -99999;
bool sitl = false;
Dictionary <int, List <PointLatLngAlt> > loc_list = new Dictionary <int, List <PointLatLngAlt> >();
try
{
if (logfile.ToLower().EndsWith(".tlog"))
{
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"))
{
using (
CollectionBuffer colbuf =
new CollectionBuffer(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.ReadWrite))
)
{
loc_list[0] = new List <PointLatLngAlt>();
foreach (var item in colbuf.GetEnumeratorType("GPS"))
{
if (item.msgtype.StartsWith("GPS"))
{
if (!colbuf.dflog.logformat.ContainsKey("GPS"))
{
continue;
}
var status =
double.Parse(item.items[colbuf.dflog.FindMessageOffset(item.msgtype, "Status")]);
var lat = double.Parse(item.items[colbuf.dflog.FindMessageOffset(item.msgtype, "Lat")]);
var lon = double.Parse(item.items[colbuf.dflog.FindMessageOffset(item.msgtype, "Lng")]);
if (lat == 0 || lon == 0 || status < 3)
{
continue;
}
loc_list[0].Add(new PointLatLngAlt(lat, lon));
minx = Math.Min(minx, lon);
maxx = Math.Max(maxx, lon);
miny = Math.Min(miny, lat);
maxy = Math.Max(maxy, lat);
}
}
}
}
if (loc_list.Count > 0 && loc_list.First().Value.Count > 10)
{
// add a bit of buffer
var area = RectLatLng.FromLTRB(minx - 0.001, maxy + 0.001, maxx + 0.001, miny - 0.001);
using (var map = GetMap(area))
using (var grap = Graphics.FromImage(map))
{
if (sitl)
{
AddTextToMap(grap, "SITL");
}
Color[] colours =
{
Color.Red, Color.Orange, Color.Yellow, Color.Green, Color.Blue, Color.Indigo,
Color.Violet, Color.Pink
};
int a = 0;
foreach (var locs in loc_list.Values)
{
PointF lastpoint = new PointF();
var pen = new Pen(colours[a % (colours.Length - 1)]);
foreach (var loc in locs)
{
PointF newpoint = GetPixel(area, loc, map.Size);
if (!lastpoint.IsEmpty)
{
grap.DrawLine(pen, lastpoint, newpoint);
}
lastpoint = newpoint;
}
a++;
}
map.Save(logfile + ".jpg", System.Drawing.Imaging.ImageFormat.Jpeg);
}
}
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");
}
return;
}
}
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;
}
}
}
