public void Dispose()
{
if (_bytesReceivedSubj != null)
_bytesReceivedSubj.Dispose();
if (_bytesSentSubj != null)
_bytesSentSubj.Dispose();
this.Close();
Terrain = null;
MirrorStream = null;
logreadmode = false;
logplaybackfile = null;
}
private void OpenBg(object PRsender, bool getparams, ProgressWorkerEventArgs progressWorkerEventArgs)
{
frmProgressReporter.UpdateProgressAndStatus(-1, "Mavlink Connecting...");
giveComport = true;
// allow settings to settle - previous dtr
System.Threading.Thread.Sleep(1000);
// reset
MAV.sysid = 0;
MAV.compid = 0;
MAV.param = new Hashtable();
MAV.packets.Initialize();
MAV.VersionString = "";
MAV.SoftwareVersions = "";
MAV.SerialString = "";
Terrain = new TerrainFollow();
bool hbseen = false;
try
{
BaseStream.ReadBufferSize = 16 * 1024;
lock (objlock) // so we dont have random traffic
{
log.Info("Open port with " + BaseStream.PortName + " " + BaseStream.BaudRate);
BaseStream.Open();
BaseStream.DiscardInBuffer();
// other boards seem to have issues if there is no delay? posible bootloader timeout issue
Thread.Sleep(1000);
}
byte[] buffer = new byte[0];
byte[] buffer1 = new byte[0];
DateTime start = DateTime.Now;
DateTime deadline = start.AddSeconds(CONNECT_TIMEOUT_SECONDS);
var countDown = new System.Timers.Timer { Interval = 1000, AutoReset = false };
countDown.Elapsed += (sender, e) =>
{
int secondsRemaining = (deadline - e.SignalTime).Seconds;
frmProgressReporter.UpdateProgressAndStatus(-1, string.Format("Trying to connect.\nTimeout in {0}", secondsRemaining));
if (secondsRemaining > 0) countDown.Start();
};
countDown.Start();
int count = 0;
while (true)
{
if (progressWorkerEventArgs.CancelRequested)
{
progressWorkerEventArgs.CancelAcknowledged = true;
countDown.Stop();
if (BaseStream.IsOpen)
BaseStream.Close();
giveComport = false;
return;
}
log.Info(DateTime.Now.Millisecond + " Start connect loop ");
if (DateTime.Now > deadline)
{
//if (Progress != null)
// Progress(-1, "No Heatbeat Packets");
countDown.Stop();
this.Close();
if (hbseen)
{
progressWorkerEventArgs.ErrorMessage = "Only 1 Heatbeat Received";
throw new Exception("Only 1 Mavlink Heartbeat Packets was read from this port - Verify your hardware is setup correctly\nMission Planner waits for 2 valid heartbeat packets before connecting");
}
else
{
progressWorkerEventArgs.ErrorMessage = "No Heatbeat Packets Received";
throw new Exception(@"Can not establish a connection\n
Please check the following
1. You have firmware loaded
2. You have the correct serial port selected
3. PX4 - You have the microsd card installed
4. Try a diffrent usb port\n\n"+"No Mavlink Heartbeat Packets where read from this port - Verify Baud Rate and setup\nMission Planner waits for 2 valid heartbeat packets before connecting");
}
}
System.Threading.Thread.Sleep(1);
// can see 2 heartbeat packets at any time, and will connect - was one after the other
if (buffer.Length == 0)
buffer = getHeartBeat();
System.Threading.Thread.Sleep(1);
if (buffer1.Length == 0)
buffer1 = getHeartBeat();
if (buffer.Length > 0 || buffer1.Length > 0)
hbseen = true;
count++;
// 2 hbs that match
if (buffer.Length > 5 && buffer1.Length > 5 && buffer[3] == buffer1[3] && buffer[4] == buffer1[4])
{
mavlink_heartbeat_t hb = buffer.ByteArrayToStructure<mavlink_heartbeat_t>(6);
if (hb.type != (byte)MAVLink.MAV_TYPE.GCS)
{
SetupMavConnect(buffer[3],buffer[4], buffer[2], hb);
break;
}
}
// 2 hb's that dont match. more than one sysid here
if (buffer.Length > 5 && buffer1.Length > 5 && (buffer[3] != buffer1[3] || buffer[4] != buffer1[4]))
{
mavlink_heartbeat_t hb = buffer.ByteArrayToStructure<mavlink_heartbeat_t>(6);
if (hb.type != (byte)MAVLink.MAV_TYPE.ANTENNA_TRACKER && hb.type != (byte)MAVLink.MAV_TYPE.GCS)
{
SetupMavConnect(buffer[3], buffer[4], buffer[2], hb);
break;
}
hb = buffer1.ByteArrayToStructure<mavlink_heartbeat_t>(6);
if (hb.type != (byte)MAVLink.MAV_TYPE.ANTENNA_TRACKER && hb.type != (byte)MAVLink.MAV_TYPE.GCS)
{
SetupMavConnect(buffer1[3], buffer1[4], buffer1[2], hb);
break;
}
}
}
countDown.Stop();
frmProgressReporter.UpdateProgressAndStatus(0, "Getting Params.. (sysid " + MAV.sysid + " compid " + MAV.compid + ") ");
if (getparams)
{
getParamListBG();
}
if (frmProgressReporter.doWorkArgs.CancelAcknowledged == true)
{
giveComport = false;
if (BaseStream.IsOpen)
BaseStream.Close();
return;
}
}
catch (Exception e)
{
try
{
BaseStream.Close();
}
catch { }
giveComport = false;
if (string.IsNullOrEmpty(progressWorkerEventArgs.ErrorMessage))
progressWorkerEventArgs.ErrorMessage = "Connect Failed";
log.Error(e);
throw;
}
//frmProgressReporter.Close();
giveComport = false;
frmProgressReporter.UpdateProgressAndStatus(100, "Done.");
log.Info("Done open " + MAV.sysid + " " + MAV.compid);
packetslost = 0;
synclost = 0;
}
private void OpenBg(object PRsender, bool getparams, ProgressWorkerEventArgs progressWorkerEventArgs)
{
frmProgressReporter.UpdateProgressAndStatus(-1, Strings.MavlinkConnecting);
giveComport = true;
if (BaseStream is SerialPort)
{
// allow settings to settle - previous dtr
System.Threading.Thread.Sleep(1000);
}
Terrain = new TerrainFollow(this);
bool hbseen = false;
try
{
BaseStream.ReadBufferSize = 16*1024;
lock (objlock) // so we dont have random traffic
{
log.Info("Open port with " + BaseStream.PortName + " " + BaseStream.BaudRate);
BaseStream.Open();
BaseStream.DiscardInBuffer();
// other boards seem to have issues if there is no delay? posible bootloader timeout issue
Thread.Sleep(1000);
}
MAVLinkMessage buffer = new MAVLinkMessage();
MAVLinkMessage buffer1 = new MAVLinkMessage();
DateTime start = DateTime.Now;
DateTime deadline = start.AddSeconds(CONNECT_TIMEOUT_SECONDS);
var countDown = new System.Timers.Timer {Interval = 1000, AutoReset = false};
countDown.Elapsed += (sender, e) =>
{
int secondsRemaining = (deadline - e.SignalTime).Seconds;
frmProgressReporter.UpdateProgressAndStatus(-1, string.Format(Strings.Trying, secondsRemaining));
if (secondsRemaining > 0) countDown.Start();
};
countDown.Start();
// px4 native
BaseStream.WriteLine("sh /etc/init.d/rc.usb");
int count = 0;
while (true)
{
if (progressWorkerEventArgs.CancelRequested)
{
progressWorkerEventArgs.CancelAcknowledged = true;
countDown.Stop();
if (BaseStream.IsOpen)
BaseStream.Close();
giveComport = false;
return;
}
log.Info(DateTime.Now.Millisecond + " Start connect loop ");
if (DateTime.Now > deadline)
{
//if (Progress != null)
// Progress(-1, "No Heartbeat Packets");
countDown.Stop();
this.Close();
if (hbseen)
{
progressWorkerEventArgs.ErrorMessage = Strings.Only1Hb;
throw new Exception(Strings.Only1HbD);
}
else
{
progressWorkerEventArgs.ErrorMessage = "No Heartbeat Packets Received";
throw new Exception(@"Can not establish a connection\n
Please check the following
1. You have firmware loaded
2. You have the correct serial port selected
3. PX4 - You have the microsd card installed
4. Try a diffrent usb port\n\n" +
"No Mavlink Heartbeat Packets where read from this port - Verify Baud Rate and setup\nMission Planner waits for 2 valid heartbeat packets before connecting");
}
}
System.Threading.Thread.Sleep(1);
// can see 2 heartbeat packets at any time, and will connect - was one after the other
if (buffer.Length == 0)
buffer = getHeartBeat();
System.Threading.Thread.Sleep(1);
if (buffer1.Length == 0)
buffer1 = getHeartBeat();
if (buffer.Length > 0 || buffer1.Length > 0)
hbseen = true;
count++;
// 2 hbs that match
if (buffer.Length > 5 && buffer1.Length > 5 && buffer.sysid == buffer1.sysid && buffer.compid == buffer1.compid)
{
mavlink_heartbeat_t hb = buffer.ToStructure<mavlink_heartbeat_t>();
if (hb.type != (byte) MAVLink.MAV_TYPE.GCS)
{
SetupMavConnect(buffer, hb);
break;
}
}
// 2 hb's that dont match. more than one sysid here
if (buffer.Length > 5 && buffer1.Length > 5 && (buffer.sysid == buffer1.sysid || buffer.compid == buffer1.compid))
{
mavlink_heartbeat_t hb = buffer.ToStructure<mavlink_heartbeat_t>();
if (hb.type != (byte) MAVLink.MAV_TYPE.ANTENNA_TRACKER && hb.type != (byte) MAVLink.MAV_TYPE.GCS)
{
SetupMavConnect(buffer, hb);
break;
}
hb = buffer1.ToStructure<mavlink_heartbeat_t>();
if (hb.type != (byte) MAVLink.MAV_TYPE.ANTENNA_TRACKER && hb.type != (byte) MAVLink.MAV_TYPE.GCS)
{
SetupMavConnect(buffer1, hb);
break;
}
}
}
countDown.Stop();
getVersion();
frmProgressReporter.UpdateProgressAndStatus(0,
"Getting Params.. (sysid " + MAV.sysid + " compid " + MAV.compid + ") ");
if (getparams)
{
getParamListBG();
}
byte[] temp = ASCIIEncoding.ASCII.GetBytes("Mission Planner " + Application.ProductVersion + "\0");
Array.Resize(ref temp, 50);
generatePacket((byte) MAVLINK_MSG_ID.STATUSTEXT,
new mavlink_statustext_t() {severity = (byte) MAV_SEVERITY.INFO, text = temp});
if (frmProgressReporter.doWorkArgs.CancelAcknowledged == true)
{
giveComport = false;
if (BaseStream.IsOpen)
BaseStream.Close();
return;
}
}
catch (Exception e)
{
try
{
BaseStream.Close();
}
catch
{
}
giveComport = false;
if (string.IsNullOrEmpty(progressWorkerEventArgs.ErrorMessage))
progressWorkerEventArgs.ErrorMessage = Strings.ConnectFailed;
log.Error(e);
throw;
}
//frmProgressReporter.Close();
giveComport = false;
frmProgressReporter.UpdateProgressAndStatus(100, Strings.Done);
log.Info("Done open " + MAV.sysid + " " + MAV.compid);
MAV.packetslost = 0;
MAV.synclost = 0;
}
public FlightData()
{
log.Info("Ctor Start");
InitializeComponent();
log.Info("Components Done");
instance = this;
// _serializer = new DockStateSerializer(dockContainer1);
// _serializer.SavePath = Application.StartupPath + Path.DirectorySeparatorChar + "FDscreen.xml";
// dockContainer1.PreviewRenderer = new PreviewRenderer();
//
mymap = gMapControl1;
myhud = hud1;
MainHcopy = MainH;
mymap.Paint += mymap_Paint;
// mymap.Manager.UseMemoryCache = false;
log.Info("Tunning Graph Settings");
// setup default tuning graph
if (Settings.Instance["Tuning_Graph_Selected"] != null)
{
string line = Settings.Instance["Tuning_Graph_Selected"].ToString();
string[] lines = line.Split(new[] {'|'}, StringSplitOptions.RemoveEmptyEntries);
foreach (string option in lines)
{
using (var cb = new CheckBox {Name = option, Checked = true})
{
chk_box_CheckedChanged(cb, EventArgs.Empty);
}
}
}
else
{
using (var cb = new CheckBox {Name = "roll", Checked = true})
{
chk_box_CheckedChanged(cb, EventArgs.Empty);
}
using (var cb = new CheckBox {Name = "pitch", Checked = true})
{
chk_box_CheckedChanged(cb, EventArgs.Empty);
}
using (var cb = new CheckBox {Name = "nav_roll", Checked = true})
{
chk_box_CheckedChanged(cb, EventArgs.Empty);
}
using (var cb = new CheckBox {Name = "nav_pitch", Checked = true})
{
chk_box_CheckedChanged(cb, EventArgs.Empty);
}
}
if (!string.IsNullOrEmpty(Settings.Instance["hudcolor"]))
{
hud1.hudcolor = Color.FromName(Settings.Instance["hudcolor"]);
}
log.Info("HUD Settings");
foreach (string item in Settings.Instance.Keys)
{
if (item.StartsWith("hud1_useritem_"))
{
string selection = item.Replace("hud1_useritem_", "");
CheckBox chk = new CheckBox();
chk.Name = selection;
chk.Checked = true;
HUD.Custom cust = new HUD.Custom();
cust.Header = Settings.Instance[item];
HUD.Custom.src = MainV2.comPort.MAV.cs;
addHudUserItem(ref cust, chk);
}
}
List<string> list = new List<string>();
{
list.Add("LOITER_UNLIM");
list.Add("RETURN_TO_LAUNCH");
list.Add("PREFLIGHT_CALIBRATION");
list.Add("MISSION_START");
list.Add("PREFLIGHT_REBOOT_SHUTDOWN");
list.Add("Trigger Camera NOW");
//DO_SET_SERVO
//DO_REPEAT_SERVO
}
CMB_action.DataSource = list;
CMB_modes.DataSource = Common.getModesList(MainV2.comPort.MAV.cs);
CMB_modes.ValueMember = "Key";
CMB_modes.DisplayMember = "Value";
//default to auto
CMB_modes.Text = "Auto";
CMB_setwp.SelectedIndex = 0;
log.Info("Graph Setup");
CreateChart(zg1);
// config map
log.Info("Map Setup");
gMapControl1.CacheLocation = Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar +
"gmapcache" + Path.DirectorySeparatorChar;
gMapControl1.MapProvider = GMapProviders.GoogleSatelliteMap;
gMapControl1.MinZoom = 0;
gMapControl1.MaxZoom = 24;
gMapControl1.Zoom = 3;
gMapControl1.OnMapZoomChanged += gMapControl1_OnMapZoomChanged;
gMapControl1.KeyPress += GMapControl1_KeyPress;
gMapControl1.DisableFocusOnMouseEnter = true;
gMapControl1.OnMarkerEnter += gMapControl1_OnMarkerEnter;
gMapControl1.OnMarkerLeave += gMapControl1_OnMarkerLeave;
gMapControl1.RoutesEnabled = true;
gMapControl1.PolygonsEnabled = true;
tfrpolygons = new GMapOverlay("tfrpolygons");
gMapControl1.Overlays.Add(tfrpolygons);
kmlpolygons = new GMapOverlay("kmlpolygons");
gMapControl1.Overlays.Add(kmlpolygons);
geofence = new GMapOverlay("geofence");
gMapControl1.Overlays.Add(geofence);
polygons = new GMapOverlay("polygons");
gMapControl1.Overlays.Add(polygons);
routes = new GMapOverlay("routes");
gMapControl1.Overlays.Add(routes);
rallypointoverlay = new GMapOverlay("rally points");
gMapControl1.Overlays.Add(rallypointoverlay);
gMapControl1.Overlays.Add(poioverlay);
float gspeedMax = Settings.Instance.GetFloat("GspeedMAX");
if (gspeedMax != 0)
{
Gspeed.MaxValue = gspeedMax;
}
MainV2.comPort.ParamListChanged += FlightData_ParentChanged;
MainV2.AdvancedChanged += MainV2_AdvancedChanged;
// first run
MainV2_AdvancedChanged(null, null);
TerrainFollow lFollow = new TerrainFollow(MainV2.comPort);
}
