public bool ReadHeartBeat(int timeout = 2000) { DateTime deadline = DateTime.Now.AddMilliseconds(timeout); while (DateTime.Now < deadline) { try { var packet = _mavlinkparse.ReadPacket(_datastream); if (packet == null) { continue; } else if (packet.GetType().ToString() == "MAVLink+mavlink_heartbeat_t") { MAVLink.mavlink_heartbeat_t _heartbeatpack = (MAVLink.mavlink_heartbeat_t)packet; Vehicle.HeartBeating = true; Vehicle.Type = _heartbeatpack.type; Vehicle.State = _heartbeatpack.system_status; Vehicle.AutoPilot = _heartbeatpack.autopilot; return(true); } } catch { Vehicle.HeartBeating = false; Vehicle.Connected = false; } } Vehicle.HeartBeating = false; return(false); }
private string GetLabel(object row, string fieldName) { if (row is MAVLink.mavlink_heartbeat_t) { MAVLink.mavlink_heartbeat_t heartbeat = (MAVLink.mavlink_heartbeat_t)row; if (fieldName == "base_mode") { return(GetModeFlags(heartbeat.base_mode)); } else if (fieldName == "system_status") { return(GetSystemStateName(heartbeat.system_status)); } else if (fieldName == "custom_mode") { return(GetCustomModeFlags(heartbeat.custom_mode)); } } else if (row is MAVLink.mavlink_extended_sys_state_t) { MAVLink.mavlink_extended_sys_state_t state = (MAVLink.mavlink_extended_sys_state_t)row; if (fieldName == "landed_state") { return(GetLandedStateName(state.landed_state)); } else if (fieldName == "vtol_state") { return(GetVTolStateName(state.vtol_state)); } } return(null); }
public void CheckMavLinkMessageSerialization() { MavLinkMessage sampleMessage = createSampleMessage(); MAVLink.mavlink_heartbeat_t sampleStruct = (MAVLink.mavlink_heartbeat_t)sampleMessage.data_struct; JsonSerializerSettings settings = new JsonSerializerSettings { TypeNameHandling = TypeNameHandling.All }; String serialized = JsonConvert.SerializeObject(sampleMessage, settings); MavLinkMessage deserialized = JsonConvert.DeserializeObject <MavLinkMessage>(serialized, settings); MAVLink.mavlink_heartbeat_t dstruct = (MAVLink.mavlink_heartbeat_t)deserialized.data_struct; Assert.AreEqual(sampleMessage.compid, deserialized.compid); Assert.AreEqual(sampleMessage.sysid, deserialized.sysid); Assert.AreEqual(sampleMessage.seq, deserialized.seq); Assert.AreEqual(sampleMessage.messid, deserialized.messid); Assert.AreEqual(sampleStruct.autopilot, dstruct.autopilot); Assert.AreEqual(sampleStruct.base_mode, dstruct.base_mode); Assert.AreEqual(sampleStruct.custom_mode, dstruct.custom_mode); Assert.AreEqual(sampleStruct.mavlink_version, dstruct.mavlink_version); Assert.AreEqual(sampleStruct.system_status, dstruct.system_status); Assert.AreEqual(sampleStruct.type, dstruct.type); }
public void CheckHeartbeatObject() { MavLinkMessage message = createSampleMessage(); MAVLink.mavlink_heartbeat_t sampleStruct = (MAVLink.mavlink_heartbeat_t)message.data_struct; Heartbeat heartbeatContainer = new Heartbeat(message); Assert.AreEqual((MAVLink.MAV_AUTOPILOT)sampleStruct.autopilot, heartbeatContainer.autopilot); //Assert.AreEqual((MAVLink.MAV_MODE_FLAG)sampleStruct.base_mode, heartbeatContainer.base_mode); Assert.AreEqual(sampleStruct.custom_mode, heartbeatContainer.custom_mode); Assert.AreEqual(sampleStruct.mavlink_version, heartbeatContainer.mavlink_version); Assert.AreEqual((MAVLink.MAV_STATE)sampleStruct.system_status, heartbeatContainer.system_status); Assert.AreEqual((MAVLink.MAV_TYPE)sampleStruct.type, heartbeatContainer.type); HeartbeatDTO dto = DTOFactory.createHeartbeatDTO(heartbeatContainer); Assert.AreEqual(dto.autopilot, heartbeatContainer.autopilot.ToString()); Assert.AreEqual(dto.base_mode.Count, heartbeatContainer.base_mode.Count); Assert.AreEqual(dto.custom_mode, heartbeatContainer.custom_mode); Assert.AreEqual(dto.mavlink_version, heartbeatContainer.mavlink_version); Assert.AreEqual(dto.system_status, heartbeatContainer.system_status.ToString()); Assert.AreEqual(dto.type, heartbeatContainer.type.ToString()); String json = JsonConvert.SerializeObject(dto); }
string GetLog(ushort no) { MainV2.comPort.Progress += comPort_Progress; status = serialstatus.Reading; // get df log from mav var ms = MainV2.comPort.GetLog(no); status = serialstatus.Done; updateDisplay(); MainV2.comPort.Progress -= comPort_Progress; // set log fn byte[] hbpacket = MainV2.comPort.getHeartBeat(); MAVLink.mavlink_heartbeat_t hb = (MAVLink.mavlink_heartbeat_t)MainV2.comPort.DebugPacket(hbpacket); logfile = MainV2.LogDir + Path.DirectorySeparatorChar + MainV2.comPort.MAV.aptype.ToString() + Path.DirectorySeparatorChar + hbpacket[3] + Path.DirectorySeparatorChar + DateTime.Now.ToString("yyyy-MM-dd HH-mm-ss") + " " + no + ".bin"; // make log dir Directory.CreateDirectory(Path.GetDirectoryName(logfile)); // save memorystream to file using (BinaryWriter bw = new BinaryWriter(File.OpenWrite(logfile))) { bw.Write(ms.ToArray()); } // create ascii log BinaryLog.ConvertBin(logfile, logfile + ".log"); //update the new filename logfile = logfile + ".log"; // get gps time of assci log DateTime logtime = DFLog.GetFirstGpsTime(logfile); // rename log is we have a valid gps time if (logtime != DateTime.MinValue) { string newlogfilename = MainV2.LogDir + Path.DirectorySeparatorChar + MainV2.comPort.MAV.aptype.ToString() + Path.DirectorySeparatorChar + hbpacket[3] + Path.DirectorySeparatorChar + logtime.ToString("yyyy-MM-dd HH-mm-ss") + ".log"; try { File.Move(logfile, newlogfilename); // rename bin as well File.Move(logfile.Replace(".log", ""), newlogfilename.Replace(".log", ".bin")); logfile = newlogfilename; } catch { CustomMessageBox.Show("Failed to rename file " + logfile + "\nto " + newlogfilename, "Error"); } } return(logfile); }
string GetLog(ushort no) { MainV2.comPort.Progress += comPort_Progress; status = serialstatus.Reading; // get df log from mav var ms = MainV2.comPort.GetLog(no); status = serialstatus.Done; MainV2.comPort.Progress -= comPort_Progress; // set log fn byte[] hbpacket = MainV2.comPort.getHeartBeat(); MAVLink.mavlink_heartbeat_t hb = (MAVLink.mavlink_heartbeat_t)MainV2.comPort.DebugPacket(hbpacket); logfile = MainV2.LogDir + Path.DirectorySeparatorChar + MainV2.comPort.MAV.aptype.ToString() + Path.DirectorySeparatorChar + hbpacket[3] + Path.DirectorySeparatorChar + DateTime.Now.ToString("yyyy-MM-dd HH-mm") + " " + no + ".bin"; // make log dir Directory.CreateDirectory(Path.GetDirectoryName(logfile)); // save memorystream to file using (BinaryWriter bw = new BinaryWriter(File.OpenWrite(logfile))) { bw.Write(ms.ToArray()); } // read binary log to assci log var temp1 = Log.BinaryLog.ReadLog(logfile); // delete binary log file //File.Delete(logfile); logfile = logfile + ".log"; // write assci log File.WriteAllLines(logfile, temp1); // get gps time of assci log DateTime logtime = DFLog.GetFirstGpsTime(logfile); // rename log is we have a valid gps time if (logtime != DateTime.MinValue) { string newlogfilename = MainV2.LogDir + Path.DirectorySeparatorChar + MainV2.comPort.MAV.aptype.ToString() + Path.DirectorySeparatorChar + hbpacket[3] + Path.DirectorySeparatorChar + logtime.ToString("yyyy-MM-dd HH-mm") + ".log"; File.Move(logfile, newlogfilename); // rename bin as well File.Move(logfile.Replace(".log", ""), newlogfilename.Replace(".log", ".bin")); logfile = newlogfilename; } return(logfile); }
private void sendHeartbeat() { MAVLink.mavlink_heartbeat_t heartbeat = new MAVLink.mavlink_heartbeat_t(); heartbeat.autopilot = (byte)MAVLink.MAV_AUTOPILOT.INVALID; heartbeat.type = (byte)MAVLink.MAV_TYPE.GCS; byte[] packet = this.mavlinkParse.GenerateMAVLinkPacket(MAVLink.MAVLINK_MSG_ID.HEARTBEAT, heartbeat); this.port.Write(packet, 0, packet.Length); }
public static void Heartbeat() { // Straight from mission planner MAVLink.mavlink_heartbeat_t hb = new MAVLink.mavlink_heartbeat_t(); hb.autopilot = (byte)MAVLink.MAV_AUTOPILOT.INVALID; hb.type = (byte)MAVLink.MAV_TYPE.GCS; hb.mavlink_version = 3; byte[] commandBytes = mavlink.GenerateMAVLinkPacket(MAVLink.MAVLINK_MSG_ID.HEARTBEAT, hb); SendPacket(commandBytes); }
MavLinkMessage createSampleMessage() { MAVLink.mavlink_heartbeat_t sampleStruct = new MAVLink.mavlink_heartbeat_t(); sampleStruct.autopilot = 1; sampleStruct.base_mode = 2; sampleStruct.custom_mode = 3; sampleStruct.mavlink_version = 4; sampleStruct.system_status = 5; sampleStruct.type = 6; MavLinkMessage sampleMessage = createSampleMessage(MAVLink.MAVLINK_MSG_ID.HEARTBEAT, sampleStruct); return(sampleMessage); }
void TestSend2(object sender, DoWorkEventArgs e)//sender与e为开启单独线程所用 { while (serialPort1.IsOpen) { if (DateTime.Now > Time4TestSend) { MAVLink.mavlink_heartbeat_t req2 = new MAVLink.mavlink_heartbeat_t(); byte[] packet2 = mavlink.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.HEARTBEAT, req2); serialPort1.Write(packet2, 0, packet2.Length); Time4TestSend = DateTime.Now.AddSeconds(1); } } }
public Heartbeat(MavLinkMessage message) : base(null) { if (message.messid == this.MessageID) { // this code is faster... run it and pass null to base message for speed increase // base message uses reflection MAVLink.mavlink_heartbeat_t raw_data = (MAVLink.mavlink_heartbeat_t)message.data_struct; type = (MAVLink.MAV_TYPE)raw_data.type; autopilot = (MAVLink.MAV_AUTOPILOT)raw_data.autopilot; custom_mode = raw_data.custom_mode; base_mode = Utilities.BitwiseOperations.parseBitValues <MAVLink.MAV_MODE_FLAG>(raw_data.base_mode); system_status = (MAVLink.MAV_STATE)raw_data.system_status; mavlink_version = (int)raw_data.mavlink_version; } }
/// <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(); }
private void serialReader() { heartBeatSend = DateTime.Now; serialReaderThreadRun = true; int minBytesToRead = 0; while (serialReaderThreadRun) { try { Thread.Sleep(1); // Send heart beat every second if (heartBeatSend.Second != DateTime.Now.Second) { MAVLink.mavlink_heartbeat_t hb = new MAVLink.mavlink_heartbeat_t() { type = (byte)MAVLink.MAV_TYPE.GCS, autopilot = (byte)MAVLink.MAV_AUTOPILOT.INVALID, mavlink_version = 3, }; comPort.sendPacket(hb); heartBeatSend = DateTime.Now; } while (comPort.BaseStream.IsOpen && comPort.BaseStream.BytesToRead > minBytesToRead) { try { comPort.ReadPacket(); } catch { throw; } } // update current state try { comPort.cs.UpdateCurrentSettings(null, false, comPort); } catch { throw; } } catch (Exception e) { // log serial reader fail try { comPort.Close(); } catch { } } } }
public static void Start(string[] args) { Program.args = args; Console.WriteLine( "If your error is about Microsoft.DirectX.DirectInput, please install the latest directx redist from here http://www.microsoft.com/en-us/download/details.aspx?id=35 \n\n"); Console.WriteLine("Debug under mono MONO_LOG_LEVEL=debug mono MissionPlanner.exe"); Console.WriteLine("Data Dir " + Settings.GetDataDirectory()); Console.WriteLine("Log Dir " + Settings.GetDefaultLogDir()); Console.WriteLine("Running Dir " + Settings.GetRunningDirectory()); Console.WriteLine("User Data Dir " + Settings.GetUserDataDirectory()); var t = Type.GetType("Mono.Runtime"); MONO = (t != null); Thread = Thread.CurrentThread; System.Windows.Forms.Application.EnableVisualStyles(); XmlConfigurator.Configure(); log.Info("******************* Logging Configured *******************"); ServicePointManager.DefaultConnectionLimit = 10; System.Windows.Forms.Application.ThreadException += Application_ThreadException; // fix ssl on mono ServicePointManager.ServerCertificateValidationCallback = new System.Net.Security.RemoteCertificateValidationCallback( (sender, certificate, chain, policyErrors) => { return(true); }); if (args.Length > 0 && args[0] == "/update") { Utilities.Update.DoUpdate(); return; } name = "Mission Planner"; try { if (File.Exists(Settings.GetRunningDirectory() + "logo.txt")) { name = File.ReadAllLines(Settings.GetRunningDirectory() + "logo.txt", Encoding.UTF8)[0]; } } catch { } if (File.Exists(Settings.GetRunningDirectory() + "logo.png")) { Logo = new Bitmap(Settings.GetRunningDirectory() + "logo.png"); } if (File.Exists(Settings.GetRunningDirectory() + "logo2.png")) { Logo2 = new Bitmap(Settings.GetRunningDirectory() + "logo2.png"); } if (File.Exists(Settings.GetRunningDirectory() + "icon.png")) { // 128*128 IconFile = new Bitmap(Settings.GetRunningDirectory() + "icon.png"); } else { IconFile = MissionPlanner.Properties.Resources.mpdesktop.ToBitmap(); } if (File.Exists(Settings.GetRunningDirectory() + "splashbg.png")) // 600*375 { SplashBG = new Bitmap(Settings.GetRunningDirectory() + "splashbg.png"); } Splash = new MissionPlanner.Splash(); if (SplashBG != null) { Splash.BackgroundImage = SplashBG; Splash.pictureBox1.Visible = false; } if (IconFile != null) { Splash.Icon = Icon.FromHandle(((Bitmap)IconFile).GetHicon()); } string strVersion = File.Exists("version.txt") ? File.ReadAllText("version.txt") : System.Reflection.Assembly.GetExecutingAssembly().GetName().Version.ToString(); Splash.Text = name + " " + Application.ProductVersion + " build " + strVersion; Splash.Show(); Application.DoEvents(); Application.DoEvents(); CustomMessageBox.ShowEvent += (text, caption, buttons, icon) => { return((CustomMessageBox.DialogResult)(int) MsgBox.CustomMessageBox.Show(text, caption, (MessageBoxButtons)(int)buttons, (MessageBoxIcon)(int)icon)); }; // setup theme provider MsgBox.CustomMessageBox.ApplyTheme += MissionPlanner.Utilities.ThemeManager.ApplyThemeTo; Controls.MainSwitcher.ApplyTheme += MissionPlanner.Utilities.ThemeManager.ApplyThemeTo; MissionPlanner.Controls.InputBox.ApplyTheme += MissionPlanner.Utilities.ThemeManager.ApplyThemeTo; Controls.BackstageView.BackstageViewPage.ApplyTheme += MissionPlanner.Utilities.ThemeManager.ApplyThemeTo; Controls.MainSwitcher.Tracking += MissionPlanner.Utilities.Tracking.AddPage; Controls.BackstageView.BackstageView.Tracking += MissionPlanner.Utilities.Tracking.AddPage; // setup settings provider MissionPlanner.Comms.CommsBase.Settings += CommsBase_Settings; MissionPlanner.Comms.CommsBase.InputBoxShow += CommsBaseOnInputBoxShow; MissionPlanner.Comms.CommsBase.ApplyTheme += MissionPlanner.Utilities.ThemeManager.ApplyThemeTo; // set the cache provider to my custom version GMap.NET.GMaps.Instance.PrimaryCache = new Maps.MyImageCache(); // add my custom map providers GMap.NET.MapProviders.GMapProviders.List.Add(Maps.WMSProvider.Instance); GMap.NET.MapProviders.GMapProviders.List.Add(Maps.Custom.Instance); GMap.NET.MapProviders.GMapProviders.List.Add(Maps.Earthbuilder.Instance); GMap.NET.MapProviders.GMapProviders.List.Add(Maps.Statkart_Topo2.Instance); GMap.NET.MapProviders.GMapProviders.List.Add(Maps.Eniro_Topo.Instance); GMap.NET.MapProviders.GMapProviders.List.Add(Maps.MapBox.Instance); GMap.NET.MapProviders.GMapProviders.List.Add(Maps.MapboxNoFly.Instance); GMap.NET.MapProviders.GMapProviders.List.Add(Maps.Japan.Instance); GMap.NET.MapProviders.GMapProviders.List.Add(Maps.Japan_Lake.Instance); GMap.NET.MapProviders.GMapProviders.List.Add(Maps.Japan_1974.Instance); GMap.NET.MapProviders.GMapProviders.List.Add(Maps.Japan_1979.Instance); GMap.NET.MapProviders.GMapProviders.List.Add(Maps.Japan_1984.Instance); GMap.NET.MapProviders.GMapProviders.List.Add(Maps.Japan_1988.Instance); GMap.NET.MapProviders.GMapProviders.List.Add(Maps.Japan_Relief.Instance); GMap.NET.MapProviders.GMapProviders.List.Add(Maps.Japan_Slopezone.Instance); GMap.NET.MapProviders.GMapProviders.List.Add(Maps.Japan_Sea.Instance); GoogleMapProvider.APIKey = "AIzaSyA5nFp39fEHruCezXnG3r8rGyZtuAkmCug"; // optionally add gdal support if (Directory.Exists(Application.StartupPath + Path.DirectorySeparatorChar + "gdal")) { GMap.NET.MapProviders.GMapProviders.List.Add(GDAL.GDALProvider.Instance); } // add proxy settings GMap.NET.MapProviders.GMapProvider.WebProxy = WebRequest.GetSystemWebProxy(); GMap.NET.MapProviders.GMapProvider.WebProxy.Credentials = CredentialCache.DefaultCredentials; // generic status report screen MAVLinkInterface.CreateIProgressReporterDialogue += title => new ProgressReporterDialogue() { StartPosition = FormStartPosition.CenterScreen, Text = title }; WebRequest.DefaultWebProxy = WebRequest.GetSystemWebProxy(); WebRequest.DefaultWebProxy.Credentials = CredentialCache.DefaultNetworkCredentials; if (name == "VVVVZ") { // set pw Settings.Instance["password"] = "******"; Settings.Instance["password_protect"] = "True"; // prevent wizard Settings.Instance["newuser"] = "******"; // invalidate update url System.Configuration.ConfigurationManager.AppSettings["UpdateLocationVersion"] = ""; } CleanupFiles(); //LoadDlls(); log.InfoFormat("64bit os {0}, 64bit process {1}", System.Environment.Is64BitOperatingSystem, System.Environment.Is64BitProcess); Device.DeviceStructure test1 = new Device.DeviceStructure(73225); Device.DeviceStructure test2 = new Device.DeviceStructure(262434); Device.DeviceStructure test3 = new Device.DeviceStructure(131874); //ph2 - cube with here Device.DeviceStructure test5 = new Device.DeviceStructure(466441); Device.DeviceStructure test6 = new Device.DeviceStructure(131874); Device.DeviceStructure test7 = new Device.DeviceStructure(263178); // Device.DeviceStructure test8 = new Device.DeviceStructure(1442082); Device.DeviceStructure test9 = new Device.DeviceStructure(1114914); Device.DeviceStructure test10 = new Device.DeviceStructure(1442826); // Device.DeviceStructure test11 = new Device.DeviceStructure(2359586); Device.DeviceStructure test12 = new Device.DeviceStructure(2229282); Device.DeviceStructure test13 = new Device.DeviceStructure(2360330); Device.DeviceStructure test21 = new Device.DeviceStructure(592905); Device.DeviceStructure test22 = new Device.DeviceStructure(131874); Device.DeviceStructure test23 = new Device.DeviceStructure(263178); MAVLink.MavlinkParse tmp = new MAVLink.MavlinkParse(); MAVLink.mavlink_heartbeat_t hb = new MAVLink.mavlink_heartbeat_t() { autopilot = 1, base_mode = 2, custom_mode = 3, mavlink_version = 2, system_status = 6, type = 7 }; var t1 = tmp.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.HEARTBEAT, hb); var t2 = tmp.GenerateMAVLinkPacket20(MAVLink.MAVLINK_MSG_ID.HEARTBEAT, hb); tmp.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.HEARTBEAT, hb); tmp.GenerateMAVLinkPacket20(MAVLink.MAVLINK_MSG_ID.HEARTBEAT, hb); tmp.GenerateMAVLinkPacket20(MAVLink.MAVLINK_MSG_ID.HEARTBEAT, hb, true); tmp.GenerateMAVLinkPacket20(MAVLink.MAVLINK_MSG_ID.HEARTBEAT, hb, true); var msg = new MAVLink.MAVLinkMessage(t2); try { //System.Diagnostics.Process.GetCurrentProcess().PriorityClass = System.Diagnostics.ProcessPriorityClass.RealTime; Thread.CurrentThread.Name = "Base Thread"; Application.Run(new MainV2()); } catch (Exception ex) { log.Fatal("Fatal app exception", ex); Console.WriteLine(ex.ToString()); Console.WriteLine("\nPress any key to exit!"); Console.ReadLine(); } try { // kill sim background process if its still running if (Controls.SITL.simulator != null) { Controls.SITL.simulator.Kill(); } } catch { } }
public static void SortLogs(string[] logs) { foreach (var logfile in logs) { FileInfo info = new FileInfo(logfile); if (info.Length == 0) { try { File.Delete(logfile); } catch { } continue; } 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) { mine.logreadmode = false; mine.logplaybackfile.Close(); if (!Directory.Exists(Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + "BAD")) { Directory.CreateDirectory(Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + "BAD"); } File.Move(logfile, Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + "BAD" + Path.DirectorySeparatorChar + Path.GetFileName(logfile)); 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; } } }
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; } } }
/// <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(); }
private void SerialReader() { if (serialThread == true) return; serialThread = true; while (serialThread) { try { Thread.Sleep(1); // was 5 UpdateConnectIcon(); // 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// MAVLink.MAVLINK_VERSION }; try { comPort.sendPacket(htb); } catch (Exception ex) { log.Error(ex); // close the bad port comPort.Close(); } 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 if (comPort.BaseStream.IsOpen) { Console.WriteLine("Main comport shut, swapping to other mav"); break; } } System.Threading.Thread.Sleep(100); } if (comPort.BaseStream.IsOpen && comPort.BaseStream.BytesToRead > 0) comPort.readPacket(); // update currentstate of sysids on the port try { comPort.MAV.cs.UpdateCurrentSettings(null, false, comPort, comPort.MAV); } catch (Exception ex) { log.Error(ex); } } catch { }; } }
public static void Main(string[] args) { Program.args = args; Console.WriteLine( "If your error is about Microsoft.DirectX.DirectInput, please install the latest directx redist from here http://www.microsoft.com/en-us/download/details.aspx?id=35 \n\n"); Console.WriteLine("Debug under mono MONO_LOG_LEVEL=debug mono MissionPlanner.exe"); Thread = Thread.CurrentThread; System.Windows.Forms.Application.EnableVisualStyles(); XmlConfigurator.Configure(); log.Info("******************* Logging Configured *******************"); System.Windows.Forms.Application.SetCompatibleTextRenderingDefault(false); ServicePointManager.DefaultConnectionLimit = 10; System.Windows.Forms.Application.ThreadException += Application_ThreadException; AppDomain.CurrentDomain.UnhandledException += new UnhandledExceptionEventHandler(CurrentDomain_UnhandledException); // fix ssl on mono ServicePointManager.ServerCertificateValidationCallback = new System.Net.Security.RemoteCertificateValidationCallback( (sender, certificate, chain, policyErrors) => { return true; }); if (args.Length > 0 && args[0] == "/update") { Utilities.Update.DoUpdate(); return; } name = "Mission Planner"; try { if (File.Exists(Settings.GetRunningDirectory() + "logo.txt")) name = File.ReadAllLines(Settings.GetRunningDirectory() + "logo.txt", Encoding.UTF8)[0]; } catch { } if (File.Exists(Settings.GetRunningDirectory() + "logo.png")) Logo = new Bitmap(Settings.GetRunningDirectory() + "logo.png"); if (File.Exists(Settings.GetRunningDirectory() + "icon.png")) { // 128*128 IconFile = new Bitmap(Settings.GetRunningDirectory() + "icon.png"); } else { IconFile = MissionPlanner.Properties.Resources.mpdesktop.ToBitmap(); } if (File.Exists(Settings.GetRunningDirectory() + "splashbg.png")) // 600*375 SplashBG = new Bitmap(Settings.GetRunningDirectory() + "splashbg.png"); Splash = new MissionPlanner.Splash(); if (SplashBG != null) { Splash.BackgroundImage = SplashBG; Splash.pictureBox1.Visible = false; } if (IconFile != null) Splash.Icon = Icon.FromHandle(((Bitmap)IconFile).GetHicon()); string strVersion = System.Reflection.Assembly.GetExecutingAssembly().GetName().Version.ToString(); Splash.Text = name + " " + Application.ProductVersion + " build " + strVersion; Splash.Show(); Application.DoEvents(); Application.DoEvents(); // setup theme provider CustomMessageBox.ApplyTheme += MissionPlanner.Utilities.ThemeManager.ApplyThemeTo; Controls.MainSwitcher.ApplyTheme += MissionPlanner.Utilities.ThemeManager.ApplyThemeTo; MissionPlanner.Controls.InputBox.ApplyTheme += MissionPlanner.Utilities.ThemeManager.ApplyThemeTo; Controls.BackstageView.BackstageViewPage.ApplyTheme += MissionPlanner.Utilities.ThemeManager.ApplyThemeTo; // setup settings provider MissionPlanner.Comms.CommsBase.Settings += CommsBase_Settings; MissionPlanner.Comms.CommsBase.ApplyTheme += MissionPlanner.Utilities.ThemeManager.ApplyThemeTo; // set the cache provider to my custom version GMap.NET.GMaps.Instance.PrimaryCache = new Maps.MyImageCache(); // add my custom map providers GMap.NET.MapProviders.GMapProviders.List.Add(Maps.WMSProvider.Instance); GMap.NET.MapProviders.GMapProviders.List.Add(Maps.Custom.Instance); GMap.NET.MapProviders.GMapProviders.List.Add(Maps.Earthbuilder.Instance); GMap.NET.MapProviders.GMapProviders.List.Add(Maps.Statkart_Topo2.Instance); GMap.NET.MapProviders.GMapProviders.List.Add(Maps.MapBox.Instance); GMap.NET.MapProviders.GMapProviders.List.Add(Maps.MapboxNoFly.Instance); GMap.NET.MapProviders.GMapProviders.List.Add(GDAL.GDALProvider.Instance); // add proxy settings GMap.NET.MapProviders.GMapProvider.WebProxy = WebRequest.GetSystemWebProxy(); GMap.NET.MapProviders.GMapProvider.WebProxy.Credentials = CredentialCache.DefaultCredentials; WebRequest.DefaultWebProxy = WebRequest.GetSystemWebProxy(); WebRequest.DefaultWebProxy.Credentials = CredentialCache.DefaultNetworkCredentials; if (name == "VVVVZ") { // set pw Settings.Instance["password"] = "******"; Settings.Instance["password_protect"] = "True"; // prevent wizard Settings.Instance["newuser"] = "******"; // invalidate update url System.Configuration.ConfigurationManager.AppSettings["UpdateLocationVersion"] = ""; } CleanupFiles(); log.InfoFormat("64bit os {0}, 64bit process {1}", System.Environment.Is64BitOperatingSystem, System.Environment.Is64BitProcess); Device.DeviceStructure test1 = new Device.DeviceStructure(73225); Device.DeviceStructure test2 = new Device.DeviceStructure(262434); Device.DeviceStructure test3 = new Device.DeviceStructure(131874); MAVLink.MavlinkParse tmp = new MAVLink.MavlinkParse(); MAVLink.mavlink_heartbeat_t hb = new MAVLink.mavlink_heartbeat_t() { autopilot = 1, base_mode = 2, custom_mode = 3, mavlink_version = 2, system_status = 6, type = 7 }; var t1 = tmp.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.HEARTBEAT, hb); var t2 = tmp.GenerateMAVLinkPacket20(MAVLink.MAVLINK_MSG_ID.HEARTBEAT, hb); tmp.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.HEARTBEAT, hb); tmp.GenerateMAVLinkPacket20(MAVLink.MAVLINK_MSG_ID.HEARTBEAT, hb); tmp.GenerateMAVLinkPacket20(MAVLink.MAVLINK_MSG_ID.HEARTBEAT, hb, true); tmp.GenerateMAVLinkPacket20(MAVLink.MAVLINK_MSG_ID.HEARTBEAT, hb, true); try { //System.Diagnostics.Process.GetCurrentProcess().PriorityClass = System.Diagnostics.ProcessPriorityClass.RealTime; Thread.CurrentThread.Name = "Base Thread"; Application.Run(new MainV2()); } catch (Exception ex) { log.Fatal("Fatal app exception", ex); Console.WriteLine(ex.ToString()); Console.WriteLine("\nPress any key to exit!"); Console.ReadLine(); } try { // kill sim background process if its still running if (Controls.SITL.simulator != null) Controls.SITL.simulator.Kill(); } catch { } }
/// <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 linkqualitytime = DateTime.Now; while (serialThread) { try { Thread.Sleep(1); // was 5 try { { if (GCSViews.Terminal.comPort != null && GCSViews.Terminal.comPort.IsOpen) continue; } } catch { } // attenuate the link qualty over time if ((DateTime.Now - MainV2.comPort.MAV.lastvalidpacket).TotalSeconds >= 1) { if (linkqualitytime.Second != DateTime.Now.Second) { MainV2.comPort.MAV.cs.linkqualitygcs = (ushort)(MainV2.comPort.MAV.cs.linkqualitygcs * 0.8f); linkqualitytime = DateTime.Now; // force redraw is no other packets are being read // GCSViews.FlightData.myhud.Invalidate(); } } // data loss warning - wait min of 10 seconds, ignore first 30 seconds of connect, repeat at 5 seconds interval if ((DateTime.Now - MainV2.comPort.MAV.lastvalidpacket).TotalSeconds > 10 && (DateTime.Now - connecttime).TotalSeconds > 30 && (DateTime.Now - nodatawarning).TotalSeconds > 5 && (MainV2.comPort.logreadmode || comPort.BaseStream.IsOpen) && MainV2.comPort.MAV.cs.armed) // get home point on armed status change. if (armedstatus != MainV2.comPort.MAV.cs.armed && comPort.BaseStream.IsOpen) { armedstatus = MainV2.comPort.MAV.cs.armed; // status just changed to armed if (MainV2.comPort.MAV.cs.armed == true) { try { MainV2.comPort.MAV.cs.HomeLocation = new PointLatLngAlt(MainV2.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 { CustomMessageBox.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// MAVLink.MAVLINK_VERSION }; foreach (var port in Comports) { try { port.sendPacket(htb); } catch (Exception ex) { log.Error(ex); // close the bad port port.Close(); // refresh the screen if needed if (port == MainV2.comPort) { // refresh config window if needed if (MyView.current != null) { if (MyView.current.Name == "HWConfig") MyView.ShowScreen("HWConfig"); if (MyView.current.Name == "SWConfig") MyView.ShowScreen("SWConfig"); } } } } 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); } // read the interfaces foreach (var port in Comports) { if (!port.BaseStream.IsOpen) { // skip primary interface if (port == comPort) continue; // modify array and drop out Comports.Remove(port); break; } while (port.BaseStream.IsOpen && port.BaseStream.BytesToRead > minbytes && port.giveComport == false) { try { port.readPacket(); } catch (Exception ex) { log.Error(ex); } } // 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 (Exception ex) { log.Error(ex); } } } } catch (Exception e) { log.Error("Serial Reader fail :" + e.ToString()); try { comPort.Close(); } catch (Exception ex) { log.Error(ex); } } } Console.WriteLine("SerialReader Done"); SerialThreadrunner.Set(); }
/// <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 speechlowspeedtime = DateTime.Now; DateTime linkqualitytime = DateTime.Now; while (serialThread) { try { Thread.Sleep(1); // was 5 try { if (GCSViews.Terminal.comPort is MAVLinkSerialPort) { } else { if (GCSViews.Terminal.comPort != null && GCSViews.Terminal.comPort.IsOpen) continue; } } catch { } // update connect/disconnect button and info stats UpdateConnectIcon(); // 30 seconds interval speech options if (speechEnable && speechEngine != null && (DateTime.Now - speechcustomtime).TotalSeconds > 30 && (MainV2.comPort.logreadmode || comPort.BaseStream.IsOpen)) { if (MainV2.speechEngine.State == SynthesizerState.Ready) { if (MainV2.getConfig("speechcustomenabled") == "True") { MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechcustom"))); } speechcustomtime = DateTime.Now; } // speech for battery alerts //speechbatteryvolt float warnvolt = 0; float.TryParse(MainV2.getConfig("speechbatteryvolt"), out warnvolt); float warnpercent = 0; float.TryParse(MainV2.getConfig("speechbatterypercent"), out warnpercent); if (MainV2.getConfig("speechbatteryenabled") == "True" && MainV2.comPort.MAV.cs.battery_voltage <= warnvolt && MainV2.comPort.MAV.cs.battery_voltage >= 5.0) { if (MainV2.speechEngine.State == SynthesizerState.Ready) { MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechbattery"))); } } else if (MainV2.getConfig("speechbatteryenabled") == "True" && (MainV2.comPort.MAV.cs.battery_remaining) < warnpercent && MainV2.comPort.MAV.cs.battery_voltage >= 5.0 && MainV2.comPort.MAV.cs.battery_remaining != 0.0) { if (MainV2.speechEngine.State == SynthesizerState.Ready) { MainV2.speechEngine.SpeakAsync( Common.speechConversion(MainV2.getConfig("speechbattery"))); } } } // speech for airspeed alerts if (speechEnable && speechEngine != null && (DateTime.Now - speechlowspeedtime).TotalSeconds > 10 && (MainV2.comPort.logreadmode || comPort.BaseStream.IsOpen)) { if (MainV2.getConfig("speechlowspeedenabled") == "True" && MainV2.comPort.MAV.cs.armed) { float warngroundspeed = 0; float.TryParse(MainV2.getConfig("speechlowgroundspeedtrigger"), out warngroundspeed); float warnairspeed = 0; float.TryParse(MainV2.getConfig("speechlowairspeedtrigger"), out warnairspeed); if (MainV2.comPort.MAV.cs.airspeed < warnairspeed) { if (MainV2.speechEngine.State == SynthesizerState.Ready) { MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechlowairspeed"))); speechlowspeedtime = DateTime.Now; } } else if (MainV2.comPort.MAV.cs.groundspeed < warngroundspeed) { if (MainV2.speechEngine.State == SynthesizerState.Ready) { MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechlowgroundspeed"))); speechlowspeedtime = DateTime.Now; } } else { speechlowspeedtime = DateTime.Now; } } } // speech altitude warning - message high warning if (speechEnable && speechEngine != null && (MainV2.comPort.logreadmode || comPort.BaseStream.IsOpen)) { float warnalt = float.MaxValue; float.TryParse(MainV2.getConfig("speechaltheight"), out warnalt); try { int todo; // need a reset method altwarningmax = (int)Math.Max(MainV2.comPort.MAV.cs.alt, altwarningmax); if (MainV2.getConfig("speechaltenabled") == "True" && MainV2.comPort.MAV.cs.alt != 0.00 && (MainV2.comPort.MAV.cs.alt <= warnalt) && MainV2.comPort.MAV.cs.armed) { if (altwarningmax > warnalt) { if (MainV2.speechEngine.State == SynthesizerState.Ready) MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechalt"))); } } } catch { } // silent fail try { // say the latest high priority message if (MainV2.speechEngine.State == SynthesizerState.Ready && lastmessagehigh != MainV2.comPort.MAV.cs.messageHigh) { MainV2.speechEngine.SpeakAsync(MainV2.comPort.MAV.cs.messageHigh); lastmessagehigh = MainV2.comPort.MAV.cs.messageHigh; } } catch { } } // attenuate the link qualty over time if ((DateTime.Now - MainV2.comPort.MAV.lastvalidpacket).TotalSeconds >= 1) { if (linkqualitytime.Second != DateTime.Now.Second) { MainV2.comPort.MAV.cs.linkqualitygcs = (ushort)(MainV2.comPort.MAV.cs.linkqualitygcs * 0.8f); linkqualitytime = DateTime.Now; // force redraw is no other packets are being read GCSViews.FlightData.myhud.Invalidate(); } } // data loss warning - wait min of 10 seconds, ignore first 30 seconds of connect, repeat at 5 seconds interval if ((DateTime.Now - MainV2.comPort.MAV.lastvalidpacket).TotalSeconds > 10 && (DateTime.Now - connecttime).TotalSeconds > 30 && (DateTime.Now - nodatawarning).TotalSeconds > 5 && (MainV2.comPort.logreadmode || comPort.BaseStream.IsOpen) && MainV2.comPort.MAV.cs.armed) { if (speechEnable && speechEngine != null) { if (MainV2.speechEngine.State == SynthesizerState.Ready) { MainV2.speechEngine.SpeakAsync("WARNING No Data for " + (int)(DateTime.Now - MainV2.comPort.MAV.lastvalidpacket).TotalSeconds + " Seconds"); nodatawarning = DateTime.Now; } } } // get home point on armed status change. if (armedstatus != MainV2.comPort.MAV.cs.armed && comPort.BaseStream.IsOpen) { armedstatus = MainV2.comPort.MAV.cs.armed; // status just changed to armed if (MainV2.comPort.MAV.cs.armed == true) { try { MainV2.comPort.MAV.cs.HomeLocation = new PointLatLngAlt(MainV2.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 { CustomMessageBox.Show("Failed to update home location"); }); } } if (speechEnable && speechEngine != null) { if (MainV2.getConfig("speecharmenabled") == "True") { if (armedstatus) MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speecharm"))); else MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechdisarm"))); } } } // 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// MAVLink.MAVLINK_VERSION }; foreach (var port in Comports) { try { port.sendPacket(htb); } catch (Exception ex) { log.Error(ex); // close the bad port port.Close(); // refresh the screen if needed if (port == MainV2.comPort) { // refresh config window if needed if (MyView.current != null) { if (MyView.current.Name == "HWConfig") MyView.ShowScreen("HWConfig"); if (MyView.current.Name == "SWConfig") MyView.ShowScreen("SWConfig"); } } } } 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); } // read the interfaces foreach (var port in Comports) { if (!port.BaseStream.IsOpen) { // skip primary interface if (port == comPort) continue; // modify array and drop out Comports.Remove(port); break; } while (port.BaseStream.IsOpen && port.BaseStream.BytesToRead > minbytes && port.giveComport == false) { try { port.readPacket(); } catch (Exception ex) { log.Error(ex); } } // 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 (Exception ex) { log.Error(ex); } } } } catch (Exception e) { log.Error("Serial Reader fail :" + e.ToString()); try { comPort.Close(); } catch (Exception ex) { log.Error(ex); } } } Console.WriteLine("SerialReader Done"); SerialThreadrunner.Set(); }
/// <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 speechlowspeedtime = DateTime.Now; DateTime linkqualitytime = DateTime.Now; while (serialThread) { try { Thread.Sleep(1); // was 5 try { if (GCSViews.Terminal.comPort is MAVLinkSerialPort) { } else { if (GCSViews.Terminal.comPort != null && GCSViews.Terminal.comPort.IsOpen) continue; } } catch (Exception ex) { log.Error(ex); } // update connect/disconnect button and info stats try { UpdateConnectIcon(); } catch (Exception ex) { log.Error(ex); } // 30 seconds interval speech options if (speechEnable && speechEngine != null && (DateTime.Now - speechcustomtime).TotalSeconds > 30 && (MainV2.comPort.logreadmode || comPort.BaseStream.IsOpen)) { if (MainV2.speechEngine.IsReady) { if (Settings.Instance.GetBoolean("speechcustomenabled")) { MainV2.speechEngine.SpeakAsync(Common.speechConversion(""+ Settings.Instance["speechcustom"])); } speechcustomtime = DateTime.Now; } // speech for battery alerts //speechbatteryvolt float warnvolt = Settings.Instance.GetFloat("speechbatteryvolt"); float warnpercent = Settings.Instance.GetFloat("speechbatterypercent"); if (Settings.Instance.GetBoolean("speechbatteryenabled") == true && MainV2.comPort.MAV.cs.battery_voltage <= warnvolt && MainV2.comPort.MAV.cs.battery_voltage >= 5.0) { if (MainV2.speechEngine.IsReady) { MainV2.speechEngine.SpeakAsync(Common.speechConversion(""+ Settings.Instance["speechbattery"])); } } else if (Settings.Instance.GetBoolean("speechbatteryenabled") == true && (MainV2.comPort.MAV.cs.battery_remaining) < warnpercent && MainV2.comPort.MAV.cs.battery_voltage >= 5.0 && MainV2.comPort.MAV.cs.battery_remaining != 0.0) { if (MainV2.speechEngine.IsReady) { MainV2.speechEngine.SpeakAsync( Common.speechConversion("" + Settings.Instance["speechbattery"])); } } } // speech for airspeed alerts if (speechEnable && speechEngine != null && (DateTime.Now - speechlowspeedtime).TotalSeconds > 10 && (MainV2.comPort.logreadmode || comPort.BaseStream.IsOpen)) { if (Settings.Instance.GetBoolean("speechlowspeedenabled") == true && MainV2.comPort.MAV.cs.armed) { float warngroundspeed = Settings.Instance.GetFloat("speechlowgroundspeedtrigger"); float warnairspeed = Settings.Instance.GetFloat("speechlowairspeedtrigger"); if (MainV2.comPort.MAV.cs.airspeed < warnairspeed) { if (MainV2.speechEngine.IsReady) { MainV2.speechEngine.SpeakAsync( Common.speechConversion(""+ Settings.Instance["speechlowairspeed"])); speechlowspeedtime = DateTime.Now; } } else if (MainV2.comPort.MAV.cs.groundspeed < warngroundspeed) { if (MainV2.speechEngine.IsReady) { MainV2.speechEngine.SpeakAsync( Common.speechConversion(""+ Settings.Instance["speechlowgroundspeed"])); speechlowspeedtime = DateTime.Now; } } else { speechlowspeedtime = DateTime.Now; } } } // speech altitude warning - message high warning if (speechEnable && speechEngine != null && (MainV2.comPort.logreadmode || comPort.BaseStream.IsOpen)) { float warnalt = float.MaxValue; if (Settings.Instance.ContainsKey("speechaltheight")) { warnalt = Settings.Instance.GetFloat("speechaltheight"); } try { altwarningmax = (int) Math.Max(MainV2.comPort.MAV.cs.alt, altwarningmax); if (Settings.Instance.GetBoolean("speechaltenabled") == true && MainV2.comPort.MAV.cs.alt != 0.00 && (MainV2.comPort.MAV.cs.alt <= warnalt) && MainV2.comPort.MAV.cs.armed) { if (altwarningmax > warnalt) { if (MainV2.speechEngine.IsReady) MainV2.speechEngine.SpeakAsync( Common.speechConversion(""+Settings.Instance["speechalt"])); } } } catch { } // silent fail try { // say the latest high priority message if (MainV2.speechEngine.IsReady && lastmessagehigh != MainV2.comPort.MAV.cs.messageHigh && MainV2.comPort.MAV.cs.messageHigh != null) { if (!MainV2.comPort.MAV.cs.messageHigh.StartsWith("PX4v2 ")) { MainV2.speechEngine.SpeakAsync(MainV2.comPort.MAV.cs.messageHigh); lastmessagehigh = MainV2.comPort.MAV.cs.messageHigh; } } } catch { } } // not doing anything if (!MainV2.comPort.logreadmode && !comPort.BaseStream.IsOpen) { altwarningmax = 0; } // attenuate the link qualty over time if ((DateTime.Now - MainV2.comPort.MAV.lastvalidpacket).TotalSeconds >= 1) { if (linkqualitytime.Second != DateTime.Now.Second) { MainV2.comPort.MAV.cs.linkqualitygcs = (ushort) (MainV2.comPort.MAV.cs.linkqualitygcs*0.8f); linkqualitytime = DateTime.Now; // force redraw if there are no other packets are being read GCSViews.FlightData.myhud.Invalidate(); } } // data loss warning - wait min of 10 seconds, ignore first 30 seconds of connect, repeat at 5 seconds interval if ((DateTime.Now - MainV2.comPort.MAV.lastvalidpacket).TotalSeconds > 10 && (DateTime.Now - connecttime).TotalSeconds > 30 && (DateTime.Now - nodatawarning).TotalSeconds > 5 && (MainV2.comPort.logreadmode || comPort.BaseStream.IsOpen) && MainV2.comPort.MAV.cs.armed) { if (speechEnable && speechEngine != null) { if (MainV2.speechEngine.IsReady) { MainV2.speechEngine.SpeakAsync("WARNING No Data for " + (int) (DateTime.Now - MainV2.comPort.MAV.lastvalidpacket) .TotalSeconds + " Seconds"); nodatawarning = DateTime.Now; } } } // get home point on armed status change. if (armedstatus != MainV2.comPort.MAV.cs.armed && comPort.BaseStream.IsOpen) { armedstatus = MainV2.comPort.MAV.cs.armed; // status just changed to armed if (MainV2.comPort.MAV.cs.armed == true && MainV2.comPort.MAV.aptype != MAVLink.MAV_TYPE.GIMBAL) { try { //MainV2.comPort.getHomePosition(); MainV2.comPort.MAV.cs.HomeLocation = new PointLatLngAlt(MainV2.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 { CustomMessageBox.Show("Failed to update home location (" + MainV2.comPort.MAV.sysid + ")"); }); } } if (speechEnable && speechEngine != null) { if (Settings.Instance.GetBoolean("speecharmenabled")) { string speech = armedstatus ? Settings.Instance["speecharm"] : Settings.Instance["speechdisarm"]; if (!string.IsNullOrEmpty(speech)) { MainV2.speechEngine.SpeakAsync(Common.speechConversion(speech)); } } } } // 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 // MAVLink.MAVLINK_VERSION }; // enumerate each link foreach (var port in Comports) { // poll for params at heartbeat interval if (!port.giveComport) { try { port.getParamPoll(); port.getParamPoll(); } catch { } } // there are 3 hb types we can send, mavlink1, mavlink2 signed and unsigned bool sentsigned = false; bool sentmavlink1 = false; bool sentmavlink2 = false; // enumerate each mav foreach (var MAV in port.MAVlist) { try { // are we talking to a mavlink2 device if (MAV.mavlinkv2) { // is signing enabled if (MAV.signing) { // check if we have already sent if (sentsigned) continue; sentsigned = true; } else { // check if we have already sent if (sentmavlink2) continue; sentmavlink2 = true; } } else { // check if we have already sent if (sentmavlink1) continue; sentmavlink1 = true; } port.sendPacket(htb, MAV.sysid, MAV.compid); } catch (Exception ex) { log.Error(ex); // close the bad port try { port.Close(); } catch { } // refresh the screen if needed if (port == MainV2.comPort) { // refresh config window if needed if (MyView.current != null) { this.Invoke((MethodInvoker) delegate() { if (MyView.current.Name == "HWConfig") MyView.ShowScreen("HWConfig"); if (MyView.current.Name == "SWConfig") MyView.ShowScreen("SWConfig"); }); } } } } } 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); } // read the interfaces foreach (var port in Comports.ToArray()) { if (!port.BaseStream.IsOpen) { // skip primary interface if (port == comPort) continue; // modify array and drop out Comports.Remove(port); port.Dispose(); break; } while (port.BaseStream.IsOpen && port.BaseStream.BytesToRead > minbytes && port.giveComport == false && serialThread) { try { port.readPacket(); } catch (Exception ex) { log.Error(ex); } } // update currentstate of sysids on the port foreach (var MAV in port.MAVlist) { try { MAV.cs.UpdateCurrentSettings(null, false, port, MAV); } catch (Exception ex) { log.Error(ex); } } } } catch (Exception e) { Tracking.AddException(e); log.Error("Serial Reader fail :" + e.ToString()); try { comPort.Close(); } catch (Exception ex) { log.Error(ex); } } } Console.WriteLine("SerialReader Done"); SerialThreadrunner.Set(); }
private void LogPacket(object packet, bool ingoing, int sysId, int compId) { if (threadDone) { return; } if (listViewMessages.InvokeRequired) { try { listViewMessages.Invoke(new Action <object, bool, int, int>(LogPacket), packet, ingoing, sysId, compId); } catch { }; return; } List <string> fields = new List <string>(); fields.Add(sysId.ToString()); fields.Add(compId.ToString()); if ((ingoing && !checkBoxIngoing.Checked) || (!ingoing && !checkBoxOutgoing.Checked)) { return; } string messageName = packet.ToString().Replace("MAVLink+mavlink_", ""); if (IsMessageFilteredOut(messageName)) { return; } if (listViewMessages.IsDisposed) { return; } if (packet.GetType() == typeof(MAVLink.mavlink_gps_raw_int_t)) { MAVLink.mavlink_gps_raw_int_t gps = (MAVLink.mavlink_gps_raw_int_t)packet; fields.Add("GPS Raw Int"); fields.Add(((double)gps.lat / 10000000).ToString()); fields.Add(((double)gps.lon / 10000000).ToString()); fields.Add(gps.alt.ToString()); fields.Add(gps.vel.ToString()); fields.Add(gps.satellites_visible.ToString()); } else if (packet.GetType() == typeof(MAVLink.mavlink_global_position_int_t)) { MAVLink.mavlink_global_position_int_t gps = (MAVLink.mavlink_global_position_int_t)packet; fields.Add("GPS Global Position Int"); fields.Add(((double)gps.lat / 10000000).ToString()); fields.Add(((double)gps.lon / 10000000).ToString()); fields.Add(gps.alt.ToString()); } else if (packet.GetType() == typeof(MAVLink.mavlink_attitude_t)) { MAVLink.mavlink_attitude_t att = (MAVLink.mavlink_attitude_t)packet; fields.Add("Attitude"); fields.Add((att.pitch * 180.0 / Math.PI).ToString()); fields.Add((att.roll * 180.0 / Math.PI).ToString()); fields.Add((att.yaw * 180.0 / Math.PI).ToString()); fields.Add((att.pitchspeed * 180.0 / Math.PI).ToString()); fields.Add((att.rollspeed * 180.0 / Math.PI).ToString()); fields.Add((att.yawspeed * 180.0 / Math.PI).ToString()); } else if (packet.GetType() == typeof(MAVLink.mavlink_scaled_imu_t)) { MAVLink.mavlink_scaled_imu_t imu = (MAVLink.mavlink_scaled_imu_t)packet; fields.Add("Scaled IMU"); fields.Add(imu.xmag.ToString()); fields.Add(imu.ymag.ToString()); fields.Add(imu.zmag.ToString()); } else if (packet.GetType() == typeof(MAVLink.mavlink_scaled_imu3_t)) { MAVLink.mavlink_scaled_imu3_t imu = (MAVLink.mavlink_scaled_imu3_t)packet; fields.Add("Scaled IMU3"); fields.Add(imu.xmag.ToString()); fields.Add(imu.ymag.ToString()); fields.Add(imu.zmag.ToString()); } else if (packet.GetType() == typeof(MAVLink.mavlink_scaled_imu2_t)) { MAVLink.mavlink_scaled_imu2_t imu = (MAVLink.mavlink_scaled_imu2_t)packet; fields.Add("Scaled IMU2"); fields.Add(imu.xmag.ToString()); fields.Add(imu.ymag.ToString()); fields.Add(imu.zmag.ToString()); } else if (packet.GetType() == typeof(MAVLink.mavlink_sys_status_t)) { MAVLink.mavlink_sys_status_t status = (MAVLink.mavlink_sys_status_t)packet; fields.Add("System Status"); fields.Add(status.voltage_battery.ToString()); } // else if (packet.GetType() == typeof(MAVLink.mavlink_autopilot_version_t)) // { // MAVLink.mavlink_autopilot_version_t ver = (MAVLink.mavlink_autopilot_version_t)packet; // listViewMessages.Items.Add(new ListViewItem(new string[] { // "Autopilot Version", // ver.version.ToString(), // ver.custom_version.ToString(), // ver.capabilities.ToString()})); // } else if (packet.GetType() == typeof(MAVLink.mavlink_heartbeat_t)) { MAVLink.mavlink_heartbeat_t hb = (MAVLink.mavlink_heartbeat_t)packet; fields.Add("Heartbeat"); fields.Add(hb.autopilot.ToString()); fields.Add(hb.system_status.ToString()); fields.Add(hb.mavlink_version.ToString()); } else if (packet.GetType() == typeof(MAVLink.mavlink_statustext_t)) { MAVLink.mavlink_statustext_t status = (MAVLink.mavlink_statustext_t)packet; fields.Add("Status Text"); fields.Add(ASCIIEncoding.ASCII.GetString(status.text)); fields.Add(status.severity.ToString()); } else if (packet.GetType() == typeof(MAVLink.mavlink_param_value_t)) { MAVLink.mavlink_param_value_t paramValue = (MAVLink.mavlink_param_value_t)packet; fields.Add("Param Value"); fields.Add(ASCIIEncoding.ASCII.GetString(paramValue.param_id)); fields.Add(paramValue.param_value.ToString()); fields.Add(paramValue.param_count.ToString()); fields.Add(paramValue.param_index.ToString()); fields.Add(paramValue.param_type.ToString()); } else if (packet.GetType() == typeof(MAVLink.mavlink_param_request_read_t)) { MAVLink.mavlink_param_request_read_t paramReq = (MAVLink.mavlink_param_request_read_t)packet; fields.Add("Param Request Read"); fields.Add(ASCIIEncoding.ASCII.GetString(paramReq.param_id)); } else if (packet.GetType() == typeof(MAVLink.mavlink_param_set_t)) { MAVLink.mavlink_param_set_t paramSet = (MAVLink.mavlink_param_set_t)packet; fields.Add("Param Set"); fields.Add(ASCIIEncoding.ASCII.GetString(paramSet.param_id)); fields.Add(paramSet.param_value.ToString()); } else if (packet.GetType() == typeof(MAVLink.mavlink_mission_count_t)) { MAVLink.mavlink_mission_count_t paramValue = (MAVLink.mavlink_mission_count_t)packet; fields.Add("Mission Count"); fields.Add(paramValue.count.ToString()); fields.Add(paramValue.target_component.ToString()); fields.Add(paramValue.target_system.ToString()); } else if (packet.GetType() == typeof(MAVLink.mavlink_mission_item_t)) { MAVLink.mavlink_mission_item_t item = (MAVLink.mavlink_mission_item_t)packet; fields.Add("Mission Item"); fields.Add(item.seq.ToString()); } else if (packet.GetType() == typeof(MAVLink.mavlink_mission_request_t)) { MAVLink.mavlink_mission_request_t item = (MAVLink.mavlink_mission_request_t)packet; fields.Add("Mission Request Item"); fields.Add(item.seq.ToString()); } else if (packet.GetType() == typeof(MAVLink.mavlink_command_ack_t)) { MAVLink.mavlink_command_ack_t paramValue = (MAVLink.mavlink_command_ack_t)packet; fields.Add("Ack"); fields.Add(((MAVLink.MAV_CMD)paramValue.command).ToString()); fields.Add(((MAVLink.MAV_RESULT)paramValue.result).ToString()); } else if (packet.GetType() == typeof(MAVLink.mavlink_mission_ack_t)) { MAVLink.mavlink_mission_ack_t paramValue = (MAVLink.mavlink_mission_ack_t)packet; fields.Add("Mission Ack"); fields.Add(paramValue.type.ToString()); } else if (packet.GetType() == typeof(MAVLink.mavlink_radio_status_t)) { MAVLink.mavlink_radio_status_t radio = (MAVLink.mavlink_radio_status_t)packet; fields.Add("Radio Status"); fields.Add(radio.rssi.ToString()); fields.Add(radio.remrssi.ToString()); } else if (packet.GetType() == typeof(MAVLink.mavlink_ekf_status_report_t)) { MAVLink.mavlink_ekf_status_report_t ekf = (MAVLink.mavlink_ekf_status_report_t)packet; fields.Add("EKF Status"); fields.Add(ekf.flags.ToString()); fields.Add(ekf.velocity_variance.ToString()); fields.Add(ekf.pos_horiz_variance.ToString()); fields.Add(ekf.pos_vert_variance.ToString()); fields.Add(ekf.compass_variance.ToString()); fields.Add(ekf.terrain_alt_variance.ToString()); } else { fields.Add(messageName); //Log(packet.ToString()); } if (ingoing) { listViewMessages.Items.Add(INGOING(new ListViewItem(fields.ToArray()))); } else { listViewMessages.Items.Add(OUTGOING(new ListViewItem(fields.ToArray()))); } }
public void SendHeartBeat() { MAVLink.mavlink_heartbeat_t hb = new MAVLink.mavlink_heartbeat_t(); generatePacket((byte)MAVLink.MAVLINK_MSG_ID.HEARTBEAT, hb); }
private static void HandlePacket(object packet) { if (packet.GetType() == typeof(MAVLink.mavlink_heartbeat_t)) { MAVLink.mavlink_heartbeat_t parsed = (MAVLink.mavlink_heartbeat_t)packet; droneState = (MAVLink.MAV_STATE)parsed.system_status; //MAVLink.MAV_MODE mode = (APMModes.Quadrotor)parsed.custom_mode; } else if (packet.GetType() == typeof(MAVLink.mavlink_global_position_int_t)) { MAVLink.mavlink_global_position_int_t gps = (MAVLink.mavlink_global_position_int_t)packet; // dronePosition.lat = (double)gps.lat / 10000000; // dronePosition.lng = (double)gps.lon / 10000000; // dronePosition.alt = (double)gps.alt / 1000; dronePosition.altFromGround = (double)gps.relative_alt / 1000; dronePosition.heading = (double)gps.hdg / 100; } else if (packet.GetType() == typeof(MAVLink.mavlink_gps_raw_int_t)) { MAVLink.mavlink_gps_raw_int_t gps = (MAVLink.mavlink_gps_raw_int_t)packet; GPSHdop = (double)gps.eph / 100; dronePosition.lat = (double)gps.lat / 10000000; dronePosition.lng = (double)gps.lon / 10000000; dronePosition.alt = (double)gps.alt / 1000; satCount = gps.satellites_visible; IsDroneReady(); } else if (packet.GetType() == typeof(MAVLink.mavlink_mission_count_t)) { MAVLink.mavlink_mission_count_t paramValue = (MAVLink.mavlink_mission_count_t)packet; missionWaypointsCount = paramValue.count; missionItems = new SMissionItem[missionWaypointsCount]; } else if (packet.GetType() == typeof(MAVLink.mavlink_mission_item_t)) { MAVLink.mavlink_mission_item_t paramValue = (MAVLink.mavlink_mission_item_t)packet; SMissionItem missionItem = new SMissionItem(); missionItem.command = (MAVLink.MAV_CMD)paramValue.command; // if (missionItem.command == MAVLink.MAV_CMD.TAKEOFF) // // || (missionItem.command == MAVLink.MAV_CMD.RETURN_TO_LAUNCH)) // { // missionItem.lat = dronePosition.lat; // missionItem.lng = dronePosition.lng; // } // else { missionItem.lat = paramValue.x; missionItem.lng = paramValue.y; } missionItem.p1 = paramValue.param1; missionItem.p2 = paramValue.param2; missionItem.p3 = paramValue.param3; missionItem.p4 = paramValue.param4; missionItem.alt = paramValue.z; // Is this the home? missionItem.IsHome = (paramValue.seq == 0); missionItems[paramValue.seq] = missionItem; } else if (packet.GetType() == typeof(MAVLink.mavlink_mission_request_t)) { MAVLink.mavlink_mission_request_t missionReq = (MAVLink.mavlink_mission_request_t)packet; SetMissionItem(missionReq.seq); IsDroneReady(); } else if (packet.GetType() == typeof(MAVLink.mavlink_param_value_t)) { MAVLink.mavlink_param_value_t paramValue = (MAVLink.mavlink_param_value_t)packet; string name = ASCIIEncoding.ASCII.GetString(paramValue.param_id); name = name.Trim().Replace("\0", ""); double val = paramValue.param_value; if (OnParamUpdate != null) { OnParamUpdate(name, val, paramValue.param_index); } } else if (packet.GetType() == typeof(MAVLink.mavlink_ekf_status_report_t)) { MAVLink.mavlink_ekf_status_report_t ekf = (MAVLink.mavlink_ekf_status_report_t)packet; // Check that all estimations are good EKFStatus = (float)Math.Max(ekf.pos_vert_variance, Math.Max(ekf.compass_variance, Math.Max(ekf.pos_horiz_variance, Math.Max(ekf.pos_vert_variance, ekf.terrain_alt_variance)))); EKFFlags = ekf.flags; IsDroneReady(); } }
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; } } }
void sendingOneHBps() { if (sendHBThread == true) return; sendHBThread = true; MAVLink.mavlink_heartbeat_t htb = new MAVLink.mavlink_heartbeat_t() { type = (byte)MAVLink.MAV_TYPE.QUADROTOR, autopilot = (byte)MAVLink.MAV_AUTOPILOT.ARDUPILOTMEGA, mavlink_version = 3 }; byte[] pabyte = new byte[] { 254, 9, 7, 1, 1, 0, 0, 0, 0, 0, 2, 3, 81, 4, 3, 250, 173 }; //mydata.myLink.ReturnByteArray( // myServer.Write(pabyte, 0, pabyte.Length); // mydata.myLink.sendPacket(htb); while (sendHBThread) { try { Thread.Sleep(950); if (this.IsOpen) { this.Write(pabyte, 0, pabyte.Length); } } catch (Exception e) { //log.Error("HB sending fail :" + e.ToString()); try { this.Close(); } catch { } } } }
public static void Main(string[] args) { Program.args = args; Console.WriteLine( "If your error is about Microsoft.DirectX.DirectInput, please install the latest directx redist from here http://www.microsoft.com/en-us/download/details.aspx?id=35 \n\n"); Console.WriteLine("Debug under mono MONO_LOG_LEVEL=debug mono MissionPlanner.exe"); Thread = Thread.CurrentThread; System.Windows.Forms.Application.EnableVisualStyles(); XmlConfigurator.Configure(); log.Info("******************* Logging Configured *******************"); System.Windows.Forms.Application.SetCompatibleTextRenderingDefault(false); ServicePointManager.DefaultConnectionLimit = 10; System.Windows.Forms.Application.ThreadException += Application_ThreadException; AppDomain.CurrentDomain.UnhandledException += new UnhandledExceptionEventHandler(CurrentDomain_UnhandledException); // fix ssl on mono ServicePointManager.ServerCertificateValidationCallback = new System.Net.Security.RemoteCertificateValidationCallback( (sender, certificate, chain, policyErrors) => { return(true); }); if (args.Length > 0 && args[0] == "/update") { Utilities.Update.DoUpdate(); return; } name = "Mission Planner"; try { if (File.Exists(Settings.GetRunningDirectory() + "logo.txt")) { name = File.ReadAllLines(Settings.GetRunningDirectory() + "logo.txt", Encoding.UTF8)[0]; } } catch { } if (File.Exists(Settings.GetRunningDirectory() + "logo.png")) { Logo = new Bitmap(Settings.GetRunningDirectory() + "logo.png"); } if (File.Exists(Settings.GetRunningDirectory() + "icon.png")) { // 128*128 IconFile = new Bitmap(Settings.GetRunningDirectory() + "icon.png"); } else { IconFile = MissionPlanner.Properties.Resources.mpdesktop.ToBitmap(); } if (File.Exists(Settings.GetRunningDirectory() + "splashbg.png")) // 600*375 { SplashBG = new Bitmap(Settings.GetRunningDirectory() + "splashbg.png"); } Splash = new MissionPlanner.Splash(); if (SplashBG != null) { Splash.BackgroundImage = SplashBG; Splash.pictureBox1.Visible = false; } if (IconFile != null) { Splash.Icon = Icon.FromHandle(((Bitmap)IconFile).GetHicon()); } string strVersion = System.Reflection.Assembly.GetExecutingAssembly().GetName().Version.ToString(); Splash.Text = name + " " + Application.ProductVersion + " build " + strVersion; Splash.Show(); Application.DoEvents(); Application.DoEvents(); // setup theme provider CustomMessageBox.ApplyTheme += MissionPlanner.Utilities.ThemeManager.ApplyThemeTo; Controls.MainSwitcher.ApplyTheme += MissionPlanner.Utilities.ThemeManager.ApplyThemeTo; MissionPlanner.Controls.InputBox.ApplyTheme += MissionPlanner.Utilities.ThemeManager.ApplyThemeTo; Controls.BackstageView.BackstageViewPage.ApplyTheme += MissionPlanner.Utilities.ThemeManager.ApplyThemeTo; // setup settings provider MissionPlanner.Comms.CommsBase.Settings += CommsBase_Settings; MissionPlanner.Comms.CommsBase.ApplyTheme += MissionPlanner.Utilities.ThemeManager.ApplyThemeTo; // set the cache provider to my custom version GMap.NET.GMaps.Instance.PrimaryCache = new Maps.MyImageCache(); // add my custom map providers GMap.NET.MapProviders.GMapProviders.List.Add(Maps.WMSProvider.Instance); GMap.NET.MapProviders.GMapProviders.List.Add(Maps.Custom.Instance); GMap.NET.MapProviders.GMapProviders.List.Add(Maps.Earthbuilder.Instance); GMap.NET.MapProviders.GMapProviders.List.Add(Maps.Statkart_Topo2.Instance); GMap.NET.MapProviders.GMapProviders.List.Add(Maps.MapBox.Instance); GMap.NET.MapProviders.GMapProviders.List.Add(Maps.MapboxNoFly.Instance); GMap.NET.MapProviders.GMapProviders.List.Add(GDAL.GDALProvider.Instance); // add proxy settings GMap.NET.MapProviders.GMapProvider.WebProxy = WebRequest.GetSystemWebProxy(); GMap.NET.MapProviders.GMapProvider.WebProxy.Credentials = CredentialCache.DefaultCredentials; WebRequest.DefaultWebProxy = WebRequest.GetSystemWebProxy(); WebRequest.DefaultWebProxy.Credentials = CredentialCache.DefaultNetworkCredentials; if (name == "VVVVZ") { // set pw Settings.Instance["password"] = "******"; Settings.Instance["password_protect"] = "True"; // prevent wizard Settings.Instance["newuser"] = "******"; // invalidate update url System.Configuration.ConfigurationManager.AppSettings["UpdateLocationVersion"] = ""; } CleanupFiles(); log.InfoFormat("64bit os {0}, 64bit process {1}", System.Environment.Is64BitOperatingSystem, System.Environment.Is64BitProcess); Device.DeviceStructure test1 = new Device.DeviceStructure(73225); Device.DeviceStructure test2 = new Device.DeviceStructure(262434); Device.DeviceStructure test3 = new Device.DeviceStructure(131874); MAVLink.MavlinkParse tmp = new MAVLink.MavlinkParse(); MAVLink.mavlink_heartbeat_t hb = new MAVLink.mavlink_heartbeat_t() { autopilot = 1, base_mode = 2, custom_mode = 3, mavlink_version = 2, system_status = 6, type = 7 }; var t1 = tmp.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.HEARTBEAT, hb); var t2 = tmp.GenerateMAVLinkPacket20(MAVLink.MAVLINK_MSG_ID.HEARTBEAT, hb); tmp.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.HEARTBEAT, hb); tmp.GenerateMAVLinkPacket20(MAVLink.MAVLINK_MSG_ID.HEARTBEAT, hb); tmp.GenerateMAVLinkPacket20(MAVLink.MAVLINK_MSG_ID.HEARTBEAT, hb, true); tmp.GenerateMAVLinkPacket20(MAVLink.MAVLINK_MSG_ID.HEARTBEAT, hb, true); try { //System.Diagnostics.Process.GetCurrentProcess().PriorityClass = System.Diagnostics.ProcessPriorityClass.RealTime; Thread.CurrentThread.Name = "Base Thread"; Application.Run(new MainV2()); } catch (Exception ex) { log.Fatal("Fatal app exception", ex); Console.WriteLine(ex.ToString()); Console.WriteLine("\nPress any key to exit!"); Console.ReadLine(); } try { // kill sim background process if its still running if (Controls.SITL.simulator != null) { Controls.SITL.simulator.Kill(); } } catch { } }
string GetLog(ushort no, string fileName) { log.Info("GetLog " + no); MainV2.comPort.Progress += comPort_Progress; status = SerialStatus.Reading; // used for log fn byte[] hbpacket = MainV2.comPort.getHeartBeat(); if (hbpacket != null) { log.Info("Got hbpacket length: " + hbpacket.Length); } // get df log from mav using (var ms = MainV2.comPort.GetLog(no)) { if (ms != null) { log.Info("Got Log length: " + ms.Length); } ms.Seek(0, SeekOrigin.Begin); status = SerialStatus.Done; MAVLink.mavlink_heartbeat_t hb = (MAVLink.mavlink_heartbeat_t)MainV2.comPort.DebugPacket(hbpacket); logfile = Settings.Instance.LogDir + Path.DirectorySeparatorChar + MainV2.comPort.MAV.aptype.ToString() + Path.DirectorySeparatorChar + hbpacket[3] + Path.DirectorySeparatorChar + no + " " + MakeValidFileName(fileName) + ".bin"; // make log dir Directory.CreateDirectory(Path.GetDirectoryName(logfile)); log.Info("about to write: " + logfile); // save memorystream to file using (BinaryWriter bw = new BinaryWriter(File.OpenWrite(logfile))) { byte[] buffer = new byte[256 * 1024]; while (ms.Position < ms.Length) { int read = ms.Read(buffer, 0, buffer.Length); bw.Write(buffer, 0, read); } } } log.Info("about to convertbin: " + logfile); // create ascii log BinaryLog.ConvertBin(logfile, logfile + ".log"); //update the new filename logfile = logfile + ".log"; MainV2.comPort.Progress -= comPort_Progress; return(logfile); }
string GetLog(ushort no, string fileName) { log.Info("GetLog " + no); MainV2.comPort.Progress += comPort_Progress; status = SerialStatus.Reading; // used for log fn MAVLink.MAVLinkMessage hbpacket = MainV2.comPort.getHeartBeat(); if (hbpacket != null) { log.Info("Got hbpacket length: " + hbpacket.Length); } // get df log from mav using (var ms = MainV2.comPort.GetLog(no)) { if (ms != null) { log.Info("Got Log length: " + ms.Length); } ms.Seek(0, SeekOrigin.Begin); status = SerialStatus.Done; MAVLink.mavlink_heartbeat_t hb = (MAVLink.mavlink_heartbeat_t)MainV2.comPort.DebugPacket(hbpacket); logfile = Settings.Instance.LogDir + Path.DirectorySeparatorChar + MainV2.comPort.MAV.aptype.ToString() + Path.DirectorySeparatorChar + hbpacket.sysid + Path.DirectorySeparatorChar + no + " " + MakeValidFileName(fileName) + ".bin"; // make log dir Directory.CreateDirectory(Path.GetDirectoryName(logfile)); log.Info("about to write: " + logfile); // save memorystream to file using (BinaryWriter bw = new BinaryWriter(File.OpenWrite(logfile))) { byte[] buffer = new byte[256 * 1024]; while (ms.Position < ms.Length) { int read = ms.Read(buffer, 0, buffer.Length); bw.Write(buffer, 0, read); } } } log.Info("about to convertbin: " + logfile); // create ascii log BinaryLog.ConvertBin(logfile, logfile + ".log"); //update the new filename logfile = logfile + ".log"; // rename file if needed log.Info("about to GetFirstGpsTime: " + logfile); // get gps time of assci log DateTime logtime = new DFLog().GetFirstGpsTime(logfile); // rename log is we have a valid gps time if (logtime != DateTime.MinValue) { string newlogfilename = Settings.Instance.LogDir + Path.DirectorySeparatorChar + MainV2.comPort.MAV.aptype.ToString() + Path.DirectorySeparatorChar + hbpacket.sysid + Path.DirectorySeparatorChar + logtime.ToString("yyyy-MM-dd HH-mm-ss") + ".log"; try { File.Move(logfile, newlogfilename); // rename bin as well File.Move(logfile.Replace(".log", ""), newlogfilename.Replace(".log", ".bin")); logfile = newlogfilename; } catch { CustomMessageBox.Show(Strings.ErrorRenameFile + " " + logfile + "\nto " + newlogfilename, Strings.ERROR); } } MainV2.comPort.Progress -= comPort_Progress; return(logfile); }
/// <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> internal async Task SerialReader() { if (serialThread == true) return; serialThread = true; SerialThreadrunner.Reset(); int minbytes = 10; int altwarningmax = 0; bool armedstatus = false; string lastmessagehigh = ""; DateTime speechcustomtime = DateTime.Now; DateTime speechlowspeedtime = DateTime.Now; DateTime linkqualitytime = DateTime.Now; while (serialThread) { try { Thread.Sleep(1); // was 5 // update connect/disconnect button and info stats try { //UpdateConnectIcon(); } catch (Exception ex) { log.Error(ex); } // 30 seconds interval speech options if (MainV2.speechEnabled() && speechEngine != null && (DateTime.Now - speechcustomtime).TotalSeconds > 30 && (MainV2.comPort.logreadmode || comPort.BaseStream.IsOpen)) { if (MainV2.speechEngine.IsReady) { if (Settings.Instance.GetBoolean("speechcustomenabled")) { MainV2.speechEngine.SpeakAsync(ArduPilot.Common.speechConversion(comPort.MAV, "" + Settings.Instance["speechcustom"])); } speechcustomtime = DateTime.Now; } // speech for battery alerts //speechbatteryvolt float warnvolt = Settings.Instance.GetFloat("speechbatteryvolt"); float warnpercent = Settings.Instance.GetFloat("speechbatterypercent"); if (Settings.Instance.GetBoolean("speechbatteryenabled") == true && MainV2.comPort.MAV.cs.battery_voltage <= warnvolt && MainV2.comPort.MAV.cs.battery_voltage >= 5.0) { if (MainV2.speechEngine.IsReady) { MainV2.speechEngine.SpeakAsync(ArduPilot.Common.speechConversion(comPort.MAV, "" + Settings.Instance["speechbattery"])); } } else if (Settings.Instance.GetBoolean("speechbatteryenabled") == true && (MainV2.comPort.MAV.cs.battery_remaining) < warnpercent && MainV2.comPort.MAV.cs.battery_voltage >= 5.0 && MainV2.comPort.MAV.cs.battery_remaining != 0.0) { if (MainV2.speechEngine.IsReady) { MainV2.speechEngine.SpeakAsync( ArduPilot.Common.speechConversion(comPort.MAV, "" + Settings.Instance["speechbattery"])); } } } // speech for airspeed alerts if (speechEnabled() && speechEngine != null && (DateTime.Now - speechlowspeedtime).TotalSeconds > 10 && (MainV2.comPort.logreadmode || comPort.BaseStream.IsOpen)) { if (Settings.Instance.GetBoolean("speechlowspeedenabled") == true && MainV2.comPort.MAV.cs.armed) { float warngroundspeed = Settings.Instance.GetFloat("speechlowgroundspeedtrigger"); float warnairspeed = Settings.Instance.GetFloat("speechlowairspeedtrigger"); if (MainV2.comPort.MAV.cs.airspeed < warnairspeed) { if (MainV2.speechEngine.IsReady) { MainV2.speechEngine.SpeakAsync( ArduPilot.Common.speechConversion(comPort.MAV, "" + Settings.Instance["speechlowairspeed"])); speechlowspeedtime = DateTime.Now; } } else if (MainV2.comPort.MAV.cs.groundspeed < warngroundspeed) { if (MainV2.speechEngine.IsReady) { MainV2.speechEngine.SpeakAsync( ArduPilot.Common.speechConversion(comPort.MAV, "" + Settings.Instance["speechlowgroundspeed"])); speechlowspeedtime = DateTime.Now; } } else { speechlowspeedtime = DateTime.Now; } } } // speech altitude warning - message high warning if ((speechEnabled() && speechEngine != null && (MainV2.comPort.logreadmode || comPort.BaseStream.IsOpen)) { float warnalt = float.MaxValue; if (Settings.Instance.ContainsKey("speechaltheight")) { warnalt = Settings.Instance.GetFloat("speechaltheight"); } try { altwarningmax = (int)Math.Max(MainV2.comPort.MAV.cs.alt, altwarningmax); if (Settings.Instance.GetBoolean("speechaltenabled") == true && MainV2.comPort.MAV.cs.alt != 0.00 && (MainV2.comPort.MAV.cs.alt <= warnalt) && MainV2.comPort.MAV.cs.armed) { if (altwarningmax > warnalt) { if (MainV2.speechEngine.IsReady) MainV2.speechEngine.SpeakAsync( ArduPilot.Common.speechConversion(comPort.MAV, "" + Settings.Instance["speechalt"])); } } } catch { } // silent fail try { // say the latest high priority message if (MainV2.speechEngine.IsReady && lastmessagehigh != MainV2.comPort.MAV.cs.messageHigh && MainV2.comPort.MAV.cs.messageHigh != null) { if (!MainV2.comPort.MAV.cs.messageHigh.StartsWith("PX4v2 ")) { MainV2.speechEngine.SpeakAsync(MainV2.comPort.MAV.cs.messageHigh); lastmessagehigh = MainV2.comPort.MAV.cs.messageHigh; } } } catch { } } // not doing anything if (!MainV2.comPort.logreadmode && !comPort.BaseStream.IsOpen) { altwarningmax = 0; } // attenuate the link qualty over time if ((DateTime.Now - MainV2.comPort.MAV.lastvalidpacket).TotalSeconds >= 1) { if (linkqualitytime.Second != DateTime.Now.Second) { MainV2.comPort.MAV.cs.linkqualitygcs = (ushort)(MainV2.comPort.MAV.cs.linkqualitygcs * 0.8f); linkqualitytime = DateTime.Now; // force redraw if there are no other packets are being read } } // data loss warning - wait min of 10 seconds, ignore first 30 seconds of connect, repeat at 5 seconds interval if ((DateTime.Now - MainV2.comPort.MAV.lastvalidpacket).TotalSeconds > 10 && (DateTime.Now - connecttime).TotalSeconds > 30 && (DateTime.Now - nodatawarning).TotalSeconds > 5 && (MainV2.comPort.logreadmode || comPort.BaseStream.IsOpen) && MainV2.comPort.MAV.cs.armed) { if (speechEnable && speechEngine != null) { if (MainV2.speechEngine.IsReady) { MainV2.speechEngine.SpeakAsync("WARNING No Data for " + (int) (DateTime.Now - MainV2.comPort.MAV.lastvalidpacket) .TotalSeconds + " Seconds"); nodatawarning = DateTime.Now; } } } // get home point on armed status change. if (armedstatus != MainV2.comPort.MAV.cs.armed && comPort.BaseStream.IsOpen) { armedstatus = MainV2.comPort.MAV.cs.armed; // status just changed to armed if (MainV2.comPort.MAV.cs.armed == true && MainV2.comPort.MAV.apname != MAVLink.MAV_AUTOPILOT.INVALID && MainV2.comPort.MAV.aptype != MAVLink.MAV_TYPE.GIMBAL) { System.Threading.ThreadPool.QueueUserWorkItem(state => { Thread.CurrentThread.Name = "Arm State change"; try { while (comPort.giveComport == true) Thread.Sleep(100); MainV2.comPort.MAV.cs.HomeLocation = new PointLatLngAlt(MainV2.comPort.getWP(0)); } catch { // dont hang this loop } }); } if (speechEnable && speechEngine != null) { if (Settings.Instance.GetBoolean("speecharmenabled")) { string speech = armedstatus ? Settings.Instance["speecharm"] : Settings.Instance["speechdisarm"]; if (!string.IsNullOrEmpty(speech)) { MainV2.speechEngine.SpeakAsync(ArduPilot.Common.speechConversion(comPort.MAV, speech)); } } } } // 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 // MAVLink.MAVLINK_VERSION }; // enumerate each link foreach (var port in Comports.ToArray()) { if (!port.BaseStream.IsOpen) continue; // poll for params at heartbeat interval - primary mav on this port only if (!port.giveComport) { try { // poll only when not armed if (!port.MAV.cs.armed) { port.getParamPoll(); port.getParamPoll(); } } catch { } } // there are 3 hb types we can send, mavlink1, mavlink2 signed and unsigned bool sentsigned = false; bool sentmavlink1 = false; bool sentmavlink2 = false; // enumerate each mav foreach (var MAV in port.MAVlist) { try { // poll for version if we dont have it - every mav every port if (!port.giveComport && !MAV.cs.armed && (DateTime.Now.Second % 20) == 0 && MAV.cs.version < new Version(0, 1)) port.getVersion(MAV.sysid, MAV.compid, false); // are we talking to a mavlink2 device if (MAV.mavlinkv2) { // is signing enabled if (MAV.signing) { // check if we have already sent if (sentsigned) continue; sentsigned = true; } else { // check if we have already sent if (sentmavlink2) continue; sentmavlink2 = true; } } else { // check if we have already sent if (sentmavlink1) continue; sentmavlink1 = true; } port.sendPacket(htb, MAV.sysid, MAV.compid); } catch (Exception ex) { log.Error(ex); // close the bad port try { port.Close(); } catch { } // refresh the screen if needed if (port == MainV2.comPort) { // refresh config window if needed } } } } 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); } // read the interfaces foreach (var port in Comports.ToArray()) { if (!port.BaseStream.IsOpen) { // skip primary interface if (port == comPort) continue; // modify array and drop out Comports.Remove(port); port.Dispose(); break; } DateTime startread = DateTime.Now; // must be open, we have bytes, we are not yielding the port, // the thread is meant to be running and we only spend 1 seconds max in this read loop while (port.BaseStream.IsOpen && port.BaseStream.BytesToRead > minbytes && port.giveComport == false && serialThread && startread.AddSeconds(1) > DateTime.Now) { try { await port.readPacketAsync().ConfigureAwait(false); } catch (Exception ex) { log.Error(ex); } } // update currentstate of sysids on the port foreach (var MAV in port.MAVlist) { try { MAV.cs.UpdateCurrentSettings(null, false, port, MAV); } catch (Exception ex) { log.Error(ex); } } } } catch (Exception e) { log.Error("Serial Reader fail :" + e.ToString()); try { comPort.Close(); } catch (Exception ex) { log.Error(ex); } } } Console.WriteLine("SerialReader Done"); SerialThreadrunner.Set(); }
private void mavTimer_Tick(object sender, EventArgs e) { if (mavbusy) { return; } mavbusy = true; mavlink.sysid = 7; mavlink.compid = 1; MAVLink.mavlink_heartbeat_t hb = new MAVLink.mavlink_heartbeat_t(); hb.autopilot = (byte)(MAVLink.MAV_AUTOPILOT.ARDUPILOTMEGA); //hb.system_status = MAVLink.MAV mavlink.sendPacket(hb); MAVLink.mavlink_gps_raw_int_t g = new MAVLink.mavlink_gps_raw_int_t(); lock (this) { if (angle >= 0) { // lat = (int)(5000 * Math.Cos(angle / 2 / Math.PI)); // lon = (int)(5000 * Math.Sin(angle / 2 / Math.PI)); double lat2, lon2; getDstPoint(0.0f, 0.0, 500, (angle * Math.PI / 180), out lat2, out lon2); lat = (int)(lat2 * 1.0e7); lon = (int)(lon2 * 1.0e7); double a2 = getBearing(0.0, 0.0, lat2, lon2); Console.WriteLine("angle {0} {1}", angle, a2); angle += 2; if (angle > 360) { angle -= 360; } } else if (angle >= -5) { lat = lon = 0; angle--; } else { angle = 0; } // int.TryParse(tbxLat.Text, out lat); // int.TryParse(tbxLon.Text, out lon); // int.TryParse(tbxAlt.Text, out alt); g.lat = lat; g.lon = lon; g.alt = alt; } g.fix_type = 3; mavlink.sendPacket(g); MAVLink.mavlink_vfr_hud_t vfr = new MAVLink.mavlink_vfr_hud_t(); vfr.airspeed = 10; vfr.groundspeed = 10; vfr.alt = alt / 1000.0f; vfr.heading = (short)angle; vfr.throttle = 10; mavlink.sendPacket(vfr); mavbusy = false; //if (serialPort2.BytesToRead > 0) //{ // //Console.WriteLine("mavlink read"); // Console.Write(serialPort2.ReadExisting()); //} }