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 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 MAVLinkSerialPort(MAVLinkInterface mavint, MAVLink.SERIAL_CONTROL_DEV port)
{
this.mavint = mavint;
this.port = port;
if (!mavint.BaseStream.IsOpen)
{
throw new Exception(Strings.PleaseConnect);
}
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_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 TerrainFollow(MAVLinkInterface inInterface)
{
_interface = inInterface;
log.Info("Subscribe to packets");
subscription = _interface.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.TERRAIN_DATA, ReceviedPacket);
mFlightData = FlightData.instance;
}
private void but_start_Click(object sender, EventArgs e)
{
GraphPane myPane = zedGraphControl1.GraphPane;
var line = new LineItem("ScanPoints", new RollingPointPairList(20), Color.White, SymbolType.Diamond);
var sonar = new LineItem("Sonar", new PointPairList(), Color.Red, SymbolType.Circle);
sonar.AddPoint(0, 0);
line.Line.IsVisible = false;
myPane.Title.Text = "ST-1000 Sonar";
myPane.CurveList.Add(line);
myPane.CurveList.Add(sonar);
myPane.YAxis.Scale.IsReverse = true;
myPane.YAxis.Title.Text = "Depth (m)";
myPane.YAxis.Title.Text = "Distance (m)";
myPane.XAxis.Scale.Max = 2; myPane.XAxis.Scale.Min = -2;
myPane.YAxis.Scale.Max = 5; myPane.YAxis.Scale.Min = 0;
myPane.XAxis.MajorGrid.Color = Color.DimGray;
myPane.YAxis.MajorGrid.Color = Color.DimGray;
myPane.XAxis.MajorGrid.IsVisible = true;
myPane.YAxis.MajorGrid.IsVisible = true;
var subscribeToPacket =
mav.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.SCANNING_SONAR,
message =>
{
MAVLink.mavlink_scanning_sonar_t S = (MAVLink.mavlink_scanning_sonar_t)message.data;
data_txt = $"{S.time_boot_ms},{S.range / 1000.0},{S.angle / 10.0},{S.roll / 10.0},{S.pitch / 10.0},{S.yaw / 10.0}";
data_txt += Environment.NewLine;
double deg2rad = Math.PI / 180;
double Y = S.range / 1000.0 * Math.Cos((double)(S.angle / 10.0) * deg2rad);
double X = S.range / 1000.0 * Math.Sin((double)(S.angle / 10.0) * deg2rad);
line.AddPoint(X, Y);
double Total = 0;
for (int I = 0; I < line.NPts; I++)
{
Total += line.Points[I].Y;
}
double AvgY = Total / line.NPts;
try
{
textBox1.Invoke(new Action(() => textBox1.AppendText(data_txt)));
textBoxDepth.Invoke(new Action(() => textBoxDepth.Text = AvgY.ToString("F1")));
textBoxRoll.Invoke(new Action(() => textBoxRoll.Text = (S.roll / 10.0).ToString("F1")));
}
catch (ObjectDisposedException) { }
//zedGraphControl1.AxisChange();
zedGraphControl1.Invalidate();
return(true);
});
// timer.Enabled = true;
}
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);
}
public static void SortLogs(string[] logs, string masterdestdir = "")
{
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
{
}
continue;
}
// 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
{
}
continue;
}
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);
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)
{
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
{
continue;
}
}
}
public static void SortLogs(string[] logs, string masterdestdir = "")
{
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
{
}
continue;
}
// 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
{
}
continue;
}
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);
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)
{
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
{
continue;
}
}
}
