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);
}
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
{
}
}
}
}
/// <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());
//}
}
