public static void SortLogs(string[] logs)
{
foreach (var logfile in logs)
{
FileInfo info = new FileInfo(logfile);
if (info.Length <= 1024)
{
try
{
string destdir = Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar
+ "SMALL" + Path.DirectorySeparatorChar;
if (!Directory.Exists(destdir))
Directory.CreateDirectory(destdir);
File.Move(logfile, destdir + Path.GetFileName(logfile));
File.Move(logfile.Replace(".tlog", ".rlog"), destdir + Path.GetFileName(logfile).Replace(".tlog", ".rlog"));
}
catch { }
continue;
}
MAVLinkInterface mine = new MAVLinkInterface();
try
{
using (mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read)))
{
mine.logreadmode = true;
byte[] hbpacket = mine.getHeartBeat();
if (hbpacket.Length == 0)
continue;
MAVLink.mavlink_heartbeat_t hb = (MAVLink.mavlink_heartbeat_t)mine.DebugPacket(hbpacket);
mine.logreadmode = false;
mine.logplaybackfile.Close();
string destdir = Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar
+ mine.MAV.aptype.ToString() + Path.DirectorySeparatorChar
+ hbpacket[3] + Path.DirectorySeparatorChar;
if (!Directory.Exists(destdir))
Directory.CreateDirectory(destdir);
File.Move(logfile, destdir + Path.GetFileName(logfile));
try
{
File.Move(logfile.Replace(".tlog", ".rlog"), destdir + Path.GetFileName(logfile).Replace(".tlog", ".rlog"));
}
catch { }
}
}
catch { continue; }
}
}
private void BUT_connect_Click(object sender, EventArgs e)
{
Comms.CommsSerialScan.Scan(false);
DateTime deadline = DateTime.Now.AddSeconds(50);
while (Comms.CommsSerialScan.foundport == false)
{
System.Threading.Thread.Sleep(100);
if (DateTime.Now > deadline)
{
CustomMessageBox.Show("Timeout waiting for autoscan/no mavlink device connected");
return;
}
}
MAVLinkInterface com2 = new MAVLinkInterface();
com2.BaseStream.PortName = Comms.CommsSerialScan.portinterface.PortName;
com2.BaseStream.BaudRate = Comms.CommsSerialScan.portinterface.BaudRate;
com2.Open(true);
MainV2.Comports.Add(com2);
bindingSource1.ResetBindings(false);
}
public OpticalFlow(MAVLinkInterface mav)
{
_mav = mav;
subDataTrans = mav.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.DATA_TRANSMISSION_HANDSHAKE, ReceviedPacket);
subEncapData = mav.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.ENCAPSULATED_DATA, ReceviedPacket);
}
public MAVLinkSerialPort(MAVLinkInterface mavint, MAVLink.SERIAL_CONTROL_DEV port)
{
this.mavint = mavint;
this.port = port;
if (!mavint.BaseStream.IsOpen)
{
mavint.BaseStream.Open();
}
if (mavint.getHeartBeat().Length == 0)
{
throw new Exception("No valid heartbeats read from port");
}
if (subscription.Value != null)
mavint.UnSubscribeToPacketType(subscription);
subscription = mavint.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.SERIAL_CONTROL, ReceviedPacket, true);
bgdata = new Thread(mainloop);
bgdata.Name = "MAVLinkSerialPort";
bgdata.IsBackground = true;
bgdata.Start();
}
public TerrainFollow(MAVLinkInterface inInterface)
{
_interface = inInterface;
log.Info("Subscribe to packets");
subscription = _interface.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.TERRAIN_REQUEST, ReceviedPacket);
}
public TerrainFollow(MAVLinkInterface inInterface)
{
_interface = inInterface;
log.Info("Subscribe to packets");
subscription = _interface.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.TERRAIN_DATA, ReceviedPacket);
mFlightData = FlightData.instance;
}
public static void doUpload(string file)
{
if (!validcred)
{
doUserAndPassword();
}
string droneshareusername = MainV2.getConfig("droneshareusername");
string dronesharepassword = MainV2.getConfig("dronesharepassword");
if (dronesharepassword != "")
{
try
{
// fail on bad entry
var crypto = new Crypto();
dronesharepassword = crypto.DecryptString(dronesharepassword);
}
catch { }
}
MAVLinkInterface mav = new MAVLinkInterface();
mav.BaseStream = new Comms.CommsFile();
mav.BaseStream.PortName = file;
mav.BaseStream.Open();
if (mav.getHeartBeat().Length == 0)
{
CustomMessageBox.Show("Invalid log");
return;
}
mav.Close();
string viewurl = Utilities.DroneApi.droneshare.doUpload(file, droneshareusername, dronesharepassword, mav.MAV.Guid , Utilities.DroneApi.APIConstants.apiKey);
if (viewurl != "")
{
try
{
validcred = true;
System.Diagnostics.Process.Start(viewurl);
}
catch (Exception ex) { log.Error(ex); CustomMessageBox.Show("Failed to open url " + viewurl); }
}
}
void processbg(string file)
{
if (!File.Exists(file + ".jpg"))
{
LogMap.MapLogs(new string[] {file});
}
var loginfo = new loginfo();
loginfo.fullname = file;
loginfo.Size = new FileInfo(file).Length;
if (File.Exists(file + ".jpg"))
{
loginfo.img = new Bitmap(file + ".jpg");
}
if (file.ToLower().EndsWith(".tlog"))
{
using (MAVLinkInterface mine = new MAVLinkInterface())
{
try
{
mine.logplaybackfile =
new BinaryReader(File.Open(file, FileMode.Open, FileAccess.Read, FileShare.Read));
}
catch (Exception ex)
{
log.Debug(ex.ToString());
CustomMessageBox.Show("Log Can not be opened. Are you still connected?");
return;
}
mine.logreadmode = true;
mine.getHeartBeat();
loginfo.Date = mine.lastlogread;
loginfo.Aircraft = mine.sysidcurrent;
var start = mine.lastlogread;
try
{
if (mine.logplaybackfile.BaseStream.Length > MaxPlaybackLogSize)
{
mine.logplaybackfile.BaseStream.Seek(-100000, SeekOrigin.End);
}
else
{
mine.logplaybackfile.BaseStream.Seek(0, SeekOrigin.Begin);
}
}
catch
{
}
var end = mine.lastlogread;
while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
{
mine.readPacket();
if (mine.lastlogread > end)
end = mine.lastlogread;
}
loginfo.Duration = (end - start).ToString();
}
}
logs.Add(loginfo);
}
private static void RequestCallback(IAsyncResult ar)
{
TcpClient client = (TcpClient)ar.AsyncState;
byte localsysid = newsysid++;
if (client.Connected)
{
MAVLinkInterface mav = new MAVLinkInterface();
mav.BaseStream = new TcpSerial() { client = client };
try
{
mav.GetParam("SYSID_THISMAV");
}
catch { }
var ans = mav.setParam("SYSID_THISMAV", localsysid);
Console.WriteLine("this mav set " + ans);
mav = null;
clients.Add(client);
}
}
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;
}
}
}
void dolog()
{
flightdata.Clear();
MAVLinkInterface mine = new MAVLinkInterface();
try
{
mine.logplaybackfile = new BinaryReader(File.Open(txt_tlog.Text, FileMode.Open, FileAccess.Read, FileShare.Read));
}
catch { CustomMessageBox.Show("Log Can not be opened. Are you still connected?"); return; }
mine.logreadmode = true;
mine.MAV.packets.Initialize(); // clear
mine.readPacket();
startlogtime = mine.lastlogread;
double oldlatlngsum = 0;
int appui = 0;
while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
{
byte[] packet = mine.readPacket();
cs.datetime = mine.lastlogread;
cs.UpdateCurrentSettings(null, true, mine);
if (appui != DateTime.Now.Second)
{
// cant do entire app as mixes with flightdata timer
this.Refresh();
appui = DateTime.Now.Second;
}
try
{
if (MainV2.speechEngine != null)
{
MainV2.speechEngine.SpeakAsyncCancelAll();
}
}
catch { } // ignore because of this Exception System.PlatformNotSupportedException: No voice installed on the system or none available with the current security setting.
// if ((float)(cs.lat + cs.lng + cs.alt) != oldlatlngsum
// && cs.lat != 0 && cs.lng != 0)
DateTime nexttime = mine.lastlogread.AddMilliseconds(-(mine.lastlogread.Millisecond % 100));
if (!flightdata.ContainsKey(nexttime))
{
Console.WriteLine(cs.lat + " " + cs.lng + " " + cs.alt + " lah " + (float)(cs.lat + cs.lng + cs.alt) + "!=" + oldlatlngsum);
CurrentState cs2 = (CurrentState)cs.Clone();
try
{
flightdata.Add(nexttime, cs2);
}
catch { }
oldlatlngsum = (cs.lat + cs.lng + cs.alt);
}
}
mine.logreadmode = false;
mine.logplaybackfile.Close();
mine.logplaybackfile = null;
}
public void DownloadFence(MAVLinkInterface port, Action <int, string> progress = null)
{
var list = mav_mission.download(port, MAVLink.MAV_MISSION_TYPE.FENCE, progress);
LocationToFence(list);
}
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
{
var map = new Bitmap(100, 100);
var grap = Graphics.FromImage(map);
grap.DrawString("No gps data", SystemFonts.DefaultFont, Brushes.Red, 0, 0, StringFormat.GenericDefault);
map.Save(logfile + ".jpg", System.Drawing.Imaging.ImageFormat.Jpeg);
map.Dispose();
map = null;
}
}
catch { continue; }
}
}
public static void SortLogs(string[] logs, string masterdestdir = "")
{
Parallel.ForEach(logs, logfile =>
//foreach (var logfile in logs)
{
if (masterdestdir == "")
{
masterdestdir = Path.GetDirectoryName(logfile);
}
issitl = false;
FileInfo info = new FileInfo(logfile);
// delete 0 size files
if (info.Length == 0)
{
try
{
File.Delete(logfile);
}
catch
{
}
return;
}
// move small logs - most likerly invalid
if (info.Length <= 1024)
{
try
{
string destdir = masterdestdir + Path.DirectorySeparatorChar
+ "SMALL" + Path.DirectorySeparatorChar;
if (!Directory.Exists(destdir))
{
Directory.CreateDirectory(destdir);
}
log.Info("Move log small " + logfile + " to " + destdir + Path.GetFileName(logfile));
MoveFileUsingMask(logfile, destdir);
}
catch
{
}
return;
}
try
{
using (MAVLinkInterface mine = new MAVLinkInterface())
using (
mine.logplaybackfile =
new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read)))
{
mine.logreadmode = true;
mine.speechenabled = false;
var midpoint = mine.logplaybackfile.BaseStream.Length / 2;
mine.logplaybackfile.BaseStream.Seek(midpoint, SeekOrigin.Begin);
// used for sitl detection
mine.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.SIMSTATE, SitlDetection);
mine.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.SIM_STATE, SitlDetection);
MAVLink.MAVLinkMessage hbpacket = mine.getHeartBeat();
MAVLink.MAVLinkMessage hbpacket1 = mine.getHeartBeat();
MAVLink.MAVLinkMessage hbpacket2 = mine.getHeartBeat();
MAVLink.MAVLinkMessage hbpacket3 = mine.getHeartBeat();
if (hbpacket.Length == 0 && hbpacket1.Length == 0 && hbpacket2.Length == 0 &&
hbpacket3.Length == 0)
{
mine.logreadmode = false;
mine.logplaybackfile.Close();
if (!Directory.Exists(masterdestdir + Path.DirectorySeparatorChar + "BAD"))
{
Directory.CreateDirectory(masterdestdir + Path.DirectorySeparatorChar +
"BAD");
}
log.Info("Move log bad " + logfile + " to " + masterdestdir +
Path.DirectorySeparatorChar + "BAD" + Path.DirectorySeparatorChar +
Path.GetFileName(logfile));
MoveFileUsingMask(logfile,
masterdestdir + Path.DirectorySeparatorChar + "BAD" +
Path.DirectorySeparatorChar);
return;
}
if (hbpacket.Length != 0)
{
MAVLink.mavlink_heartbeat_t hb = (MAVLink.mavlink_heartbeat_t)mine.DebugPacket(hbpacket);
}
if (hbpacket1.Length != 0)
{
MAVLink.mavlink_heartbeat_t hb1 = (MAVLink.mavlink_heartbeat_t)mine.DebugPacket(hbpacket1);
}
if (hbpacket2.Length != 0)
{
MAVLink.mavlink_heartbeat_t hb2 = (MAVLink.mavlink_heartbeat_t)mine.DebugPacket(hbpacket2);
}
if (hbpacket3.Length != 0)
{
MAVLink.mavlink_heartbeat_t hb3 = (MAVLink.mavlink_heartbeat_t)mine.DebugPacket(hbpacket3);
}
// find most appropriate
if (mine.MAVlist.Count > 1)
{
foreach (var mav in mine.MAVlist)
{
if (mav.aptype == MAVLink.MAV_TYPE.ANTENNA_TRACKER)
{
continue;
}
if (mav.aptype == MAVLink.MAV_TYPE.GCS)
{
continue;
}
mine.sysidcurrent = mav.sysid;
mine.compidcurrent = mav.compid;
}
}
mine.logreadmode = false;
mine.logplaybackfile.Close();
string destdir = masterdestdir + Path.DirectorySeparatorChar
+ mine.MAV.aptype.ToString() + Path.DirectorySeparatorChar
+ mine.MAV.sysid + Path.DirectorySeparatorChar;
if (issitl)
{
destdir = masterdestdir + Path.DirectorySeparatorChar
+ "SITL" + Path.DirectorySeparatorChar
+ mine.MAV.aptype.ToString() + Path.DirectorySeparatorChar
+ mine.MAV.sysid + Path.DirectorySeparatorChar;
}
if (!Directory.Exists(destdir))
{
Directory.CreateDirectory(destdir);
}
MoveFileUsingMask(logfile, destdir);
}
}
catch
{
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;
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)
{
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.ByteArrayToStructure <MAVLink.mavlink_global_position_int_t>();
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;
}
}
}
public static void doUpload(string file)
{
doUserAndPassword();
string droneshareusername = MainV2.getConfig("droneshareusername");
string dronesharepassword = MainV2.getConfig("dronesharepassword");
if (dronesharepassword != "")
{
try
{
// fail on bad entry
var crypto = new Crypto();
dronesharepassword = crypto.DecryptString(dronesharepassword);
}
catch { }
}
MAVLinkInterface mav = new MAVLinkInterface();
mav.BaseStream = new Comms.CommsFile();
mav.BaseStream.PortName = file;
mav.getHeartBeat();
mav.Close();
string guid = Guid.NewGuid().ToString();
switch (mav.MAV.cs.firmware)
{
case MainV2.Firmwares.ArduCopter2:
guid = MainV2.config["copter_guid"].ToString();
break;
case MainV2.Firmwares.ArduPlane:
guid = MainV2.config["plane_guid"].ToString();
break;
case MainV2.Firmwares.ArduRover:
guid = MainV2.config["rover_guid"].ToString();
break;
}
string viewurl = Utilities.droneshare.doUpload(file, droneshareusername, dronesharepassword, guid, Utilities.droneshare.APIConstants.apiKey);
if (viewurl != "")
{
try
{
System.Diagnostics.Process.Start(viewurl);
}
catch (Exception ex) { CustomMessageBox.Show("Failed to open url "+ viewurl); }
}
}
/// <summary>
/// Processes a tlog to get the offsets - creates dxf of data
/// </summary>
/// <param name="fn">Filename</param>
/// <returns>Offsets</returns>
public static double[] getOffsets(string fn, int throttleThreshold = 0)
{
// based off tridge's work
string logfile = fn;
// old method
float minx = 0;
float maxx = 0;
float miny = 0;
float maxy = 0;
float minz = 0;
float maxz = 0;
// this is for a dxf
Polyline3dVertex vertex;
List <Polyline3dVertex> vertexes = new List <Polyline3dVertex>();
// data storage
Tuple <float, float, float> offset = new Tuple <float, float, float>(0, 0, 0);
List <Tuple <float, float, float> > data = new List <Tuple <float, float, float> >();
Hashtable filter = new Hashtable();
// track data to use
bool useData = false;
if (throttleThreshold <= 0)
{
useData = true;
}
log.Info("Start log: " + DateTime.Now);
MAVLinkInterface mine = new MAVLinkInterface();
try
{
mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read));
}
catch (Exception ex) { log.Debug(ex.ToString()); CustomMessageBox.Show("Log Can not be opened. Are you still connected?"); return(new double[] { 0 }); }
mine.logreadmode = true;
mine.MAV.packets.Initialize(); // clear
// gather data
while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
{
byte[] packetraw = mine.readPacket();
var packet = mine.DebugPacket(packetraw, false);
// this is for packets we dont know about
if (packet == null)
{
continue;
}
if (packet.GetType() == typeof(MAVLink.mavlink_vfr_hud_t))
{
if (((MAVLink.mavlink_vfr_hud_t)packet).throttle >= throttleThreshold)
{
useData = true;
}
else
{
useData = false;
}
}
if (packet.GetType() == typeof(MAVLink.mavlink_sensor_offsets_t))
{
offset = new Tuple <float, float, float>(
((MAVLink.mavlink_sensor_offsets_t)packet).mag_ofs_x,
((MAVLink.mavlink_sensor_offsets_t)packet).mag_ofs_y,
((MAVLink.mavlink_sensor_offsets_t)packet).mag_ofs_z);
}
else if (packet.GetType() == typeof(MAVLink.mavlink_raw_imu_t) && useData)
{
int div = 20;
// fox dxf
vertex = new Polyline3dVertex(new Vector3f(
((MAVLink.mavlink_raw_imu_t)packet).xmag - offset.Item1,
((MAVLink.mavlink_raw_imu_t)packet).ymag - offset.Item2,
((MAVLink.mavlink_raw_imu_t)packet).zmag - offset.Item3)
);
vertexes.Add(vertex);
// for old method
setMinorMax(((MAVLink.mavlink_raw_imu_t)packet).xmag - offset.Item1, ref minx, ref maxx);
setMinorMax(((MAVLink.mavlink_raw_imu_t)packet).ymag - offset.Item2, ref miny, ref maxy);
setMinorMax(((MAVLink.mavlink_raw_imu_t)packet).zmag - offset.Item3, ref minz, ref maxz);
// for new lease sq
string item = (int)(((MAVLink.mavlink_raw_imu_t)packet).xmag / div) + "," +
(int)(((MAVLink.mavlink_raw_imu_t)packet).ymag / div) + "," +
(int)(((MAVLink.mavlink_raw_imu_t)packet).zmag / div);
if (filter.ContainsKey(item))
{
filter[item] = (int)filter[item] + 1;
if ((int)filter[item] > 3)
{
continue;
}
}
else
{
filter[item] = 1;
}
data.Add(new Tuple <float, float, float>(
((MAVLink.mavlink_raw_imu_t)packet).xmag - offset.Item1,
((MAVLink.mavlink_raw_imu_t)packet).ymag - offset.Item2,
((MAVLink.mavlink_raw_imu_t)packet).zmag - offset.Item3));
}
}
log.Info("Log Processed " + DateTime.Now);
Console.WriteLine("Extracted " + data.Count + " data points");
Console.WriteLine("Current offset: " + offset);
mine.logreadmode = false;
mine.logplaybackfile.Close();
mine.logplaybackfile = null;
if (data.Count < 10)
{
CustomMessageBox.Show("Log does not contain enough data");
throw new Exception("Not Enough Data");
}
data.Sort(
delegate(Tuple <float, float, float> d1, Tuple <float, float, float> d2)
{
// get distance from 0,0,0
double ans1 = Math.Sqrt(d1.Item1 * d1.Item1 + d1.Item2 * d1.Item2 + d1.Item3 * d1.Item3);
double ans2 = Math.Sqrt(d2.Item1 * d2.Item1 + d2.Item2 * d2.Item2 + d2.Item3 * d2.Item3);
if (ans1 > ans2)
{
return(1);
}
if (ans1 < ans2)
{
return(-1);
}
return(0);
}
);
data.RemoveRange(data.Count - (data.Count / 16), data.Count / 16);
System.Console.WriteLine("Old Method {0} {1} {2}", -(maxx + minx) / 2, -(maxy + miny) / 2, -(maxz + minz) / 2);
double[] x = LeastSq(data);
log.Info("Least Sq Done " + DateTime.Now);
doDXF(vertexes, x);
Array.Resize <double>(ref x, 3);
return(x);
}
private void start_Terminal(bool px4)
{
setcomport();
try
{
if (MainV2.comPort != null && MainV2.comPort.BaseStream != null && MainV2.comPort.BaseStream.IsOpen)
{
MainV2.comPort.BaseStream.Close();
}
if (comPort.IsOpen)
{
Console.WriteLine("Terminal Start - Close Port");
threadrun = false;
// if (DialogResult.Cancel == CustomMessageBox.Show("The port is open\n Continue?", "Continue", MessageBoxButtons.YesNo))
{
// return;
}
comPort.Close();
// allow things to cleanup
Thread.Sleep(400);
}
comPort.ReadBufferSize = 1024 * 1024 * 4;
comPort.PortName = MainV2.comPortName;
// test moving baud rate line
comPort.BaudRate = int.Parse(MainV2._connectionControl.CMB_baudrate.Text);
if (px4)
{
TXT_terminal.AppendText("Rebooting " + MainV2.comPortName + " at " + comPort.BaudRate + "\n");
// keep it local
using (var mine = new MAVLinkInterface())
{
mine.BaseStream.PortName = MainV2.comPortName;
mine.BaseStream.BaudRate = comPort.BaudRate;
mine.giveComport = true;
mine.BaseStream.Open();
// check if we are a mavlink stream
var buffer = mine.readPacket();
if (buffer.Length > 0)
{
log.Info("got packet - sending reboot via mavlink");
TXT_terminal.AppendText("Via Mavlink\n");
mine.doReboot(false, false);
try
{
mine.BaseStream.Close();
}
catch
{
}
}
else
{
log.Info("no packet - sending reboot via console");
TXT_terminal.AppendText("Via Console\n");
try
{
mine.BaseStream.Write("reboot\r");
mine.BaseStream.Write("exit\rreboot\r");
}
catch
{
}
try
{
mine.BaseStream.Close();
}
catch
{
}
}
}
TXT_terminal.AppendText("Waiting for reboot\n");
// wait 7 seconds for px4 reboot
log.Info("waiting for reboot");
var deadline = DateTime.Now.AddSeconds(9);
while (DateTime.Now < deadline)
{
Thread.Sleep(500);
Application.DoEvents();
}
var a = 0;
while (a < 5)
{
try
{
if (!comPort.IsOpen)
{
comPort.Open();
}
}
catch
{
}
Thread.Sleep(200);
a++;
}
}
else
{
log.Info("About to open " + comPort.PortName);
comPort.Open();
log.Info("toggle dtr");
comPort.toggleDTR();
}
try
{
comPort.DiscardInBuffer();
}
catch
{
}
startreadthread();
}
catch (Exception ex)
{
log.Error(ex);
TXT_terminal.AppendText("Cant open serial port\r\n");
return;
}
}
/// <summary>
/// main serial reader thread
/// controls
/// serial reading
/// link quality stats
/// speech voltage - custom - alt warning - data lost
/// heartbeat packet sending
///
/// and can't fall out
/// </summary>
private void SerialReader()
{
if (serialThread == true)
{
return;
}
serialThread = true;
SerialThreadrunner.Reset();
int minbytes = 0;
int altwarningmax = 0;
bool armedstatus = false;
string lastmessagehigh = "";
DateTime speechcustomtime = DateTime.Now;
DateTime speechbatterytime = DateTime.Now;
DateTime speechlowspeedtime = DateTime.Now;
DateTime linkqualitytime = DateTime.Now;
while (serialThread)
{
try
{
Thread.Sleep(1); // was 5
//// get home point on armed status change.
//if (armedstatus != Form1.comPort.MAV.cs.armed && comPort.BaseStream.IsOpen)
//{
// armedstatus = Form1.comPort.MAV.cs.armed;
// // status just changed to armed
// if (Form1.comPort.MAV.cs.armed == true)
// {
// try
// {
// //Form1.comPort.MAV.cs.HomeLocation = new PointLatLngAlt(Form1.comPort.getWP(0));
// //if (MyView.current != null && MyView.current.Name == "FlightPlanner")
// //{
// // // update home if we are on flight data tab
// // FlightPlanner.updateHome();
// //}
// }
// catch
// {
// // dont hang this loop
// this.BeginInvoke((MethodInvoker)delegate { MessageBox.Show("Failed to update home location"); });
// }
// }
//}
// send a hb every seconds from gcs to ap
if (heatbeatSend.Second != DateTime.Now.Second)
{
MAVLink.mavlink_heartbeat_t htb = new MAVLink.mavlink_heartbeat_t()
{
type = (byte)MAVLink.MAV_TYPE.GCS,
autopilot = (byte)MAVLink.MAV_AUTOPILOT.INVALID,
mavlink_version = 3,
};
foreach (var port in Form1.Comports)
{
try
{
port.sendPacket(htb);
}
catch { }
}
heatbeatSend = DateTime.Now;
}
// if not connected or busy, sleep and loop
if (!comPort.BaseStream.IsOpen || comPort.giveComport == true)
{
if (!comPort.BaseStream.IsOpen)
{
// check if other ports are still open
foreach (var port in Comports)
{
if (port.BaseStream.IsOpen)
{
Console.WriteLine("Main comport shut, swapping to other mav");
comPort = port;
break;
}
}
}
System.Threading.Thread.Sleep(100);
//continue;
}
// actualy read the packets
while (comPort.BaseStream.IsOpen && comPort.BaseStream.BytesToRead > minbytes && comPort.giveComport == false)
{
try
{
comPort.readPacket();
}
catch { }
}
// update currentstate of sysids on main port
foreach (var sysid in comPort.sysidseen)
{
try
{
comPort.MAVlist[sysid].cs.UpdateCurrentSettings(null, false, comPort, comPort.MAVlist[sysid]);
}
catch { }
}
//C_Log.writeLog(comPort.MAV.cs.pitch + " " + comPort.MAV.cs.roll + " " + comPort.MAV.cs.yaw + "\n\r");
this.Invoke((MethodInvoker) delegate
{
//update teks
//lbl_test.Visible = true;
//lbl_test.Text = "YAW :" + comPort.MAV.cs.yaw.ToString() + "\nAltitude :" + comPort.MAV.cs.alt.ToString();
//lbl_test.Text = "YAW :" + loop1.ToString() + "\nAltitude :" + loop2.ToString();
label1.Text = comPort.MAV.cs.yaw.ToString();
label2.Text = comPort.MAV.cs.pitch.ToString();
label3.Text = comPort.MAV.cs.roll.ToString();
});
////Ngirim Data
//if (USock != null)
//{
// if (USock.isConnected())
// {
// this.Invoke(sdtc);
// }
//}
//// read the other interfaces
//foreach (var port in Comports)
//{
// // skip primary interface
// if (port == comPort)
// continue;
// if (!port.BaseStream.IsOpen)
// {
// // modify array and drop out
// Comports.Remove(port);
// break;
// }
// while (port.BaseStream.IsOpen && port.BaseStream.BytesToRead > minbytes)
// {
// try
// {
// port.readPacket();
// }
// catch { }
// }
// // update currentstate of sysids on the port
// foreach (var sysid in port.sysidseen)
// {
// try
// {
// port.MAVlist[sysid].cs.UpdateCurrentSettings(null, false, port, port.MAVlist[sysid]);
// }
// catch { }
// }
//}
//UPDATE JOYSTICK OUTPUT FROM SERIAL
//updateProgressJoystickInputSERIAL();
//COMPARE DELAY
//JoystickDelayCalculation();
}
catch (Exception e)
{
log.Error("Serial Reader fail :" + e.ToString());
MessageBox.Show(e.ToString());
try
{
comPort.Close();
}
catch { }
}
}
Console.WriteLine("SerialReader Done");
SerialThreadrunner.Set();
}
public static void MapLogs(string[] logs)
{
foreach (var logfile in logs)
{
if (File.Exists(logfile + ".jpg") || logfile.Contains("Telemetry Logs"))
continue;
MAVLinkInterface mine = new MAVLinkInterface();
List<MAVLink.mavlink_global_position_int_t> locs = new List<MAVLink.mavlink_global_position_int_t>();
try
{
double minx = 99999;
double maxx = -99999;
double miny = 99999;
double maxy = -99999;
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();
//Console.Write((mine.logplaybackfile.BaseStream.Position / (double)mine.logplaybackfile.BaseStream.Length) +"\r");
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(loc);
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);
}
}
}
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
{
var map = new Bitmap(100, 100);
var grap = Graphics.FromImage(map);
grap.DrawString("NO MAP", SystemFonts.DefaultFont, Brushes.Red, 0, 0, StringFormat.GenericDefault);
map.Save(logfile + ".jpg", System.Drawing.Imaging.ImageFormat.Jpeg);
map.Dispose();
map = null;
}
}
catch { continue; }
}
}
public static PointLatLngAlt ProjectPoint(MAVLinkInterface comPort)
{
GimbalPoint.comPort = comPort;
comPort.GetMountStatus();
// this should be looking at rc_channel function
yawchannel = (int)(float)comPort.MAV.param["MNT_RC_IN_PAN"];
pitchchannel = (int)(float)comPort.MAV.param["MNT_RC_IN_TILT"];
rollchannel = (int)(float)comPort.MAV.param["MNT_RC_IN_ROLL"];
//if (!comPort.BaseStream.IsOpen)
// return PointLatLngAlt.Zero;
PointLatLngAlt currentlocation = new PointLatLngAlt(comPort.MAV.cs.lat, comPort.MAV.cs.lng);
double yawangle = comPort.MAV.cs.campointc;
double rollangle = comPort.MAV.cs.campointb;
double pitchangle = comPort.MAV.cs.campointa;
//
if ((double)comPort.MAV.param["MNT_TYPE"] == 4)
{
yawangle = comPort.MAVlist[comPort.sysidcurrent, 67].cs.yaw;
rollangle = comPort.MAVlist[comPort.sysidcurrent, 67].cs.roll;
pitchangle = comPort.MAVlist[comPort.sysidcurrent, 67].cs.pitch;
}
if (Math.Abs(rollangle) > 180 || yawangle == 0 && pitchangle == 0)
{
yawangle = ConvertPwmtoAngle(axis.yaw);
//rollangle = ConvertPwmtoAngle(axis.roll);
pitchangle = ConvertPwmtoAngle(axis.pitch) + comPort.MAV.cs.pitch;
pitchangle -= Math.Sin(yawangle * MathHelper.deg2rad) * comPort.MAV.cs.roll;
}
// tan (o/a)
// todo account for ground elevation change.
int distout = 10;
PointLatLngAlt newpos = PointLatLngAlt.Zero;
//dist = Math.Tan((90 + pitchangle) * MathHelper.deg2rad) * (comPort.MAV.cs.alt);
while (distout < 1000)
{
// get a projected point to test intersection against - not using slope distance
PointLatLngAlt newposdist = currentlocation.newpos(yawangle + comPort.MAV.cs.yaw, distout);
newposdist.Alt = srtm.getAltitude(newposdist.Lat, newposdist.Lng).alt;
// get another point 50 infront
PointLatLngAlt newposdist2 = currentlocation.newpos(yawangle + comPort.MAV.cs.yaw, distout + 50);
newposdist2.Alt = srtm.getAltitude(newposdist2.Lat, newposdist2.Lng).alt;
// get the flat terrain distance out - at 0 alt
double distflat = Math.Tan((90 + pitchangle) * MathHelper.deg2rad) * (comPort.MAV.cs.altasl);
// x is dist from plane, y is alt
var newpoint = FindLineIntersection(start1: new Point(0, comPort.MAV.cs.altasl),
new Point((float)distflat, 0),
new Point((float)distout, (float)newposdist.Alt),
new Point((float)distout + 50, (float)newposdist2.Alt));
if (newpoint.X != 0)
{
newpos = newposdist2;
break;
}
distout += 50;
}
//Console.WriteLine("pitch " + pitchangle.ToString("0.000") + " yaw " + yawangle.ToString("0.000") + " dist" + dist.ToString("0.000"));
//PointLatLngAlt newpos = currentlocation.newpos( yawangle + comPort.MAV.cs.yaw, dist);
//Console.WriteLine(newpos);
return(newpos);
}
static string process(MAVLinkInterface mavint)
{
DateTime Deadline = DateTime.Now.AddSeconds(60);
Vector3 last_mag = null;
double last_usec = 0;
double count = 0;
double[] total_error = new double[rotations.Length];
float AHRS_ORIENTATION = 0;
float COMPASS_ORIENT = 0;
float COMPASS_EXTERNAL = 0;
//# now gather all the data
while (DateTime.Now < Deadline || mavint.BaseStream.BytesToRead > 0)
{
byte[] packetbytes = mavint.readPacket();
if (packetbytes.Length < 5)
{
continue;
}
object packet = mavint.GetPacket(packetbytes);
if (packet == null)
{
continue;
}
if (packet is MAVLink.mavlink_param_value_t)
{
MAVLink.mavlink_param_value_t m = (MAVLink.mavlink_param_value_t)packet;
if (str(m.param_id) == "AHRS_ORIENTATION")
{
AHRS_ORIENTATION = (int)(m.param_value);
}
if (str(m.param_id) == "COMPASS_ORIENT")
{
COMPASS_ORIENT = (int)(m.param_value);
}
if (str(m.param_id) == "COMPASS_EXTERNAL")
{
COMPASS_EXTERNAL = (int)(m.param_value);
}
}
if (packet is MAVLink.mavlink_raw_imu_t)
{
MAVLink.mavlink_raw_imu_t m = (MAVLink.mavlink_raw_imu_t)packet;
Vector3 mag = new Vector3(m.xmag, m.ymag, m.zmag);
mag = mag_fixup(mag, AHRS_ORIENTATION, COMPASS_ORIENT, COMPASS_EXTERNAL);
Vector3 gyr = new Vector3(m.xgyro, m.ygyro, m.zgyro) * 0.001;
double usec = m.time_usec;
if (last_mag != null && gyr.length() > radians(5.0))
{
add_errors(mag, gyr, last_mag, (usec - last_usec) * 1.0e-6, total_error, rotations);
count += 1;
}
last_mag = mag;
last_usec = usec;
}
}
int best_i = 0;
double best_err = total_error[0];
foreach (var i in range(len(rotations)))
{
Rotation r = rotations[i];
// if (!r.is_90_degrees())
// continue;
//if ( opts.verbose) {
// print("%s err=%.2f" % (r, total_error[i]/count));}
if (total_error[i] < best_err)
{
best_i = i;
best_err = total_error[i];
}
}
Rotation rans = rotations[best_i];
Console.WriteLine("Best rotation is {0} err={1} from {2} points", rans, best_err / count, count);
//print("Best rotation is %s err=%.2f from %u points" % (r, best_err/count, count));
return(rans.name);
}
private void start_Terminal(bool px4)
{
setcomport();
try
{
if (MainV2.comPort != null && MainV2.comPort.BaseStream != null && MainV2.comPort.BaseStream.IsOpen)
MainV2.comPort.BaseStream.Close();
if (comPort.IsOpen)
{
Console.WriteLine("Terminal Start - Close Port");
threadrun = false;
// if (DialogResult.Cancel == CustomMessageBox.Show("The port is open\n Continue?", "Continue", MessageBoxButtons.YesNo))
{
// return;
}
comPort.Close();
// allow things to cleanup
Thread.Sleep(400);
}
comPort.ReadBufferSize = 1024*1024*4;
comPort.PortName = MainV2.comPortName;
// test moving baud rate line
comPort.BaudRate = int.Parse(MainV2._connectionControl.CMB_baudrate.Text);
if (px4)
{
TXT_terminal.AppendText("Rebooting " + MainV2.comPortName + " at " + comPort.BaudRate + "\n");
// keep it local
using (var mine = new MAVLinkInterface())
{
mine.BaseStream.PortName = MainV2.comPortName;
mine.BaseStream.BaudRate = comPort.BaudRate;
mine.giveComport = true;
mine.BaseStream.Open();
// check if we are a mavlink stream
var buffer = mine.readPacket();
if (buffer.Length > 0)
{
log.Info("got packet - sending reboot via mavlink");
TXT_terminal.AppendText("Via Mavlink\n");
mine.doReboot(false);
try
{
mine.BaseStream.Close();
}
catch
{
}
}
else
{
log.Info("no packet - sending reboot via console");
TXT_terminal.AppendText("Via Console\n");
try
{
mine.BaseStream.Write("reboot\r");
mine.BaseStream.Write("exit\rreboot\r");
}
catch
{
}
try
{
mine.BaseStream.Close();
}
catch
{
}
}
}
TXT_terminal.AppendText("Waiting for reboot\n");
// wait 7 seconds for px4 reboot
log.Info("waiting for reboot");
var deadline = DateTime.Now.AddSeconds(9);
while (DateTime.Now < deadline)
{
Thread.Sleep(500);
Application.DoEvents();
}
var a = 0;
while (a < 5)
{
try
{
if (!comPort.IsOpen)
comPort.Open();
}
catch
{
}
Thread.Sleep(200);
a++;
}
}
else
{
log.Info("About to open " + comPort.PortName);
comPort.Open();
log.Info("toggle dtr");
comPort.toggleDTR();
}
try
{
comPort.DiscardInBuffer();
}
catch
{
}
startreadthread();
}
catch (Exception ex)
{
log.Error(ex);
TXT_terminal.AppendText("Cant open serial port\r\n");
return;
}
}
public async Task StartSwarmSeperate()
{
var max = 10;
if (InputBox.Show("how many?", "how many?", ref max) != DialogResult.OK)
{
return;
}
// kill old session
try
{
simulator.ForEach(a =>
{
try
{
a.Kill();
}
catch { }
});
}
catch
{
}
var exepath = CheckandGetSITLImage("ArduCopter.elf");
var model = "+";
var config = await GetDefaultConfig(model);
max--;
for (int a = (int)max; a >= 0; a--)
{
var extra = " --disable-fgview -r50 ";
if (!string.IsNullOrEmpty(config))
{
extra += @" --defaults """ + config + @",identity.parm"" -P SERIAL0_PROTOCOL=2 -P SERIAL1_PROTOCOL=2 ";
}
var home = new PointLatLngAlt(markeroverlay.Markers[0].Position).newpos((double)NUM_heading.Value, a * 4);
if (max == a)
{
extra += String.Format(
" -M{4} -s1 --home {3} --instance {0} --uartA tcp:0 {1} -P SYSID_THISMAV={2} ",
a, "", a + 1, BuildHomeLocation(home, (int)NUM_heading.Value), model);
}
else
{
extra += String.Format(
" -M{4} -s1 --home {3} --instance {0} --uartA tcp:0 {1} -P SYSID_THISMAV={2} ",
a, "" /*"--uartD tcpclient:127.0.0.1:" + (5770 + 10 * a)*/, a + 1,
BuildHomeLocation(home, (int)NUM_heading.Value), model);
}
string simdir = sitldirectory + model + (a + 1) + Path.DirectorySeparatorChar;
Directory.CreateDirectory(simdir);
File.WriteAllText(simdir + "identity.parm", String.Format(@"SERIAL0_PROTOCOL=2
SERIAL1_PROTOCOL=2
SYSID_THISMAV={0}
SIM_TERRAIN=0
TERRAIN_ENABLE=0
SCHED_LOOP_RATE=50
SIM_RATE_HZ=400
SIM_DRIFT_SPEED=0
SIM_DRIFT_TIME=0
", a + 1));
string path = Environment.GetEnvironmentVariable("PATH");
Environment.SetEnvironmentVariable("PATH", sitldirectory + ";" + simdir + ";" + path,
EnvironmentVariableTarget.Process);
Environment.SetEnvironmentVariable("HOME", simdir, EnvironmentVariableTarget.Process);
ProcessStartInfo exestart = new ProcessStartInfo();
exestart.FileName = await exepath;
exestart.Arguments = extra;
exestart.WorkingDirectory = simdir;
exestart.WindowStyle = ProcessWindowStyle.Minimized;
exestart.UseShellExecute = true;
simulator.Add(System.Diagnostics.Process.Start(exestart));
}
System.Threading.Thread.Sleep(2000);
MainV2.View.ShowScreen(MainV2.View.screens[0].Name);
try
{
for (int a = (int)max; a >= 0; a--)
{
var mav = new MAVLinkInterface();
var client = new Comms.TcpSerial();
client.client = new TcpClient("127.0.0.1", 5760 + (10 * (a)));
mav.BaseStream = client;
//SITLSEND = new UdpClient("127.0.0.1", 5501);
Thread.Sleep(200);
MainV2.instance.doConnect(mav, "preset", "5760", false);
MainV2.Comports.Add(mav);
}
return;
}
catch
{
CustomMessageBox.Show(Strings.Failed_to_connect_to_SITL_instance, Strings.ERROR);
return;
}
}
public async void StartSwarmSeperate()
{
var exepath = CheckandGetSITLImage("ArduCopter.elf");
var model = "+";
var config = await GetDefaultConfig(model);
var max = 10.0;
if (InputBox.Show("how many?", "how many?", ref max) != DialogResult.OK)
{
return;
}
max--;
for (int a = (int)max; a >= 0; a--)
{
var extra = " --disable-fgview -r50 ";
if (!string.IsNullOrEmpty(config))
{
extra += @" --defaults """ + config + @""" -P SERIAL0_PROTOCOL=2 -P SERIAL1_PROTOCOL=2 ";
}
var home = new PointLatLngAlt(markeroverlay.Markers[0].Position).newpos((double)NUM_heading.Value, a * 4);
if (max == a)
{
extra += String.Format(
" -M{4} -s1 --home {3} --instance {0} --uartA tcp:0 {1} -P SYSID_THISMAV={2} ",
a, "", a + 1, BuildHomeLocation(home, (int)NUM_heading.Value), model);
}
else
{
extra += String.Format(
" -M{4} -s1 --home {3} --instance {0} --uartA tcp:0 {1} -P SYSID_THISMAV={2} ",
a, "" /*"--uartD tcpclient:127.0.0.1:" + (5770 + 10 * a)*/, a + 1,
BuildHomeLocation(home, (int)NUM_heading.Value), model);
}
string simdir = sitldirectory + model + (a + 1) + Path.DirectorySeparatorChar;
Directory.CreateDirectory(simdir);
string path = Environment.GetEnvironmentVariable("PATH");
Environment.SetEnvironmentVariable("PATH", sitldirectory + ";" + simdir + ";" + path,
EnvironmentVariableTarget.Process);
Environment.SetEnvironmentVariable("HOME", simdir, EnvironmentVariableTarget.Process);
ProcessStartInfo exestart = new ProcessStartInfo();
exestart.FileName = await exepath;
exestart.Arguments = extra;
exestart.WorkingDirectory = simdir;
exestart.WindowStyle = ProcessWindowStyle.Minimized;
exestart.UseShellExecute = true;
Process.Start(exestart);
}
try
{
for (int a = (int)max; a >= 0; a--)
{
var mav = new MAVLinkInterface();
var client = new Comms.TcpSerial();
client.client = new TcpClient("127.0.0.1", 5760 + (10 * (a)));
mav.BaseStream = client;
//SITLSEND = new UdpClient("127.0.0.1", 5501);
Thread.Sleep(200);
MainV2.instance.doConnect(mav, "preset", "5760");
try
{
mav.GetParam("SYSID_THISMAV");
mav.setParam("SYSID_THISMAV", a + 1, true);
mav.GetParam("FRAME_CLASS");
mav.setParam("FRAME_CLASS", 1, true);
}
catch
{
}
MainV2.Comports.Add(mav);
}
return;
}
catch
{
CustomMessageBox.Show(Strings.Failed_to_connect_to_SITL_instance, Strings.ERROR);
return;
}
}
private void start_Terminal(bool px4)
{
setcomport();
try
{
if (MainV2.comPort != null && MainV2.comPort.BaseStream != null && MainV2.comPort.BaseStream.IsOpen)
{
MainV2.comPort.BaseStream.Close();
}
if (comPort.IsOpen)
{
Console.WriteLine("Terminal Start - Close Port");
threadrun = false;
// if (DialogResult.Cancel == CustomMessageBox.Show("The port is open\n Continue?", "Continue", MessageBoxButtons.YesNo))
{
// return;
}
comPort.Close();
// allow things to cleanup
System.Threading.Thread.Sleep(400);
}
comPort.ReadBufferSize = 1024 * 1024 * 4;
comPort.PortName = MainV2.comPortName;
// test moving baud rate line
comPort.BaudRate = int.Parse(MainV2._connectionControl.CMB_baudrate.Text);
if (px4)
{
TXT_terminal.AppendText("Rebooting " + MainV2.comPortName + " at " + comPort.BaudRate + "\n");
// keep it local
using (MAVLinkInterface mine = new MAVLinkInterface())
{
mine.BaseStream.PortName = MainV2.comPortName;
mine.BaseStream.BaudRate = comPort.BaudRate;
mine.giveComport = true;
mine.BaseStream.Open();
// check if we are a mavlink stream
byte[] buffer = mine.readPacket();
if (buffer.Length > 0)
{
log.Info("got packet - sending reboot via mavlink");
TXT_terminal.AppendText("Via Mavlink\n");
mine.doReboot(false);
try
{
mine.BaseStream.Close();
}
catch { }
}
else
{
log.Info("no packet - sending reboot via console");
TXT_terminal.AppendText("Via Console\n");
try
{
mine.BaseStream.Write("reboot\r");
mine.BaseStream.Write("exit\rreboot\r");
}
catch { }
try
{
mine.BaseStream.Close();
}
catch { }
}
}
TXT_terminal.AppendText("Waiting for reboot\n");
// wait 7 seconds for px4 reboot
log.Info("waiting for reboot");
DateTime deadline = DateTime.Now.AddSeconds(9);
while (DateTime.Now < deadline)
{
System.Threading.Thread.Sleep(500);
Application.DoEvents();
}
int a = 0;
while (a < 5)
{
try
{
if (!comPort.IsOpen)
{
comPort.Open();
}
}
catch { }
System.Threading.Thread.Sleep(200);
a++;
}
}
else
{
log.Info("About to open " + comPort.PortName);
comPort.Open();
log.Info("toggle dtr");
comPort.toggleDTR();
}
try
{
comPort.DiscardInBuffer();
}
catch { }
Console.WriteLine("Terminal_Load run " + threadrun + " " + comPort.IsOpen);
BUT_disconnect.Enabled = true;
System.Threading.Thread t11 = new System.Threading.Thread(delegate()
{
threadrun = true;
Console.WriteLine("Terminal thread start run run " + threadrun + " " + comPort.IsOpen);
try
{
comPort.Write("\r");
}
catch { }
// 10 sec
waitandsleep(10000);
Console.WriteLine("Terminal thread 1 run " + threadrun + " " + comPort.IsOpen);
// 100 ms
readandsleep(100);
Console.WriteLine("Terminal thread 2 run " + threadrun + " " + comPort.IsOpen);
try
{
if (!inlogview && comPort.IsOpen)
{
comPort.Write("\n\n\n");
}
// 1 secs
if (!inlogview && comPort.IsOpen)
{
readandsleep(1000);
}
if (!inlogview && comPort.IsOpen)
{
comPort.Write("\r\r\r?\r");
}
}
catch (Exception ex) { Console.WriteLine("Terminal thread 3 " + ex.ToString()); ChangeConnectStatus(false); threadrun = false; return; }
Console.WriteLine("Terminal thread 3 run " + threadrun + " " + comPort.IsOpen);
while (threadrun)
{
try
{
System.Threading.Thread.Sleep(10);
if (!threadrun)
{
break;
}
if (this.Disposing)
{
break;
}
if (inlogview)
{
continue;
}
if (!comPort.IsOpen)
{
Console.WriteLine("Comport Closed");
ChangeConnectStatus(false);
break;
}
if (comPort.BytesToRead > 0)
{
comPort_DataReceived((object)null, (SerialDataReceivedEventArgs)null);
}
}
catch (Exception ex) { Console.WriteLine("Terminal thread 4 " + ex.ToString()); }
}
threadrun = false;
try
{
comPort.DtrEnable = false;
}
catch { }
try
{
Console.WriteLine("term thread close run " + threadrun + " " + comPort.IsOpen);
ChangeConnectStatus(false);
comPort.Close();
}
catch { }
Console.WriteLine("Comport thread close run " + threadrun);
});
t11.IsBackground = true;
t11.Name = "Terminal serial thread";
t11.Start();
// doesnt seem to work on mac
//comPort.DataReceived += new SerialDataReceivedEventHandler(comPort_DataReceived);
if (this.IsDisposed || this.Disposing)
{
return;
}
TXT_terminal.AppendText("Opened com port\r\n");
inputStartPos = TXT_terminal.SelectionStart;
}
catch (Exception ex) { log.Error(ex); TXT_terminal.AppendText("Cant open serial port\r\n"); return; }
TXT_terminal.Focus();
}
void processbg(string file)
{
if (!File.Exists(file + ".jpg"))
{
LogMap.MapLogs(new string[] { file });
}
var loginfo = new loginfo();
loginfo.fullname = file;
loginfo.Size = new FileInfo(file).Length;
if (File.Exists(file + ".jpg"))
{
loginfo.img = new Bitmap(file + ".jpg");
}
if (file.ToLower().EndsWith(".tlog"))
{
using (MAVLinkInterface mine = new MAVLinkInterface())
{
try
{
mine.logplaybackfile =
new BinaryReader(File.Open(file, FileMode.Open, FileAccess.Read, FileShare.Read));
}
catch (Exception ex)
{
log.Debug(ex.ToString());
CustomMessageBox.Show("Log Can not be opened. Are you still connected?");
return;
}
mine.logreadmode = true;
mine.getHeartBeat();
loginfo.Date = mine.lastlogread;
loginfo.Aircraft = mine.sysidcurrent;
var start = mine.lastlogread;
try
{
if (mine.logplaybackfile.BaseStream.Length > MaxPlaybackLogSize)
{
mine.logplaybackfile.BaseStream.Seek(-100000, SeekOrigin.End);
}
else
{
mine.logplaybackfile.BaseStream.Seek(0, SeekOrigin.Begin);
}
}
catch
{
}
var end = mine.lastlogread;
while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
{
mine.readPacket();
if (mine.lastlogread > end)
{
end = mine.lastlogread;
}
}
loginfo.Duration = (end - start).ToString();
}
}
logs.Add(loginfo);
}
static void doread(object o)
{
lock (runlock)
{
run++;
running++;
}
MAVLinkInterface port = (MAVLinkInterface)o;
try
{
port.BaseStream.Open();
int baud = 0;
redo:
if (run == 0)
{
return;
}
DateTime deadline = DateTime.Now.AddSeconds(5);
while (DateTime.Now < deadline)
{
Console.WriteLine("Scan port {0} at {1}", port.BaseStream.PortName, port.BaseStream.BaudRate);
byte[] packet = new byte[0];
try
{
packet = port.readPacket();
}
catch { }
if (packet.Length > 0)
{
port.BaseStream.Close();
Console.WriteLine("Found Mavlink on port {0} at {1}", port.BaseStream.PortName, port.BaseStream.BaudRate);
foundport = true;
portinterface = port.BaseStream;
if (CommsSerialScan.connect)
{
MainV2.comPort.BaseStream = port.BaseStream;
doconnect();
}
lock (runlock)
{
running--;
}
return;
}
if (foundport)
{
break;
}
}
if (!foundport && port.BaseStream.BaudRate > 0)
{
baud++;
if (baud < bauds.Length)
{
port.BaseStream.BaudRate = bauds[baud];
goto redo;
}
}
try
{
port.BaseStream.Close();
}
catch { }
}
catch (Exception ex) { Console.WriteLine(ex.ToString()); }
finally
{
lock (runlock)
{
running--;
}
}
Console.WriteLine("Scan port {0} Finished!!", port.BaseStream.PortName);
}
public static void tlog(string logfile)
{
List <MLArray> mlList = new List <MLArray>();
Hashtable datappl = new Hashtable();
using (MAVLinkInterface mine = new MAVLinkInterface())
{
try
{
mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read));
}
catch { CustomMessageBox.Show("Log Can not be opened. Are you still connected?"); return; }
mine.logreadmode = true;
mine.MAV.packets.Initialize(); // clear
while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
{
byte[] packet = mine.readPacket();
object data = mine.GetPacket(packet);
if (data == null)
{
continue;
}
if (data is MAVLink.mavlink_heartbeat_t)
{
if (((MAVLink.mavlink_heartbeat_t)data).type == (byte)MAVLink.MAV_TYPE.GCS)
{
continue;
}
}
Type test = data.GetType();
DateTime time = mine.lastlogread;
double matlabtime = GetMatLabSerialDate(time);
try
{
foreach (var field in test.GetFields())
{
// field.Name has the field's name.
object fieldValue = field.GetValue(data); // Get value
if (field.FieldType.IsArray)
{
}
else
{
if (!datappl.ContainsKey(field.Name + "_" + field.DeclaringType.Name))
{
datappl[field.Name + "_" + field.DeclaringType.Name] = new List <double[]>();
}
List <double[]> list = ((List <double[]>)datappl[field.Name + "_" + field.DeclaringType.Name]);
object value = fieldValue;
if (value.GetType() == typeof(Single))
{
list.Add(new double[] { matlabtime, (double)(Single)field.GetValue(data) });
}
else if (value.GetType() == typeof(short))
{
list.Add(new double[] { matlabtime, (double)(short)field.GetValue(data) });
}
else if (value.GetType() == typeof(ushort))
{
list.Add(new double[] { matlabtime, (double)(ushort)field.GetValue(data) });
}
else if (value.GetType() == typeof(byte))
{
list.Add(new double[] { matlabtime, (double)(byte)field.GetValue(data) });
}
else if (value.GetType() == typeof(sbyte))
{
list.Add(new double[] { matlabtime, (double)(sbyte)field.GetValue(data) });
}
else if (value.GetType() == typeof(Int32))
{
list.Add(new double[] { matlabtime, (double)(Int32)field.GetValue(data) });
}
else if (value.GetType() == typeof(UInt32))
{
list.Add(new double[] { matlabtime, (double)(UInt32)field.GetValue(data) });
}
else if (value.GetType() == typeof(ulong))
{
list.Add(new double[] { matlabtime, (double)(ulong)field.GetValue(data) });
}
else if (value.GetType() == typeof(long))
{
list.Add(new double[] { matlabtime, (double)(long)field.GetValue(data) });
}
else if (value.GetType() == typeof(double))
{
list.Add(new double[] { matlabtime, (double)(double)field.GetValue(data) });
}
else
{
Console.WriteLine("Unknown data type");
}
}
}
}
catch { }
}
mine.logreadmode = false;
mine.logplaybackfile.Close();
mine.logplaybackfile = null;
}
foreach (string item in datappl.Keys)
{
double[][] temp = ((List <double[]>)datappl[item]).ToArray();
MLArray dbarray = new MLDouble(item.Replace(" ", "_"), temp);
mlList.Add(dbarray);
}
try
{
MatFileWriter mfw = new MatFileWriter(logfile + ".mat", mlList, true);
}
catch (Exception err)
{
MessageBox.Show("There was an error when creating the MAT-file: \n" + err.ToString(),
"MAT-File Creation Error!", MessageBoxButtons.OK, MessageBoxIcon.Exclamation);
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;
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 MAVFtp(MAVLinkInterface mavint, byte sysid, byte compid)
{
_mavint = mavint;
_sysid = sysid;
_compid = compid;
}
public static void SortLogs(string[] logs)
{
foreach (var logfile in logs)
{
FileInfo info = new FileInfo(logfile);
if (info.Length <= 1024)
{
try
{
string destdir = Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar
+ "SMALL" + Path.DirectorySeparatorChar;
if (!Directory.Exists(destdir))
{
Directory.CreateDirectory(destdir);
}
File.Move(logfile, destdir + Path.GetFileName(logfile));
File.Move(logfile.Replace(".tlog", ".rlog"), destdir + Path.GetFileName(logfile).Replace(".tlog", ".rlog"));
}
catch { }
continue;
}
MAVLinkInterface mine = new MAVLinkInterface();
try
{
using (mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read)))
{
mine.logreadmode = true;
byte[] hbpacket = mine.getHeartBeat();
if (hbpacket.Length == 0)
{
continue;
}
MAVLink.mavlink_heartbeat_t hb = (MAVLink.mavlink_heartbeat_t)mine.DebugPacket(hbpacket);
mine.logreadmode = false;
mine.logplaybackfile.Close();
string destdir = Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar
+ mine.MAV.aptype.ToString() + Path.DirectorySeparatorChar
+ hbpacket[3] + Path.DirectorySeparatorChar;
if (!Directory.Exists(destdir))
{
Directory.CreateDirectory(destdir);
}
File.Move(logfile, destdir + Path.GetFileName(logfile));
try
{
File.Move(logfile.Replace(".tlog", ".rlog"), destdir + Path.GetFileName(logfile).Replace(".tlog", ".rlog"));
}
catch { }
}
}
catch { continue; }
}
}
void processbg(string file)
{
a++;
Loading.ShowLoading(a + "/" + files.Count + " " + file, this);
if (!File.Exists(file + ".jpg"))
{
LogMap.MapLogs(new string[] { file });
}
var loginfo = new loginfo();
loginfo.fullname = file;
try
{
// file not found exception even though it passes the exists check above.
loginfo.Size = new FileInfo(file).Length;
}
catch
{
}
if (File.Exists(file + ".jpg"))
{
loginfo.imgfile = file + ".jpg";
}
if (file.ToLower().EndsWith(".tlog"))
{
using (MAVLinkInterface mine = new MAVLinkInterface())
{
try
{
mine.logplaybackfile =
new BinaryReader(File.Open(file, FileMode.Open, FileAccess.Read, FileShare.Read));
}
catch (Exception ex)
{
log.Debug(ex.ToString());
CustomMessageBox.Show("Log Can not be opened. Are you still connected?");
return;
}
mine.logreadmode = true;
mine.speechenabled = false;
// file is to small
if (mine.logplaybackfile.BaseStream.Length < 1024 * 4)
{
return;
}
mine.getHeartBeat();
loginfo.Date = mine.lastlogread;
loginfo.Aircraft = mine.sysidcurrent;
loginfo.Frame = mine.MAV.aptype.ToString();
var start = mine.lastlogread;
try
{
mine.logplaybackfile.BaseStream.Seek(0, SeekOrigin.Begin);
}
catch
{
}
var end = mine.lastlogread;
var length = mine.logplaybackfile.BaseStream.Length;
var a = 0;
// abandon last 100 bytes
while (mine.logplaybackfile.BaseStream.Position < (length - 100))
{
var packet = mine.readPacket();
// gcs
if (packet.sysid == 255)
{
continue;
}
if (packet.msgid == (uint)MAVLink.MAVLINK_MSG_ID.CAMERA_FEEDBACK)
{
loginfo.CamMSG++;
}
if (a % 10 == 0)
{
mine.MAV.cs.UpdateCurrentSettings(null, true, mine);
}
a++;
if (mine.lastlogread > end)
{
end = mine.lastlogread;
}
}
loginfo.Home = mine.MAV.cs.Location;
loginfo.TimeInAir = mine.MAV.cs.timeInAir;
loginfo.DistTraveled = mine.MAV.cs.distTraveled;
loginfo.Duration = (end - start).ToString();
}
}
else if (file.ToLower().EndsWith(".bin") || file.ToLower().EndsWith(".log"))
{
using (CollectionBuffer colbuf = new CollectionBuffer(File.OpenRead(file)))
{
PointLatLngAlt lastpos = null;
DateTime start = DateTime.MinValue;
DateTime end = DateTime.MinValue;
DateTime tia = DateTime.MinValue;
// set time in air/home/distancetraveled
foreach (var dfItem in colbuf.GetEnumeratorType("GPS"))
{
if (dfItem["Status"] != null)
{
var status = int.Parse(dfItem["Status"]);
if (status >= 3)
{
var pos = new PointLatLngAlt(
double.Parse(dfItem["Lat"], CultureInfo.InvariantCulture),
double.Parse(dfItem["Lng"], CultureInfo.InvariantCulture),
double.Parse(dfItem["Alt"], CultureInfo.InvariantCulture));
if (lastpos == null)
{
lastpos = pos;
}
if (start == DateTime.MinValue)
{
loginfo.Date = dfItem.time;
start = dfItem.time;
}
end = dfItem.time;
// add distance
loginfo.DistTraveled += (float)lastpos.GetDistance(pos);
// set home
if (loginfo.Home == null)
{
loginfo.Home = pos;
}
if (dfItem.time > tia.AddSeconds(1))
{
// ground speed > 0.2 or alt > homelat+2
if (double.Parse(dfItem["Spd"], CultureInfo.InvariantCulture) > 0.2 ||
pos.Alt > (loginfo.Home.Alt + 2))
{
loginfo.TimeInAir++;
}
tia = dfItem.time;
}
}
}
}
loginfo.Duration = (end - start).ToString();
loginfo.CamMSG = colbuf.GetEnumeratorType("CAM").Count();
loginfo.Aircraft = 0; //colbuf.dflog.param[""];
loginfo.Frame = "Unknown"; //mine.MAV.aptype.ToString();
}
}
logs.Add(loginfo);
}
/// <summary>
/// Return a list of all gps messages with there timestamp from the log
/// </summary>
/// <param name="fn"></param>
/// <returns></returns>
private Dictionary<long, VehicleLocation> readGPSMsgInLog(string fn)
{
Dictionary<long, VehicleLocation> vehiclePositionList = new Dictionary<long, VehicleLocation>();
// Telemetry Log
if (fn.ToLower().EndsWith("tlog"))
{
using (MAVLinkInterface mine = new MAVLinkInterface())
{
mine.logplaybackfile =
new BinaryReader(File.Open(fn, FileMode.Open, FileAccess.Read, FileShare.Read));
mine.logreadmode = true;
CurrentState cs = new CurrentState();
while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
{
MAVLink.MAVLinkMessage packet = mine.readPacket();
cs.datetime = mine.lastlogread;
cs.UpdateCurrentSettings(null, true, mine);
// check for valid lock
if (!(cs.gpsstatus >=3 || cs.gpsstatus2 >= 3))
continue;
VehicleLocation location = new VehicleLocation();
location.Time = cs.datetime;
location.Lat = cs.lat;
location.Lon = cs.lng;
location.RelAlt = cs.alt;
location.AltAMSL = cs.altasl;
location.Roll = cs.roll;
location.Pitch = cs.pitch;
location.Yaw = cs.yaw;
location.SAlt = cs.sonarrange;
vehiclePositionList[ToMilliseconds(location.Time)] = location;
// 4 5 7
Console.Write((mine.logplaybackfile.BaseStream.Position*100/
mine.logplaybackfile.BaseStream.Length) + " \r");
}
mine.logplaybackfile.Close();
}
}
// DataFlash Log
else
{
using (var sr = new CollectionBuffer(File.OpenRead(fn)))
{
// Will hold the last seen Attitude information in order to incorporate them into the GPS Info
float currentYaw = 0f;
float currentRoll = 0f;
float currentPitch = 0f;
float currentSAlt = 0f;
foreach (var item in sr.GetEnumeratorType(new string[] { "GPS", "GPS2", "ATT", "CTUN", "RFND" }))
{
// Look for GPS Messages. However GPS Messages do not have Roll, Pitch and Yaw
// So we have to look for one ATT message after having read a GPS one
var gpstouse = UseGpsorGPS2();
if (item.msgtype == gpstouse)
{
if (!sr.dflog.logformat.ContainsKey(gpstouse))
continue;
int latindex = sr.dflog.FindMessageOffset(gpstouse, "Lat");
int lngindex = sr.dflog.FindMessageOffset(gpstouse, "Lng");
int altindex = sr.dflog.FindMessageOffset(gpstouse, "Alt");
int statusindex = sr.dflog.FindMessageOffset(gpstouse, "Status");
int raltindex = sr.dflog.FindMessageOffset(gpstouse, "RAlt");
if (raltindex == -1)
raltindex = sr.dflog.FindMessageOffset(gpstouse, "RelAlt");
VehicleLocation location = new VehicleLocation();
try
{
location.Time = item.time;
if (statusindex != -1)
{
// check for valid lock
if (double.Parse(item.items[statusindex], CultureInfo.InvariantCulture) < 3)
continue;
}
if (latindex != -1)
location.Lat = double.Parse(item.items[latindex], CultureInfo.InvariantCulture);
if (lngindex != -1)
location.Lon = double.Parse(item.items[lngindex], CultureInfo.InvariantCulture);
if (raltindex != -1)
location.RelAlt = double.Parse(item.items[raltindex], CultureInfo.InvariantCulture);
if (altindex != -1)
location.AltAMSL = double.Parse(item.items[altindex], CultureInfo.InvariantCulture);
location.Roll = currentRoll;
location.Pitch = currentPitch;
location.Yaw = currentYaw;
location.SAlt = currentSAlt;
long millis = ToMilliseconds(location.Time);
//System.Diagnostics.Debug.WriteLine("GPS MSG - UTCMillis = " + millis + " GPS Week = " + getValueFromStringArray(gpsLineValues, gpsweekpos) + " TimeMS = " + getValueFromStringArray(gpsLineValues, timepos));
if (!vehiclePositionList.ContainsKey(millis) && (location.Time != DateTime.MinValue))
vehiclePositionList[millis] = location;
}
catch
{
Console.WriteLine("Bad "+gpstouse+" Line");
}
}
else if (item.msgtype == "ATT")
{
int Rindex = sr.dflog.FindMessageOffset("ATT", "Roll");
int Pindex = sr.dflog.FindMessageOffset("ATT", "Pitch");
int Yindex = sr.dflog.FindMessageOffset("ATT", "Yaw");
if (Rindex != -1)
currentRoll = float.Parse(item.items[Rindex], CultureInfo.InvariantCulture);
if (Pindex != -1)
currentPitch = float.Parse(item.items[Pindex], CultureInfo.InvariantCulture);
if (Yindex != -1)
currentYaw = float.Parse(item.items[Yindex], CultureInfo.InvariantCulture);
}
else if (item.msgtype == "CTUN")
{
int SAltindex = sr.dflog.FindMessageOffset("CTUN", "SAlt");
if (SAltindex != -1)
{
currentSAlt = float.Parse(item.items[SAltindex], CultureInfo.InvariantCulture);
}
}
else if (item.msgtype == "RFND")
{
int SAltindex = sr.dflog.FindMessageOffset("RFND", "Dist1");
if (SAltindex != -1)
{
currentSAlt = float.Parse(item.items[SAltindex], CultureInfo.InvariantCulture);
}
}
}
}
}
return vehiclePositionList;
}
public MAVLinkSerialPort(MAVLinkInterface mavint, int port)
: this(mavint, (MAVLink.SERIAL_CONTROL_DEV)port)
{
}
public static void ScanAccel()
{
string[] files = Directory.GetFiles(MainV2.LogDir, "*.tlog", SearchOption.AllDirectories);
List<string> badfiles = new List<string>();
foreach (var file in files)
{
bool board = false;
Console.WriteLine(file);
MAVLinkInterface mavi = new MAVLinkInterface();
using (mavi.logplaybackfile = new BinaryReader(File.OpenRead(file)))
{
mavi.logreadmode = true;
try
{
while (mavi.logplaybackfile.BaseStream.Position < mavi.logplaybackfile.BaseStream.Length)
{
byte[] packet = mavi.readPacket();
if (packet.Length == 0)
break;
var objectds = mavi.DebugPacket(packet, false);
if (objectds is MAVLink.mavlink_param_value_t)
{
MAVLink.mavlink_param_value_t param = (MAVLink.mavlink_param_value_t)objectds;
if (ASCIIEncoding.ASCII.GetString(param.param_id).Contains("INS_PRODUCT_ID"))
{
if (param.param_value == 0 || param.param_value == 5)
{
board = true;
}
else
{
break;
}
}
}
if (objectds is MAVLink.mavlink_raw_imu_t)
{
MAVLink.mavlink_raw_imu_t rawimu = (MAVLink.mavlink_raw_imu_t)objectds;
if (board && Math.Abs(rawimu.xacc) > 2000 && Math.Abs(rawimu.yacc) > 2000 && Math.Abs(rawimu.zacc) > 2000)
{
//CustomMessageBox.Show("Bad Log " + file);
badfiles.Add(file);
break;
}
}
}
}
catch { }
}
}
if (badfiles.Count > 0)
{
FileStream fs = File.Open(MainV2.LogDir + Path.DirectorySeparatorChar + "SearchResults.zip", FileMode.Create);
ZipOutputStream zipStream = new ZipOutputStream(fs);
zipStream.SetLevel(9); //0-9, 9 being the highest level of compression
zipStream.UseZip64 = UseZip64.Off; // older zipfile
foreach (var file in badfiles)
{
// entry 2
string entryName = ZipEntry.CleanName(Path.GetFileName(file)); // Removes drive from name and fixes slash direction
ZipEntry newEntry = new ZipEntry(entryName);
newEntry.DateTime = DateTime.Now;
zipStream.PutNextEntry(newEntry);
// Zip the file in buffered chunks
// the "using" will close the stream even if an exception occurs
byte[] buffer = new byte[4096];
using (FileStream streamReader = File.OpenRead(file))
{
StreamUtils.Copy(streamReader, zipStream, buffer);
}
zipStream.CloseEntry();
}
zipStream.IsStreamOwner = true; // Makes the Close also Close the underlying stream
zipStream.Close();
CustomMessageBox.Show("Added " + badfiles.Count + " logs to " + MainV2.LogDir + Path.DirectorySeparatorChar + "SearchResults.zip\n Please send this file to Craig Elder <*****@*****.**>");
}
else
{
CustomMessageBox.Show("No Bad Logs Found");
}
}
public static void SortLogs(string[] logs)
{
foreach (var logfile in logs)
{
FileInfo info = new FileInfo(logfile);
// delete 0 size files
if (info.Length == 0)
{
try
{
File.Delete(logfile);
}
catch
{
}
continue;
}
// move small logs - most likerly invalid
if (info.Length <= 1024)
{
try
{
string destdir = Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar
+ "SMALL" + Path.DirectorySeparatorChar;
if (!Directory.Exists(destdir))
{
Directory.CreateDirectory(destdir);
}
log.Info("Move log small " + logfile + " to " + destdir + Path.GetFileName(logfile));
movefileusingmask(logfile, destdir);
}
catch
{
}
continue;
}
try
{
using (MAVLinkInterface mine = new MAVLinkInterface())
using (
mine.logplaybackfile =
new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read)))
{
mine.logreadmode = true;
var midpoint = mine.logplaybackfile.BaseStream.Length / 2;
mine.logplaybackfile.BaseStream.Seek(midpoint, SeekOrigin.Begin);
byte[] hbpacket = mine.getHeartBeat();
byte[] hbpacket1 = mine.getHeartBeat();
byte[] hbpacket2 = mine.getHeartBeat();
byte[] hbpacket3 = mine.getHeartBeat();
if (hbpacket.Length == 0 && hbpacket1.Length == 0 && hbpacket2.Length == 0 && hbpacket3.Length == 0)
{
mine.logreadmode = false;
mine.logplaybackfile.Close();
if (!Directory.Exists(Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + "BAD"))
{
Directory.CreateDirectory(Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar +
"BAD");
}
log.Info("Move log bad " + logfile + " to " + Path.GetDirectoryName(logfile) +
Path.DirectorySeparatorChar + "BAD" + Path.DirectorySeparatorChar +
Path.GetFileName(logfile));
movefileusingmask(logfile,
Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + "BAD" +
Path.DirectorySeparatorChar);
continue;
}
if (hbpacket.Length != 0)
{
MAVLink.mavlink_heartbeat_t hb = (MAVLink.mavlink_heartbeat_t)mine.DebugPacket(hbpacket);
}
if (hbpacket1.Length != 0)
{
MAVLink.mavlink_heartbeat_t hb1 = (MAVLink.mavlink_heartbeat_t)mine.DebugPacket(hbpacket1);
}
if (hbpacket2.Length != 0)
{
MAVLink.mavlink_heartbeat_t hb2 = (MAVLink.mavlink_heartbeat_t)mine.DebugPacket(hbpacket2);
}
if (hbpacket3.Length != 0)
{
MAVLink.mavlink_heartbeat_t hb3 = (MAVLink.mavlink_heartbeat_t)mine.DebugPacket(hbpacket3);
}
// find most appropriate
if (mine.MAVlist.Count > 1)
{
foreach (var mav in mine.MAVlist.GetMAVStates())
{
if (mav.aptype == MAVLink.MAV_TYPE.ANTENNA_TRACKER)
{
continue;
}
if (mav.aptype == MAVLink.MAV_TYPE.GCS)
{
continue;
}
mine.sysidcurrent = mav.sysid;
mine.compidcurrent = mav.compid;
}
}
mine.logreadmode = false;
mine.logplaybackfile.Close();
string destdir = Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar
+ mine.MAV.aptype.ToString() + Path.DirectorySeparatorChar
+ mine.MAV.sysid + Path.DirectorySeparatorChar;
if (!Directory.Exists(destdir))
{
Directory.CreateDirectory(destdir);
}
movefileusingmask(logfile, destdir);
}
}
catch
{
continue;
}
}
}
protected override bool ProcessCmdKey(ref Message msg, Keys keyData)
{
if (keyData == (Keys.Control | Keys.S))
{
var exepath = CheckandGetSITLImage("ArduCopter.elf");
var model = "+";
var config = GetDefaultConfig(model);
var max = 10;
for (int a = max; a >= 0; a--)
{
var extra = "";
if (!string.IsNullOrEmpty(config))
{
extra += @" --defaults """ + config + @""" -P SERIAL0_PROTOCOL=2 -P SERIAL1_PROTOCOL=2 ";
}
var home = new PointLatLngAlt(markeroverlay.Markers[0].Position).newpos((double)NUM_heading.Value, a * 4);
if (max == a)
{
extra += String.Format(
" -M{4} -s1 --home {3} --instance {0} --uartA tcp:0 {1} -P SYSID_THISMAV={2} ",
a, "", a + 1, BuildHomeLocation(home, (int)NUM_heading.Value), model);
}
else
{
extra += String.Format(
" -M{4} -s1 --home {3} --instance {0} --uartA tcp:0 {1} -P SYSID_THISMAV={2} ",
a, "--uartD tcpclient:127.0.0.1:" + (5770 + 10 * a), a + 1, BuildHomeLocation(home, (int)NUM_heading.Value), model);
}
string simdir = sitldirectory + model + (a + 1) + Path.DirectorySeparatorChar;
Directory.CreateDirectory(simdir);
string path = Environment.GetEnvironmentVariable("PATH");
Environment.SetEnvironmentVariable("PATH", sitldirectory + ";" + simdir + ";" + path, EnvironmentVariableTarget.Process);
Environment.SetEnvironmentVariable("HOME", simdir, EnvironmentVariableTarget.Process);
ProcessStartInfo exestart = new ProcessStartInfo();
exestart.FileName = exepath;
exestart.Arguments = extra;
exestart.WorkingDirectory = simdir;
exestart.WindowStyle = ProcessWindowStyle.Minimized;
exestart.UseShellExecute = true;
Process.Start(exestart);
}
try
{
var client = new Comms.TcpSerial();
client.client = new TcpClient("127.0.0.1", 5760);
MainV2.comPort.BaseStream = client;
SITLSEND = new UdpClient("127.0.0.1", 5501);
Thread.Sleep(200);
MainV2.instance.doConnect(MainV2.comPort, "preset", "5760");
return(true);
}
catch
{
CustomMessageBox.Show(Strings.Failed_to_connect_to_SITL_instance, Strings.ERROR);
return(true);
}
}
if (keyData == (Keys.Control | Keys.D))
{
var exepath = CheckandGetSITLImage("ArduCopter.elf");
var model = "+";
var config = GetDefaultConfig(model);
var max = 10;
for (int a = max; a >= 0; a--)
{
var extra = "";
if (!string.IsNullOrEmpty(config))
{
extra += @" --defaults """ + config + @""" -P SERIAL0_PROTOCOL=2 -P SERIAL1_PROTOCOL=2 ";
}
var home = new PointLatLngAlt(markeroverlay.Markers[0].Position).newpos((double)NUM_heading.Value, a * 4);
if (max == a)
{
extra += String.Format(
" -M{4} -s1 --home {3} --instance {0} --uartA tcp:0 {1} -P SYSID_THISMAV={2} ",
a, "", a + 1, BuildHomeLocation(home, (int)NUM_heading.Value), model);
}
else
{
extra += String.Format(
" -M{4} -s1 --home {3} --instance {0} --uartA tcp:0 {1} -P SYSID_THISMAV={2} ",
a, "" /*"--uartD tcpclient:127.0.0.1:" + (5770 + 10 * a)*/, a + 1, BuildHomeLocation(home, (int)NUM_heading.Value), model);
}
string simdir = sitldirectory + model + (a + 1) + Path.DirectorySeparatorChar;
Directory.CreateDirectory(simdir);
string path = Environment.GetEnvironmentVariable("PATH");
Environment.SetEnvironmentVariable("PATH", sitldirectory + ";" + simdir + ";" + path, EnvironmentVariableTarget.Process);
Environment.SetEnvironmentVariable("HOME", simdir, EnvironmentVariableTarget.Process);
ProcessStartInfo exestart = new ProcessStartInfo();
exestart.FileName = exepath;
exestart.Arguments = extra;
exestart.WorkingDirectory = simdir;
exestart.WindowStyle = ProcessWindowStyle.Minimized;
exestart.UseShellExecute = true;
Process.Start(exestart);
}
try
{
for (int a = max; a >= 0; a--)
{
var mav = new MAVLinkInterface();
var client = new Comms.TcpSerial();
client.client = new TcpClient("127.0.0.1", 5760 + (10 * (a)));
mav.BaseStream = client;
//SITLSEND = new UdpClient("127.0.0.1", 5501);
Thread.Sleep(200);
MainV2.instance.doConnect(mav, "preset", "5760");
MainV2.Comports.Add(mav);
}
return(true);
}
catch
{
CustomMessageBox.Show(Strings.Failed_to_connect_to_SITL_instance, Strings.ERROR);
return(true);
}
}
return(base.ProcessCmdKey(ref msg, keyData));
}
/// <summary>
/// Return a list of all cam messages in a log with timestamp
/// </summary>
/// <param name="fn"></param>
/// <returns></returns>
private Dictionary<long, VehicleLocation> readCAMMsgInLog(string fn)
{
Dictionary<long, VehicleLocation> list = new Dictionary<long, VehicleLocation>();
// Telemetry Log
if (fn.ToLower().EndsWith("tlog"))
{
TXT_outputlog.AppendText("Warning: tlogs are not fully supported when using CAM Messages\n");
using (MAVLinkInterface mine = new MAVLinkInterface())
{
mine.logplaybackfile =
new BinaryReader(File.Open(fn, FileMode.Open, FileAccess.Read, FileShare.Read));
mine.logreadmode = true;
CurrentState cs = new CurrentState();
while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
{
MAVLink.MAVLinkMessage packet = mine.readPacket();
cs.datetime = mine.lastlogread;
cs.UpdateCurrentSettings(null, true, mine);
if (packet.msgid == (uint)MAVLink.MAVLINK_MSG_ID.CAMERA_FEEDBACK)
{
var msg = (MAVLink.mavlink_camera_feedback_t)packet.data;
VehicleLocation location = new VehicleLocation();
location.Time = FromUTCTimeMilliseconds((long)(msg.time_usec / 1000));// cs.datetime;
location.Lat = msg.lat;
location.Lon = msg.lng;
location.RelAlt = msg.alt_rel;
location.AltAMSL = msg.alt_msl;
location.Roll = msg.roll;
location.Pitch = msg.pitch;
location.Yaw = msg.yaw;
location.SAlt = cs.sonarrange;
list[ToMilliseconds(location.Time)] = location;
Console.Write((mine.logplaybackfile.BaseStream.Position*100/
mine.logplaybackfile.BaseStream.Length) + " \r");
}
}
mine.logplaybackfile.Close();
}
}
// DataFlash Log
else
{
float currentSAlt = 0;
using (var sr = new CollectionBuffer(File.OpenRead(fn)))
{
foreach (var item in sr.GetEnumeratorType(new string[] { "CAM", "RFND" }))
{
if (item.msgtype == "CAM")
{
int latindex = sr.dflog.FindMessageOffset("CAM", "Lat");
int lngindex = sr.dflog.FindMessageOffset("CAM", "Lng");
int altindex = sr.dflog.FindMessageOffset("CAM", "Alt");
int raltindex = sr.dflog.FindMessageOffset("CAM", "RelAlt");
int rindex = sr.dflog.FindMessageOffset("CAM", "Roll");
int pindex = sr.dflog.FindMessageOffset("CAM", "Pitch");
int yindex = sr.dflog.FindMessageOffset("CAM", "Yaw");
int gtimeindex = sr.dflog.FindMessageOffset("CAM", "GPSTime");
int gweekindex = sr.dflog.FindMessageOffset("CAM", "GPSWeek");
VehicleLocation p = new VehicleLocation();
p.Time = GetTimeFromGps(int.Parse(item.items[gweekindex], CultureInfo.InvariantCulture),
int.Parse(item.items[gtimeindex], CultureInfo.InvariantCulture));
p.Lat = double.Parse(item.items[latindex], CultureInfo.InvariantCulture);
p.Lon = double.Parse(item.items[lngindex], CultureInfo.InvariantCulture);
p.AltAMSL = double.Parse(item.items[altindex], CultureInfo.InvariantCulture);
if (raltindex != -1)
p.RelAlt = double.Parse(item.items[raltindex], CultureInfo.InvariantCulture);
p.Pitch = float.Parse(item.items[pindex], CultureInfo.InvariantCulture);
p.Roll = float.Parse(item.items[rindex], CultureInfo.InvariantCulture);
p.Yaw = float.Parse(item.items[yindex], CultureInfo.InvariantCulture);
p.SAlt = currentSAlt;
list[ToMilliseconds(p.Time)] = p;
}
else if (item.msgtype == "RFND")
{
int SAltindex = sr.dflog.FindMessageOffset("RFND", "Dist1");
if (SAltindex != -1)
{
currentSAlt = float.Parse(item.items[SAltindex], CultureInfo.InvariantCulture);
}
}
}
}
}
return list;
}
public void setOffsets(MAVLinkInterface mav, double x, double y, double z)
{
offsets[mav] = new HIL.Vector3(x, y, z);
//log.Info(mav.ToString() + " " + offsets[mav].ToString());
}
void processbg(string file)
{
Loading.ShowLoading(file, this);
if (!File.Exists(file + ".jpg"))
{
LogMap.MapLogs(new string[] {file});
}
var loginfo = new loginfo();
loginfo.fullname = file;
loginfo.Size = new FileInfo(file).Length;
if (File.Exists(file + ".jpg"))
{
loginfo.img = new Bitmap(file + ".jpg");
}
if (file.ToLower().EndsWith(".tlog"))
{
using (MAVLinkInterface mine = new MAVLinkInterface())
{
try
{
mine.logplaybackfile =
new BinaryReader(File.Open(file, FileMode.Open, FileAccess.Read, FileShare.Read));
}
catch (Exception ex)
{
log.Debug(ex.ToString());
CustomMessageBox.Show("Log Can not be opened. Are you still connected?");
return;
}
mine.logreadmode = true;
mine.speechenabled = false;
// file is to small
if (mine.logplaybackfile.BaseStream.Length < 1024*4)
return;
mine.getHeartBeat();
loginfo.Date = mine.lastlogread;
loginfo.Aircraft = mine.sysidcurrent;
loginfo.Frame = mine.MAV.aptype.ToString();
var start = mine.lastlogread;
try
{
mine.logplaybackfile.BaseStream.Seek(0, SeekOrigin.Begin);
}
catch
{
}
var end = mine.lastlogread;
var length = mine.logplaybackfile.BaseStream.Length;
var a = 0;
// abandon last 100 bytes
while (mine.logplaybackfile.BaseStream.Position < (length-100))
{
var packet = mine.readPacket();
// gcs
if(packet.sysid == 255)
continue;
if(a % 10 == 0)
mine.MAV.cs.UpdateCurrentSettings(null, true, mine);
a++;
if (mine.lastlogread > end)
end = mine.lastlogread;
}
loginfo.Home = mine.MAV.cs.Location;
loginfo.TimeInAir = mine.MAV.cs.timeInAir;
loginfo.DistTraveled = mine.MAV.cs.distTraveled;
loginfo.Duration = (end - start).ToString();
}
}
logs.Add(loginfo);
}
/// <summary>
/// Return a list of all gps messages with there timestamp from the log
/// </summary>
/// <param name="fn"></param>
/// <returns></returns>
private Dictionary<long, VehicleLocation> readGPSMsgInLog(string fn)
{
Dictionary<long, VehicleLocation> vehiclePositionList = new Dictionary<long, VehicleLocation>();
// Telemetry Log
if (fn.ToLower().EndsWith("tlog"))
{
using (MAVLinkInterface mine = new MAVLinkInterface())
{
mine.logplaybackfile =
new BinaryReader(File.Open(fn, FileMode.Open, FileAccess.Read, FileShare.Read));
mine.logreadmode = true;
mine.MAV.packets.Initialize(); // clear
CurrentState cs = new CurrentState();
while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
{
byte[] packet = mine.readPacket();
cs.datetime = mine.lastlogread;
cs.UpdateCurrentSettings(null, true, mine);
VehicleLocation location = new VehicleLocation();
location.Time = cs.datetime;
location.Lat = cs.lat;
location.Lon = cs.lng;
location.RelAlt = cs.alt;
location.AltAMSL = cs.altasl;
location.Roll = cs.roll;
location.Pitch = cs.pitch;
location.Yaw = cs.yaw;
vehiclePositionList[ToMilliseconds(location.Time)] = location;
// 4 5 7
Console.Write((mine.logplaybackfile.BaseStream.Position*100/
mine.logplaybackfile.BaseStream.Length) + " \r");
}
mine.logplaybackfile.Close();
}
}
// DataFlash Log
else
{
// convert bin to log
if (fn.ToLower().EndsWith("bin"))
{
string tempfile = Path.GetTempFileName();
Log.BinaryLog.ConvertBin(fn, tempfile);
fn = tempfile;
}
using (StreamReader sr = new StreamReader(fn))
{
// Will hold the last seen Attitude information in order to incorporate them into the GPS Info
float currentYaw = 0f;
float currentRoll = 0f;
float currentPitch = 0f;
int a = 0;
while (!sr.EndOfStream)
{
a++;
string line = sr.ReadLine();
var item = dflog.GetDFItemFromLine(line, a);
// Look for GPS Messages. However GPS Messages do not have Roll, Pitch and Yaw
// So we have to look for one ATT message after having read a GPS one
if (item.msgtype == "GPS")
{
if (!dflog.logformat.ContainsKey("GPS"))
continue;
int latindex = dflog.FindMessageOffset("GPS", "Lat");
int lngindex = dflog.FindMessageOffset("GPS", "Lng");
int altindex = dflog.FindMessageOffset("GPS", "Alt");
int raltindex = dflog.FindMessageOffset("GPS", "RAlt");
if (raltindex == -1)
raltindex = dflog.FindMessageOffset("GPS", "RelAlt");
VehicleLocation location = new VehicleLocation();
try
{
location.Time = dflog.GetTimeGPS(line);
location.Lat = double.Parse(item.items[latindex], CultureInfo.InvariantCulture);
location.Lon = double.Parse(item.items[lngindex], CultureInfo.InvariantCulture);
location.RelAlt = double.Parse(item.items[raltindex], CultureInfo.InvariantCulture);
location.AltAMSL = double.Parse(item.items[altindex], CultureInfo.InvariantCulture);
location.Roll = currentRoll;
location.Pitch = currentPitch;
location.Yaw = currentYaw;
long millis = ToMilliseconds(location.Time);
//System.Diagnostics.Debug.WriteLine("GPS MSG - UTCMillis = " + millis + " GPS Week = " + getValueFromStringArray(gpsLineValues, gpsweekpos) + " TimeMS = " + getValueFromStringArray(gpsLineValues, timepos));
if (!vehiclePositionList.ContainsKey(millis) && (location.Time != DateTime.MinValue))
vehiclePositionList[millis] = location;
}
catch
{
Console.WriteLine("Bad GPS Line");
}
}
else if (item.msgtype == "ATT")
{
int Rindex = dflog.FindMessageOffset("ATT", "Roll");
int Pindex = dflog.FindMessageOffset("ATT", "Pitch");
int Yindex = dflog.FindMessageOffset("ATT", "Yaw");
currentRoll = float.Parse(item.items[Rindex], CultureInfo.InvariantCulture);
currentPitch = float.Parse(item.items[Pindex], CultureInfo.InvariantCulture);
currentYaw = float.Parse(item.items[Yindex], CultureInfo.InvariantCulture);
}
}
}
}
return vehiclePositionList;
}
public static void ScanAccel()
{
string[] files = Directory.GetFiles(Settings.Instance.LogDir, "*.tlog", SearchOption.AllDirectories);
List <string> badfiles = new List <string>();
foreach (var file in files)
{
bool board = false;
Console.WriteLine(file);
using (MAVLinkInterface mavi = new MAVLinkInterface())
using (mavi.logplaybackfile = new BinaryReader(File.OpenRead(file)))
{
mavi.logreadmode = true;
try
{
while (mavi.logplaybackfile.BaseStream.Position < mavi.logplaybackfile.BaseStream.Length)
{
MAVLink.MAVLinkMessage packet = mavi.readPacket();
if (packet.Length == 0)
{
break;
}
var objectds = mavi.DebugPacket(packet, false);
if (objectds is MAVLink.mavlink_param_value_t)
{
MAVLink.mavlink_param_value_t param = (MAVLink.mavlink_param_value_t)objectds;
if (ASCIIEncoding.ASCII.GetString(param.param_id).Contains("INS_PRODUCT_ID"))
{
if (param.param_value == 0 || param.param_value == 5)
{
board = true;
}
else
{
break;
}
}
}
if (objectds is MAVLink.mavlink_raw_imu_t)
{
MAVLink.mavlink_raw_imu_t rawimu = (MAVLink.mavlink_raw_imu_t)objectds;
if (board && Math.Abs(rawimu.xacc) > 2000 && Math.Abs(rawimu.yacc) > 2000 &&
Math.Abs(rawimu.zacc) > 2000)
{
//CustomMessageBox.Show("Bad Log " + file);
badfiles.Add(file);
break;
}
}
}
}
catch
{
}
}
}
if (badfiles.Count > 0)
{
FileStream fs = File.Open(Settings.Instance.LogDir + Path.DirectorySeparatorChar + "SearchResults.zip",
FileMode.Create);
ZipOutputStream zipStream = new ZipOutputStream(fs);
zipStream.SetLevel(9); //0-9, 9 being the highest level of compression
zipStream.UseZip64 = UseZip64.Off; // older zipfile
foreach (var file in badfiles)
{
// entry 2
string entryName = ZipEntry.CleanName(Path.GetFileName(file));
// Removes drive from name and fixes slash direction
ZipEntry newEntry = new ZipEntry(entryName);
newEntry.DateTime = DateTime.Now;
zipStream.PutNextEntry(newEntry);
// Zip the file in buffered chunks
// the "using" will close the stream even if an exception occurs
byte[] buffer = new byte[4096];
using (FileStream streamReader = File.OpenRead(file))
{
StreamUtils.Copy(streamReader, zipStream, buffer);
}
zipStream.CloseEntry();
}
zipStream.IsStreamOwner = true; // Makes the Close also Close the underlying stream
zipStream.Close();
CustomMessageBox.Show("Added " + badfiles.Count + " logs to " + Settings.Instance.LogDir +
Path.DirectorySeparatorChar +
"SearchResults.zip\n Please send this file to Craig Elder <*****@*****.**>");
}
else
{
CustomMessageBox.Show("No Bad Logs Found");
}
}
public static void SortLogs(string[] logs)
{
foreach (var logfile in logs)
{
FileInfo info = new FileInfo(logfile);
// delete 0 size files
if (info.Length == 0)
{
try
{
File.Delete(logfile);
}
catch
{
}
continue;
}
// move small logs - most likerly invalid
if (info.Length <= 1024)
{
try
{
string destdir = Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar
+ "SMALL" + Path.DirectorySeparatorChar;
if (!Directory.Exists(destdir))
{
Directory.CreateDirectory(destdir);
}
log.Info("Move log small " + logfile + " to " + destdir + Path.GetFileName(logfile));
movefileusingmask(logfile, destdir);
}
catch
{
}
continue;
}
try
{
using (MAVLinkInterface mine = new MAVLinkInterface())
using (
mine.logplaybackfile =
new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read)))
{
mine.logreadmode = true;
byte[] hbpacket = mine.getHeartBeat();
if (hbpacket.Length == 0)
{
mine.logreadmode = false;
mine.logplaybackfile.Close();
if (!Directory.Exists(Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + "BAD"))
{
Directory.CreateDirectory(Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar +
"BAD");
}
log.Info("Move log bad " + logfile + " to " + Path.GetDirectoryName(logfile) +
Path.DirectorySeparatorChar + "BAD" + Path.DirectorySeparatorChar +
Path.GetFileName(logfile));
movefileusingmask(logfile,
Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + "BAD" +
Path.DirectorySeparatorChar);
continue;
}
MAVLink.mavlink_heartbeat_t hb = (MAVLink.mavlink_heartbeat_t)mine.DebugPacket(hbpacket);
mine.logreadmode = false;
mine.logplaybackfile.Close();
string destdir = Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar
+ mine.MAV.aptype.ToString() + Path.DirectorySeparatorChar
+ hbpacket[3] + Path.DirectorySeparatorChar;
if (!Directory.Exists(destdir))
{
Directory.CreateDirectory(destdir);
}
movefileusingmask(logfile, destdir);
}
}
catch
{
continue;
}
}
}
public static void SortLogs(string[] logs)
{
foreach (var logfile in logs)
{
FileInfo info = new FileInfo(logfile);
// delete 0 size files
if (info.Length == 0)
{
try
{
File.Delete(logfile);
}
catch { }
continue;
}
// move small logs - most likerly invalid
if (info.Length <= 1024)
{
try
{
string destdir = Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar
+ "SMALL" + Path.DirectorySeparatorChar;
if (!Directory.Exists(destdir))
Directory.CreateDirectory(destdir);
log.Info("Move log small " + logfile + " to " + destdir + Path.GetFileName(logfile));
movefileusingmask(logfile, destdir);
}
catch { }
continue;
}
MAVLinkInterface mine = new MAVLinkInterface();
try
{
using (mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read)))
{
mine.logreadmode = true;
byte[] hbpacket = mine.getHeartBeat();
if (hbpacket.Length == 0)
{
mine.logreadmode = false;
mine.logplaybackfile.Close();
if (!Directory.Exists(Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + "BAD"))
Directory.CreateDirectory(Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + "BAD");
log.Info("Move log bad " + logfile + " to " + Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + "BAD" + Path.DirectorySeparatorChar + Path.GetFileName(logfile));
movefileusingmask(logfile, Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + "BAD" + Path.DirectorySeparatorChar);
continue;
}
MAVLink.mavlink_heartbeat_t hb = (MAVLink.mavlink_heartbeat_t)mine.DebugPacket(hbpacket);
mine.logreadmode = false;
mine.logplaybackfile.Close();
string destdir = Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar
+ mine.MAV.aptype.ToString() + Path.DirectorySeparatorChar
+ hbpacket[3] + Path.DirectorySeparatorChar;
if (!Directory.Exists(destdir))
Directory.CreateDirectory(destdir);
movefileusingmask(logfile, destdir);
}
}
catch { continue; }
}
}
void processbg(object directory)
{
//this.Invoke((MethodInvoker)delegate { objectListView1.Clear(); });
string[] files1 = Directory.GetFiles(directory.ToString(), "*.tlog", SearchOption.AllDirectories);
string[] files2 = Directory.GetFiles(directory.ToString(), "*.bin", SearchOption.AllDirectories);
string[] files3 = Directory.GetFiles(directory.ToString(), "*.log", SearchOption.AllDirectories);
List<string> files = new List<string>();
files.AddRange(files1);
files.AddRange(files2);
files.AddRange(files3);
//objectListView1.VirtualListSize = files.Length;
List<object> logs = new List<object>();
foreach (var file in files)
{
if (!File.Exists(file + ".jpg"))
{
LogMap.MapLogs(new string[] {file});
}
var loginfo = new loginfo();
loginfo.fullname = file;
if (File.Exists(file + ".jpg"))
{
loginfo.img = new Bitmap(file + ".jpg");
}
if (file.ToLower().EndsWith(".tlog"))
{
using (MAVLinkInterface mine = new MAVLinkInterface())
{
try
{
mine.logplaybackfile =
new BinaryReader(File.Open(file, FileMode.Open, FileAccess.Read, FileShare.Read));
}
catch (Exception ex)
{
log.Debug(ex.ToString());
CustomMessageBox.Show("Log Can not be opened. Are you still connected?");
return;
}
mine.logreadmode = true;
mine.MAV.packets.Initialize(); // clear
mine.getHeartBeat();
loginfo.Date = mine.lastlogread;
loginfo.Aircraft = mine.sysidcurrent;
var start = mine.lastlogread;
try
{
mine.logplaybackfile.BaseStream.Seek(-100000, SeekOrigin.End);
}
catch
{
}
var end = mine.lastlogread;
while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
{
mine.readPacket();
if (mine.lastlogread > end)
end = mine.lastlogread;
}
loginfo.Duration = (end - start).ToString();
}
}
objectListView1.AddObject(loginfo);
logs.Add(loginfo);
}
/*
this.Invoke((MethodInvoker)delegate
{
objectListView1.AddObjects(logs);
});
*/
}
public MAVList(MAVLinkInterface mavLinkInterface)
{
parent = mavLinkInterface;
// add blank item
hiddenlist.Add(0, new MAVState(parent, 0, 0));
}
public MAVState(MAVLinkInterface mavLinkInterface, byte sysid, byte compid)
{
this.parent = mavLinkInterface;
this.sysid = sysid;
this.compid = compid;
this.packetspersecond = new Dictionary <uint, double>(byte.MaxValue);
this.packetspersecondbuild = new Dictionary <uint, DateTime>(byte.MaxValue);
this.lastvalidpacket = DateTime.MinValue;
sendlinkid = (byte)(new Random().Next(256));
signing = false;
this.param = new MAVLinkParamList();
bool queuewrite = false;
this.param.PropertyChanged += (s, a) =>
{
lock (param)
{
if (queuewrite == true)
{
return;
}
queuewrite = true;
}
new Timer((o) =>
{
try
{
if (cs.uid2 == null || cs.uid2 == "" || sysid == 0)
{
return;
}
if (!Directory.Exists(Path.GetDirectoryName(ParamCachePath)))
{
Directory.CreateDirectory(Path.GetDirectoryName(ParamCachePath));
}
lock (this.param)
File.WriteAllText(ParamCachePath, param.ToJSON());
}
catch (Exception e)
{
log.Error(e);
}
queuewrite = false;
}, null, 2000, -1);
};
this.packets = new Dictionary <uint, Queue <MAVLinkMessage> >(byte.MaxValue);
this.packetsLast = new Dictionary <uint, MAVLinkMessage>(byte.MaxValue);
this.aptype = 0;
this.apname = 0;
this.recvpacketcount = 0;
this.VersionString = "";
this.SoftwareVersions = "";
this.SerialString = "";
this.FrameString = "";
if (sysid != 255 && !(compid == 0 && sysid == 0)) // && !parent.logreadmode)
{
this.Proximity = new Proximity(this);
}
camerapoints.Clear();
this.packetslost = 0f;
this.packetsnotlost = 0f;
this.packetlosttimer = DateTime.MinValue;
cs.parent = this;
}
public static void SortLogs(string[] logs)
{
foreach (var logfile in logs)
{
FileInfo info = new FileInfo(logfile);
// delete 0 size files
if (info.Length == 0)
{
try
{
File.Delete(logfile);
}
catch
{
}
continue;
}
// move small logs - most likerly invalid
if (info.Length <= 1024)
{
try
{
string destdir = Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar
+ "SMALL" + Path.DirectorySeparatorChar;
if (!Directory.Exists(destdir))
Directory.CreateDirectory(destdir);
log.Info("Move log small " + logfile + " to " + destdir + Path.GetFileName(logfile));
movefileusingmask(logfile, destdir);
}
catch
{
}
continue;
}
try
{
using (MAVLinkInterface mine = new MAVLinkInterface())
using (
mine.logplaybackfile =
new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read)))
{
mine.logreadmode = true;
var midpoint = mine.logplaybackfile.BaseStream.Length / 2;
mine.logplaybackfile.BaseStream.Seek(midpoint, SeekOrigin.Begin);
MAVLink.MAVLinkMessage hbpacket = mine.getHeartBeat();
MAVLink.MAVLinkMessage hbpacket1 = mine.getHeartBeat();
MAVLink.MAVLinkMessage hbpacket2 = mine.getHeartBeat();
MAVLink.MAVLinkMessage hbpacket3 = mine.getHeartBeat();
if (hbpacket.Length == 0 && hbpacket1.Length == 0 && hbpacket2.Length == 0 && hbpacket3.Length == 0)
{
mine.logreadmode = false;
mine.logplaybackfile.Close();
if (!Directory.Exists(Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + "BAD"))
Directory.CreateDirectory(Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar +
"BAD");
log.Info("Move log bad " + logfile + " to " + Path.GetDirectoryName(logfile) +
Path.DirectorySeparatorChar + "BAD" + Path.DirectorySeparatorChar +
Path.GetFileName(logfile));
movefileusingmask(logfile,
Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + "BAD" +
Path.DirectorySeparatorChar);
continue;
}
if (hbpacket.Length != 0)
{
MAVLink.mavlink_heartbeat_t hb = (MAVLink.mavlink_heartbeat_t) mine.DebugPacket(hbpacket);
}
if (hbpacket1.Length != 0)
{
MAVLink.mavlink_heartbeat_t hb1 = (MAVLink.mavlink_heartbeat_t)mine.DebugPacket(hbpacket1);
}
if (hbpacket2.Length != 0)
{
MAVLink.mavlink_heartbeat_t hb2 = (MAVLink.mavlink_heartbeat_t)mine.DebugPacket(hbpacket2);
}
if (hbpacket3.Length != 0)
{
MAVLink.mavlink_heartbeat_t hb3 = (MAVLink.mavlink_heartbeat_t)mine.DebugPacket(hbpacket3);
}
// find most appropriate
if (mine.MAVlist.Count > 1)
{
foreach (var mav in mine.MAVlist.GetMAVStates())
{
if (mav.aptype == MAVLink.MAV_TYPE.ANTENNA_TRACKER)
continue;
if (mav.aptype == MAVLink.MAV_TYPE.GCS)
continue;
mine.sysidcurrent = mav.sysid;
mine.compidcurrent = mav.compid;
}
}
mine.logreadmode = false;
mine.logplaybackfile.Close();
string destdir = Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar
+ mine.MAV.aptype.ToString() + Path.DirectorySeparatorChar
+ mine.MAV.sysid + Path.DirectorySeparatorChar;
if (!Directory.Exists(destdir))
Directory.CreateDirectory(destdir);
movefileusingmask(logfile, destdir);
}
}
catch
{
continue;
}
}
}
internal void processToScreen()
{
toolTip1.RemoveAll();
disableNumericUpDownControls(this);
// process hashdefines and update display
foreach (string value in MainV2.comPort.MAV.param.Keys)
{
if (value == null || value == "")
{
continue;
}
//System.Diagnostics.Debug.WriteLine("Doing: " + value);
string name = value;
Control[] text = this.Controls.Find(name, true);
foreach (Control ctl in text)
{
try
{
if (ctl.GetType() == typeof(NumericUpDown))
{
float numbervalue = (float)MainV2.comPort.MAV.param[value];
MAVLinkInterface.modifyParamForDisplay(true, value, ref numbervalue);
NumericUpDown thisctl = ((NumericUpDown)ctl);
thisctl.Maximum = 9000;
thisctl.Minimum = -9000;
thisctl.Value = (decimal)numbervalue;
thisctl.Increment = (decimal)0.0001;
if (thisctl.Name.EndsWith("_P") || thisctl.Name.EndsWith("_I") || thisctl.Name.EndsWith("_D") ||
thisctl.Name.EndsWith("_LOW") || thisctl.Name.EndsWith("_HIGH") || thisctl.Value == 0 ||
thisctl.Value.ToString("0.####", new System.Globalization.CultureInfo("en-US")).Contains("."))
{
thisctl.DecimalPlaces = 4;
}
else
{
thisctl.Increment = (decimal)1;
thisctl.DecimalPlaces = 1;
}
if (thisctl.Name.ToUpper().EndsWith("THR_RATE_IMAX"))
{
thisctl.Maximum = 1000; // is a pwm
thisctl.Minimum = 0;
}
else if (thisctl.Name.EndsWith("_IMAX"))
{
thisctl.Maximum = 1800;
thisctl.Minimum = -1800;
}
thisctl.Enabled = true;
ThemeManager.ApplyThemeTo(thisctl);
thisctl.Validated += null;
if (tooltips[value] != null)
{
try
{
toolTip1.SetToolTip(ctl, ((paramsettings)tooltips[value]).desc);
}
catch { }
}
thisctl.Validated += new EventHandler(EEPROM_View_float_TextChanged);
}
else if (ctl.GetType() == typeof(ComboBox))
{
ComboBox thisctl = ((ComboBox)ctl);
thisctl.SelectedValue = (int)(float)MainV2.comPort.MAV.param[value];
thisctl.Validated += new EventHandler(ComboBox_Validated);
ThemeManager.ApplyThemeTo(thisctl);
}
}
catch { }
}
if (text.Length == 0)
{
//Console.WriteLine(name + " not found");
}
}
}
/// <summary>
/// Recevied UDP packet, process and send required data to serial port.
/// </summary>
/// <param name="data">Packet</param>
/// <param name="receviedbytes">Length</param>
/// <param name="comPort">Com Port</param>
private void RECVprocess(byte[] data, int receviedbytes, MAVLinkInterface comPort)
{
var sitldata = new sitl_fdm();
sitldata.timestamp = (ulong) (simtime*1.0e6);
if (data[0] == 'D' && data[1] == 'A')
{
// Xplanes sends
// 5 byte header
// 1 int for the index - numbers on left of output
// 8 floats - might be useful. or 0 if not
var count = 5;
while (count < receviedbytes)
{
var index = BitConverter.ToInt32(data, count);
DATA[index] = new float[8];
DATA[index][0] = BitConverter.ToSingle(data, count + 1*4);
;
DATA[index][1] = BitConverter.ToSingle(data, count + 2*4);
;
DATA[index][2] = BitConverter.ToSingle(data, count + 3*4);
;
DATA[index][3] = BitConverter.ToSingle(data, count + 4*4);
;
DATA[index][4] = BitConverter.ToSingle(data, count + 5*4);
;
DATA[index][5] = BitConverter.ToSingle(data, count + 6*4);
;
DATA[index][6] = BitConverter.ToSingle(data, count + 7*4);
;
DATA[index][7] = BitConverter.ToSingle(data, count + 8*4);
;
count += 36; // 8 * float
}
var xplane9 = !CHK_xplane10.Checked;
if (xplane9)
{
sitldata.pitchDeg = (DATA[18][0]);
sitldata.rollDeg = (DATA[18][1]);
sitldata.yawDeg = (DATA[18][2]);
sitldata.pitchRate = (DATA[17][0]*rad2deg);
sitldata.rollRate = (DATA[17][1]*rad2deg);
sitldata.yawRate = (DATA[17][2]*rad2deg);
sitldata.heading = DATA[19][2];
sitldata.speedN = -DATA[21][5]; // (DATA[3][7] * 0.44704 * Math.Sin(sitldata.heading * deg2rad));
sitldata.speedE = DATA[21][3]; // (DATA[3][7] * 0.44704 * Math.Cos(sitldata.heading * deg2rad));
sitldata.speedD = -DATA[21][4];
}
else
{
sitldata.pitchDeg = (DATA[17][0]);
sitldata.rollDeg = (DATA[17][1]);
sitldata.yawDeg = (DATA[17][2]);
sitldata.pitchRate = (DATA[16][0]*rad2deg);
sitldata.rollRate = (DATA[16][1]*rad2deg);
sitldata.yawRate = (DATA[16][2]*rad2deg);
sitldata.heading = (DATA[18][2]); // 18-2
sitldata.speedN = -DATA[21][5]; // (DATA[3][7] * 0.44704 * Math.Sin(sitldata.heading * deg2rad));
sitldata.speedE = DATA[21][3]; // (DATA[3][7] * 0.44704 * Math.Cos(sitldata.heading * deg2rad));
sitldata.speedD = -DATA[21][4];
}
sitldata.airspeed = ((DATA[3][5]*.44704));
sitldata.latitude = (DATA[20][0]);
sitldata.longitude = (DATA[20][1]);
sitldata.altitude = (DATA[20][2]*ft2m);
//http://en.wikipedia.org/wiki/Banked_turn
var dcm = new Matrix3();
dcm.from_euler(sitldata.rollDeg*deg2rad, sitldata.pitchDeg*deg2rad, sitldata.yawDeg*deg2rad);
// v squared in m/s
var v_sq = (float) ((DATA[3][7]*0.44704)*(DATA[3][7]*0.44704));
// rad = tas^2 / (tan(angle) * G)
var turnrad = v_sq/(float) (9.808f*Math.Tan(sitldata.rollDeg*deg2rad));
var gload = (float) (1/Math.Cos(sitldata.rollDeg*deg2rad)); // calculated Gs
// a = v^2/r
var centripaccel = v_sq/turnrad;
var accel_body = dcm.transposed()*(new Vector3(0, 0, -9.808));
var centrip_accel = new Vector3(0, -centripaccel*Math.Cos(sitldata.rollDeg*deg2rad),
centripaccel*Math.Sin(sitldata.rollDeg*deg2rad));
var pitchpart = centrip_accel.y*Math.Cos(sitldata.pitchDeg*deg2rad);
//centrip_accel.x += pitchpart;
//centrip_accel.y += pitchpart;
//centrip_accel.y = -centripaccel;
accel_body -= centrip_accel;
var velocitydelta = dcm.transposed()*
(new Vector3((sitldata_old.speedN - sitldata.speedN),
(sitldata_old.speedE - sitldata.speedE), (sitldata_old.speedD - sitldata.speedD)));
// v = d/t a = v/t add linear accel - t is 50 hz
//accel_body.x += -velocitydelta.x / 0.02;
//Vector3 velocity = dcm.transposed() * (new Vector3((sitldata.speedN), (sitldata.speedE), (sitldata.speedD)));
//Console.WriteLine("vel " + velocity.ToString());
//Console.WriteLine("ved " + velocitydelta.ToString());
// a = dv / dt
// 50 hz = 0.02sec
var accel_mine_body = dcm.transposed()*
(new Vector3((sitldata_old.speedN - sitldata.speedN)/0.02,
(sitldata_old.speedE - sitldata.speedE)/0.02,
(sitldata_old.speedD - sitldata.speedD)/0.02));
// Console.WriteLine("G"+accel_body.ToString());
// Console.WriteLine("M"+accel_mine_body.ToString());
//sitldata.pitchRate = 0;
//sitldata.rollRate = 0;
//sitldata.yawRate = 0;
sitldata.xAccel = accel_body.x; // DATA[4][5] * 1;
sitldata.yAccel = accel_body.y; // DATA[4][6] * 1;
sitldata.zAccel = accel_body.z; // (0 - DATA[4][4]) * 9.808;
//sitldata.xAccel = DATA[4][5] *9.808;
//sitldata.yAccel = DATA[4][6] *9.808;
//sitldata.zAccel = -DATA[4][4] *9.808;
}
else if (receviedbytes == 0x64) // FG binary udp
{
//FlightGear
/*
fgIMUData imudata2 = data.ByteArrayToStructureBigEndian<fgIMUData>(0);
if (imudata2.magic != 0x4c56414d)
return;
if (imudata2.latitude == 0)
return;
chkSensor.Checked = true;
imu.time_usec = ((ulong)DateTime.Now.ToBinary());
imu.xacc = ((Int16)(imudata2.accelX * 9808 / 32.2));
imu.xgyro = ((Int16)(imudata2.rateRoll * 17.453293));
imu.xmag = 0;
imu.yacc = ((Int16)(imudata2.accelY * 9808 / 32.2));
imu.ygyro = ((Int16)(imudata2.ratePitch * 17.453293));
imu.ymag = 0;
imu.zacc = ((Int16)(imudata2.accelZ * 9808 / 32.2)); // + 1000
imu.zgyro = ((Int16)(imudata2.rateYaw * 17.453293));
imu.zmag = 0;
gps.alt = ((int)(imudata2.altitude * ft2m * 1000));
gps.fix_type = 3;
gps.cog = (ushort)(Math.Atan2(imudata2.velocityE, imudata2.velocityN) * rad2deg * 100);
gps.lat = (int)(imudata2.latitude * 1.0e7);
gps.lon = (int)(imudata2.longitude * 1.0e7);
gps.time_usec = ((ulong)DateTime.Now.Ticks);
gps.vel = (ushort)(Math.Sqrt((imudata2.velocityN * imudata2.velocityN) + (imudata2.velocityE * imudata2.velocityE)) * ft2m * 100);
//FileStream stream = File.OpenWrite("fgdata.txt");
//stream.Write(data, 0, receviedbytes);
//stream.Close();
*/
}
else if (receviedbytes == 662 || receviedbytes == 658) // 658 = 3.83 662 = 3.91
{
/*
// Model data in body frame coordinates (X=Right, Y=Front, Z=Up)
public float Model_fVel_Body_X; public float Model_fVel_Body_Y; public float Model_fVel_Body_Z; // m/s Model velocity in body coordinates
public float Model_fAngVel_Body_X; public float Model_fAngVel_Body_Y; public float Model_fAngVel_Body_Z; // rad/s Model angular velocity in body coordinates
public float Model_fAccel_Body_X; public float Model_fAccel_Body_Y; public float Model_fAccel_Body_Z; // m/s/s Model acceleration in body coordinates
*/
var aeroin_last = aeroin;
aeroin = data.ByteArrayToStructure<TDataFromAeroSimRC>(0);
sitldata.pitchDeg = (aeroin.Model_fPitch*rad2deg);
sitldata.rollDeg = (aeroin.Model_fRoll*-1*rad2deg);
sitldata.yawDeg = ((aeroin.Model_fHeading*rad2deg));
sitldata.pitchRate = aeroin.Model_fAngVel_Body_X*rad2deg;
sitldata.rollRate = aeroin.Model_fAngVel_Body_Y*rad2deg;
sitldata.yawRate = -aeroin.Model_fAngVel_Body_Z*rad2deg;
// calc gravity
var dcm = new Matrix3();
dcm.from_euler(sitldata.rollDeg*deg2rad, sitldata.pitchDeg*deg2rad, sitldata.yawDeg*deg2rad);
var accel_body = dcm.transposed()*(new Vector3(0, 0, -9.8)); // -9.8
sitldata.xAccel = aeroin.Model_fAccel_Body_Y/9.808 + accel_body.x; // pitch - back forward-
sitldata.yAccel = aeroin.Model_fAccel_Body_X/9.808 + accel_body.y; // roll - left right-
sitldata.zAccel = -aeroin.Model_fAccel_Body_Z/9.808 + accel_body.z;
// Console.WriteLine("2 {0,20} {1,20} {2,20}", aeroin.Model_fAccel_Body_X.ToString("0.000"), aeroin.Model_fAccel_Body_Y.ToString("0.000"), aeroin.Model_fAccel_Body_Z.ToString("0.000"));
sitldata.altitude = aeroin.Model_fPosZ;
sitldata.latitude = aeroin.Model_fLatitude;
sitldata.longitude = aeroin.Model_fLongitude;
sitldata.speedN = aeroin.Model_fVelY;
sitldata.speedE = aeroin.Model_fVelX;
sitldata.speedD = aeroin.Model_fVelZ;
var xvec = aeroin.Model_fVelY - aeroin.Model_fWindVelY;
var yvec = aeroin.Model_fVelX - aeroin.Model_fWindVelX;
sitldata.airspeed = ((float) Math.Sqrt((yvec*yvec) + (xvec*xvec)));
}
else if (receviedbytes == 408)
{
var fdm = data.ByteArrayToStructureBigEndian<FGNetFDM>(0);
lastfdmdata = fdm;
sitldata.timestamp = (ulong) (simtime*1.0e6);
sitldata.altitude = (fdm.altitude);
sitldata.latitude = (fdm.latitude*rad2deg);
sitldata.longitude = (fdm.longitude*rad2deg);
sitldata.rollDeg = fdm.phi*rad2deg;
sitldata.pitchDeg = fdm.theta*rad2deg;
sitldata.yawDeg = fdm.psi*rad2deg;
sitldata.rollRate = fdm.phidot*rad2deg;
sitldata.pitchRate = fdm.thetadot*rad2deg;
sitldata.yawRate = fdm.psidot*rad2deg;
sitldata.speedN = fdm.v_north*ft2m;
sitldata.speedE = fdm.v_east*ft2m;
sitldata.speedD = fdm.v_down*ft2m;
sitldata.xAccel = (fdm.A_X_pilot*9.808/32.2); // pitch
sitldata.yAccel = (fdm.A_Y_pilot*9.808/32.2); // roll
sitldata.zAccel = (fdm.A_Z_pilot/32.2*9.808);
sitldata.airspeed = fdm.vcas*0.5144444f; // knots to m/s
//Console.WriteLine("1 {0} {1} {2} {3}",(float)sitldata.rollDeg,MainV2.comPort.MAV.cs.roll,sitldata.pitchDeg,MainV2.comPort.MAV.cs.pitch);
if (RAD_JSBSim.Checked)
sitldata.airspeed = fdm.vcas*ft2m; // fps to m/s
}
else
{
log.Info("Bad Udp Packet " + receviedbytes);
return;
}
if (sitldata.altitude < 0)
sitldata.altitude = 0.00001;
sitldata_old = sitldata;
var gpsspan = DateTime.Now - lastgpsupdate;
// add gps delay
if (gpsspan.TotalMilliseconds >= GPS_rate)
{
lastgpsupdate = DateTime.Now;
// save current fix = 3
sitl_fdmbuffer[gpsbufferindex%sitl_fdmbuffer.Length] = sitldata;
// Console.WriteLine((gpsbufferindex % gpsbuffer.Length) + " " + ((gpsbufferindex + (gpsbuffer.Length - 1)) % gpsbuffer.Length));
// return buffer index + 5 = (3 + 5) = 8 % 6 = 2
oldgps = sitl_fdmbuffer[(gpsbufferindex + (sitl_fdmbuffer.Length - 1))%sitl_fdmbuffer.Length];
//comPort.sendPacket(oldgps);
gpsbufferindex++;
}
var hilstate = new MAVLink.mavlink_hil_state_t();
var epochBegin = new DateTime(1980, 1, 6, 0, 0, 0, DateTimeKind.Utc);
hilstate.time_usec = (ulong) ((DateTime.Now.Ticks - epochBegin.Ticks)/10); // microsec
hilstate.lat = (int) (oldgps.latitude*1e7); // * 1E7
hilstate.lon = (int) (oldgps.longitude*1e7); // * 1E7
hilstate.alt = (int) (oldgps.altitude*1000); // mm
// Console.WriteLine(hilstate.alt);
// Console.WriteLine("{0} {1} {2}", sitldata.rollDeg.ToString("0.0"), sitldata.pitchDeg.ToString("0.0"), sitldata.yawDeg.ToString("0.0"));
hilstate.pitch = (float) sitldata.pitchDeg*deg2rad; // (rad)
hilstate.pitchspeed = (float) sitldata.pitchRate*deg2rad; // (rad/s)
hilstate.roll = (float) sitldata.rollDeg*deg2rad; // (rad)
hilstate.rollspeed = (float) sitldata.rollRate*deg2rad; // (rad/s)
hilstate.yaw = (float) sitldata.yawDeg*deg2rad; // (rad)
hilstate.yawspeed = (float) sitldata.yawRate*deg2rad; // (rad/s)
hilstate.vx = (short) (sitldata.speedN*100); // m/s * 100 - lat
hilstate.vy = (short) (sitldata.speedE*100); // m/s * 100 - long
hilstate.vz = (short) (sitldata.speedD*100); // m/s * 100 - + speed down
hilstate.xacc = (short) (sitldata.xAccel*101.957); // (mg)
hilstate.yacc = (short) (sitldata.yAccel*101.957); // (mg)
hilstate.zacc = (short) (sitldata.zAccel*101.957); // (mg)
packetcount++;
if (!comPort.BaseStream.IsOpen)
return;
if (comPort.BaseStream.BytesToWrite > 100)
return;
// if (packetcount % 2 == 0)
// return;
comPort.sendPacket(hilstate,comPort.sysidcurrent,comPort.compidcurrent);
// comPort.sendPacket(oldgps);
//comPort.sendPacket(new MAVLink.mavlink_vfr_hud_t() { airspeed = (float)sitldata.airspeed } );
var pres = new MAVLink.mavlink_raw_pressure_t();
var calc = (101325*Math.Pow(1 - 2.25577*Math.Pow(10, -5)*sitldata.altitude, 5.25588));
// updated from valid gps
pres.press_diff1 = (short) (int) (calc - 101325); // 0 alt is 0 pa
// comPort.sendPacket(pres);
}
internal void EEPROM_View_float_TextChanged(object sender, EventArgs e)
{
if (startup == true)
{
return;
}
float value = 0;
string name = ((Control)sender).Name;
// do domainupdown state check
try
{
if (sender.GetType() == typeof(NumericUpDown))
{
value = (float)((NumericUpDown)sender).Value;
MAVLinkInterface.modifyParamForDisplay(false, ((Control)sender).Name, ref value);
changes[name] = value;
}
else if (sender.GetType() == typeof(ComboBox))
{
value = (int)((ComboBox)sender).SelectedValue;
changes[name] = value;
}
((Control)sender).BackColor = Color.Green;
}
catch (Exception)
{
((Control)sender).BackColor = Color.Red;
}
try
{
// enable roll and pitch pairing for ac2
if (CHK_lockrollpitch.Checked)
{
if (name.StartsWith("RATE_") || name.StartsWith("STB_") || name.StartsWith("ACRO_"))
{
if (name.Contains("_RLL_"))
{
string newname = name.Replace("_RLL_", "_PIT_");
Control[] arr = this.Controls.Find(newname, true);
changes[newname] = value;
if (arr.Length > 0)
{
arr[0].Text = ((Control)sender).Text;
arr[0].BackColor = Color.Green;
}
}
else if (name.Contains("_PIT_"))
{
string newname = name.Replace("_PIT_", "_RLL_");
Control[] arr = this.Controls.Find(newname, true);
changes[newname] = value;
if (arr.Length > 0)
{
arr[0].Text = ((Control)sender).Text;
arr[0].BackColor = Color.Green;
}
}
}
}
// keep nav_lat and nav_lon paired
if (name.Contains("NAV_LAT_"))
{
string newname = name.Replace("NAV_LAT_", "NAV_LON_");
Control[] arr = this.Controls.Find(newname, true);
changes[newname] = value;
if (arr.Length > 0)
{
arr[0].Text = ((Control)sender).Text;
arr[0].BackColor = Color.Green;
}
}
// keep loiter_lat and loiter_lon paired
if (name.Contains("LOITER_LAT_"))
{
string newname = name.Replace("LOITER_LAT_", "LOITER_LON_");
Control[] arr = this.Controls.Find(newname, true);
changes[newname] = value;
if (arr.Length > 0)
{
arr[0].Text = ((Control)sender).Text;
arr[0].BackColor = Color.Green;
}
}
// keep nav_lat and nav_lon paired
if (name.Contains("HLD_LAT_"))
{
string newname = name.Replace("HLD_LAT_", "HLD_LON_");
Control[] arr = this.Controls.Find(newname, true);
changes[newname] = value;
if (arr.Length > 0)
{
arr[0].Text = ((Control)sender).Text;
arr[0].BackColor = Color.Green;
}
}
}
catch { }
}
public void UploadFence(MAVLinkInterface port, Action <int, string> progress = null)
{
var fence = FenceToLocation();
mav_mission.upload(port, MAVLink.MAV_MISSION_TYPE.FENCE, fence, progress);
}
internal void processToScreen()
{
toolTip1.RemoveAll();
disableNumericUpDownControls(this);
// process hashdefines and update display
foreach (string value in MainV2.comPort.MAV.param.Keys)
{
if (value == null || value == "")
{
continue;
}
var name = value;
var text = Controls.Find(name, true);
foreach (var ctl in text)
{
try
{
if (ctl.GetType() == typeof(NumericUpDown))
{
var numbervalue = (float)MainV2.comPort.MAV.param[value];
MAVLinkInterface.modifyParamForDisplay(true, value, ref numbervalue);
var thisctl = ((NumericUpDown)ctl);
thisctl.Maximum = 9000;
thisctl.Minimum = -9000;
thisctl.Value = (decimal)numbervalue;
thisctl.Increment = (decimal)0.001;
if (thisctl.Name.EndsWith("_P") || thisctl.Name.EndsWith("_I") ||
thisctl.Name.EndsWith("_D") ||
thisctl.Name.EndsWith("_LOW") || thisctl.Name.EndsWith("_HIGH") || thisctl.Value == 0 ||
thisctl.Value.ToString("0.###", new CultureInfo("en-US")).Contains("."))
{
thisctl.DecimalPlaces = 3;
}
else
{
thisctl.Increment = 1;
thisctl.DecimalPlaces = 1;
}
if (thisctl.Name.EndsWith("_IMAX"))
{
thisctl.Maximum = 180;
thisctl.Minimum = -180;
}
thisctl.Enabled = true;
ThemeManager.ApplyThemeTo(thisctl);
thisctl.Validated += null;
if (tooltips[value] != null)
{
try
{
toolTip1.SetToolTip(ctl, ((paramsettings)tooltips[value]).desc);
}
catch
{
}
}
thisctl.Validated += EEPROM_View_float_TextChanged;
}
else if (ctl.GetType() == typeof(ComboBox))
{
var thisctl = ((ComboBox)ctl);
thisctl.SelectedIndex = (int)(float)MainV2.comPort.MAV.param[value];
thisctl.Validated += ComboBox_Validated;
ThemeManager.ApplyThemeTo(thisctl);
}
}
catch
{
}
}
if (text.Length == 0)
{
//Console.WriteLine(name + " not found");
}
}
}
