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