public VehicleViewModel(Stream _stream)
{
Vehicle = new VehicleModel();
_mavlinkparse = new MAVLink.MavlinkParse();
_datastream = _stream;
Vehicle.IsConnecting = true;
}
private static void mainloop()
{
run = true;
byte[] buffer = new byte[1024];
MAVLink.MavlinkParse mav = new MAVLink.MavlinkParse();
while (run)
{
try
{
if (Server == null)
{
System.Threading.Thread.Sleep(1);
continue;
}
while (Server.Connected && Server.Available > 0)
{
int read = Server.GetStream().Read(buffer, 0, buffer.Length);
// write to all clients
foreach (var client in clients)
{
if (client.Connected)
client.GetStream().Write(buffer, 0, read);
}
}
}
catch
{
}
// read from all clients
foreach (var client in clients)
{
try
{
while (client.Connected && client.Available > 0)
{
var packet = mav.ReadPacket(client.GetStream());
if (packet == null)
continue;
if (Server != null && Server.Connected)
Server.GetStream().Write(packet.buffer, 0, packet.Length);
}
}
catch
{
client.Close();
}
}
System.Threading.Thread.Sleep(1);
}
}
private static void mainloop()
{
run = true;
byte[] buffer = new byte[1024];
MAVLink.MavlinkParse mav = new MAVLink.MavlinkParse();
while (run)
{
try
{
while (Server.Connected && Server.Available > 0)
{
int read = Server.GetStream().Read(buffer, 0, buffer.Length);
// write to all clients
foreach (var client in clients)
{
if (client.Connected)
{
client.GetStream().Write(buffer, 0, read);
}
}
}
}
catch
{
}
// read from all clients
foreach (var client in clients)
{
try
{
while (client.Connected && client.Available > 0)
{
byte[] packet = mav.ReadPacket(client.GetStream());
if (packet == null)
{
continue;
}
if (Server != null && Server.Connected)
{
Server.GetStream().Write(packet, 0, packet.Length);
}
}
}
catch
{
client.Close();
}
}
System.Threading.Thread.Sleep(1);
}
}
// Start is called before the first frame update
void Start()
{
buf = new byte[1024];
mavlinkParse = new MAVLink.MavlinkParse();
sock = new Socket(AddressFamily.InterNetwork, SocketType.Dgram, ProtocolType.Udp)
{
Blocking = false
};
sock.Bind(new IPEndPoint(IPAddress.Loopback, 17500 + DroneID));
myproxy = new IPEndPoint(IPAddress.Loopback, 17500);
}
private void but_hexmavlink_Click(object sender, EventArgs e)
{
string input = "";
InputBox.Show("", "enter the hex value 'fd0500001a0c1'", ref input);
var packet = StringToByteArray(input);
var mavpacket = new MAVLink.MavlinkParse().ReadPacket(new MemoryStream(packet));
CustomMessageBox.Show(mavpacket.ToJSON(Formatting.Indented));
}
public override bool Init()
{
var oldlist = MAVLink.MAVLINK_MESSAGE_INFOS.ToList();
oldlist.Add(new MAVLink.message_info(27499, "VFR_HUD_EDIT", 20, 20, 20, typeof(mavlink_vfr_hud_EDIT_t)));
MAVLink.MAVLINK_MESSAGE_INFOS = oldlist.ToArray();
var parse = new MAVLink.MavlinkParse();
var packet = parse.GenerateMAVLinkPacket20((MAVLink.MAVLINK_MSG_ID) 274, new mavlink_vfr_hud_EDIT_t());
loopratehz = 1f;
return(true);
}
private void but_packetbytes_Click(object sender, EventArgs e)
{
string input = "";
InputBox.Show("input", "enter the hex byte data", ref input, false, true);
var split = input.Replace("0x", ",").Split(new char[] { ',', ' ' }, StringSplitOptions.RemoveEmptyEntries);
var buffer = split.Select(a => Convert.ToByte(a, 16));
MAVLink.MavlinkParse parse = new MAVLink.MavlinkParse();
var packet = parse.ReadPacket(new MemoryStream(buffer.ToArray()));
CustomMessageBox.Show(packet?.ToString() +
"\n" + packet.ToJSON().WrapText(5, new[] { ',' }));
}
void mainloop()
{
threadrun = true;
DowdingPlugin.UpdateOutput += DowdingPlugin_UpdateOutput;
mavlink = new MAVLink.MavlinkParse();
starttime = DateTime.Now;
SendHome();
while (threadrun)
{
Thread.Sleep(500);
}
DowdingPlugin.UpdateOutput -= DowdingPlugin_UpdateOutput;
}
// Start is called before the first frame update
void Start()
{
selectedStyle = new GUIStyle {
normal = new GUIStyleState {
background = MakeTex(Color.green)
}
};
selected = false;
cam = GameObject.FindGameObjectWithTag("MainCamera").GetComponent <Camera>();
buf = new byte[1024];
mavlinkParse = new MAVLink.MavlinkParse();
sock = new Socket(AddressFamily.InterNetwork, SocketType.Dgram, ProtocolType.Udp)
{
Blocking = false
};
sock.Bind(new IPEndPoint(IPAddress.Loopback, 17500 + DroneID));
myproxy = new IPEndPoint(IPAddress.Loopback, 17500);
wp = GameObject.Instantiate(Waypoint, Vector3.zero, Quaternion.identity);
wp.SetActive(false);
}
public static IEnumerable <MAVLink.MAVLinkMessage> GetMessageOfType(this CommsFile commsFile,
MAVLink.MAVLINK_MSG_ID[] packetids = null, bool hasTimestamp = false)
{
var parse = new MAVLink.MavlinkParse(hasTimestamp);
var list = packetids.Cast <uint>();
while (commsFile.BytesToRead > 0)
{
var packet = parse.ReadPacket(commsFile.BaseStream);
if (packet == null)
{
continue;
}
if (packetids == null || list.Contains(packet.msgid))
{
yield return(packet);
}
}
}
public static void anonymise(string logfile, string outputfile)
{
if (!File.Exists(logfile))
{
return;
}
var rand = new Random();
var latrandom = rand.NextDouble() * (rand.NextDouble() * 3);
// TLOG
if (logfile.ToLower().EndsWith(".tlog"))
{
Comms.CommsFile tlogFile = new CommsFile();
tlogFile.Open(logfile);
using (var stream = new CommsStream(tlogFile, tlogFile.BytesToRead))
using (var outfilestream = File.Open(outputfile, FileMode.Create, FileAccess.ReadWrite, FileShare.Read))
{
MAVLink.MavlinkParse parse = new MAVLink.MavlinkParse(true);
var block = new Type[]
{
typeof(MAVLink.mavlink_fence_point_t),
typeof(MAVLink.mavlink_simstate_t),
typeof(MAVLink.mavlink_rally_point_t),
typeof(MAVLink.mavlink_ahrs2_t),
typeof(MAVLink.mavlink_camera_feedback_t),
typeof(MAVLink.mavlink_ahrs3_t),
typeof(MAVLink.mavlink_deepstall_t),
typeof(MAVLink.mavlink_gps_raw_int_t),
typeof(MAVLink.mavlink_global_position_int_t),
typeof(MAVLink.mavlink_set_gps_global_origin_t),
typeof(MAVLink.mavlink_gps_global_origin_t),
typeof(MAVLink.mavlink_global_position_int_cov_t),
typeof(MAVLink.mavlink_set_position_target_global_int_t),
typeof(MAVLink.mavlink_hil_state_t),
typeof(MAVLink.mavlink_sim_state_t),
typeof(MAVLink.mavlink_hil_gps_t),
typeof(MAVLink.mavlink_hil_state_quaternion_t),
typeof(MAVLink.mavlink_gps2_raw_t),
typeof(MAVLink.mavlink_terrain_request_t),
typeof(MAVLink.mavlink_terrain_check_t),
typeof(MAVLink.mavlink_terrain_report_t),
typeof(MAVLink.mavlink_follow_target_t),
typeof(MAVLink.mavlink_gps_input_t),
typeof(MAVLink.mavlink_high_latency_t),
typeof(MAVLink.mavlink_home_position_t),
typeof(MAVLink.mavlink_set_home_position_t),
typeof(MAVLink.mavlink_adsb_vehicle_t),
typeof(MAVLink.mavlink_camera_image_captured_t),
typeof(MAVLink.mavlink_uavionix_adsb_out_dynamic_t),
typeof(MAVLink.mavlink_global_position_int_t),
typeof(MAVLink.mavlink_set_home_position_t),
typeof(MAVLink.mavlink_home_position_t),
typeof(MAVLink.mavlink_set_position_target_global_int_t),
typeof(MAVLink.mavlink_local_position_ned_t),
typeof(MAVLink.mavlink_command_long_t),
typeof(MAVLink.mavlink_mission_item_t),
typeof(MAVLink.mavlink_mission_item_int_t),
typeof(MAVLink.mavlink_uavionix_adsb_out_cfg_t)
};
var checks = new string[]
{
"lat", "latitude", "lat_int", "landing_lat",
"path_lat", "arc_entry_lat", "gpsLat", "gpsOffsetLat"
};
while (stream.Position < stream.Length)
{
var packet = parse.ReadPacket(stream);
if (packet == null)
{
continue;
}
var msginfo = MAVLink.MAVLINK_MESSAGE_INFOS.GetMessageInfo(packet.msgid);
if (block.Contains(msginfo.type))
{
bool valid = false;
var oldrxtime = packet.rxtime;
foreach (var check in checks)
{
var field = msginfo.type.GetField(check);
if (field != null)
{
var pkt = packet.data;
var value = field.GetValue(pkt);
if (value is Int32)
{
if ((int)value != 0)
{
field.SetValue(pkt, (int)((int)value + latrandom * 1e7));
}
packet = new MAVLink.MAVLinkMessage(
parse.GenerateMAVLinkPacket20((MAVLink.MAVLINK_MSG_ID)msginfo.msgid,
pkt, false, packet.sysid, packet.compid, packet.seq));
valid = true;
}
else if (value is Single)
{
if ((Single)value != 0)
{
field.SetValue(pkt, (Single)value + latrandom);
}
packet = new MAVLink.MAVLinkMessage(
parse.GenerateMAVLinkPacket20((MAVLink.MAVLINK_MSG_ID)msginfo.msgid,
pkt, false, packet.sysid, packet.compid, packet.seq));
valid = true;
}
else
{
continue;
}
}
}
packet.rxtime = oldrxtime;
}
byte[] datearray =
BitConverter.GetBytes(
(UInt64)((packet.rxtime.ToUniversalTime() - new DateTime(1970, 1, 1))
.TotalMilliseconds * 1000));
Array.Reverse(datearray);
outfilestream.Write(datearray, 0, datearray.Length);
outfilestream.Write(packet.buffer, 0, packet.Length);
}
}
}
else //LOG
{
using (DFLogBuffer col = new DFLogBuffer(File.OpenRead(logfile)))
using (var outfilestream = File.Open(outputfile, FileMode.Create, FileAccess.ReadWrite, FileShare.Read))
{
foreach (var dfItem in col.GetEnumeratorTypeAll())
{
var index = col.dflog.FindMessageOffset(dfItem.msgtype, "lat");
if (index != -1)
{
var lat = Double.Parse(dfItem.items[index], CultureInfo.InvariantCulture);
if (lat != 0)
{
dfItem.items[index] = (lat + latrandom).ToString(CultureInfo.InvariantCulture);
}
}
var str = String.Join(",", dfItem.items) + "\r\n";
outfilestream.Write(ASCIIEncoding.ASCII.GetBytes(str), 0, str.Length);
}
}
}
}
static void Main(string[] args)
{
var mavlinkudp = new UdpClient(14550);
var mavparse = new MAVLink.MavlinkParse();
var seqno = 0;
IPEndPoint ipEP = new IPEndPoint(IPAddress.Any, 0);
var oldlist = MAVLink.MAVLINK_MESSAGE_INFOS.ToList();
oldlist.Add(new MAVLink.message_info(27499, "VFR_HUD_EDIT", 20, 20, 20, typeof(mavlink_vfr_hud_EDIT_t)));
MAVLink.MAVLINK_MESSAGE_INFOS = oldlist.ToArray();
while (true)
{
while (mavlinkudp.Available > 0)
{
ipEP = new IPEndPoint(IPAddress.Any, 0);
try
{
var rawpacket = mavlinkudp.Receive(ref ipEP);
var packet = mavparse.ReadPacket(new MemoryStream(rawpacket));
Console.WriteLine(packet);
}
catch
{
continue;
}
}
if (ipEP.Address == IPAddress.Any)
{
Thread.Sleep(100);
continue;
}
var sendpacket = mavparse.GenerateMAVLinkPacket20(MAVLink.MAVLINK_MSG_ID.HEARTBEAT,
new MAVLink.mavlink_heartbeat_t()
{
autopilot = (byte)MAVLink.MAV_AUTOPILOT.ARDUPILOTMEGA,
base_mode = (byte)MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED,
system_status = (byte)MAVLink.MAV_STATE.ACTIVE,
type = (byte)MAVLink.MAV_TYPE.QUADROTOR
}, false, 1, 1, seqno++);
mavlinkudp.Send(sendpacket, sendpacket.Length, ipEP);
//sendpacket = mavparse.GenerateMAVLinkPacket20(MAVLink.MAVLINK_MSG_ID.AUTOPILOT_VERSION,new MAVLink.mavlink_autopilot_version_t(){ }, false, 1, 1, seqno++);
//mavlinkudp.Send(sendpacket, sendpacket.Length, ipEP);
sendpacket = mavparse.GenerateMAVLinkPacket20(MAVLink.MAVLINK_MSG_ID.AHRS,
new MAVLink.mavlink_ahrs_t()
{
}, false, 1, 1, seqno++);
mavlinkudp.Send(sendpacket, sendpacket.Length, ipEP);
sendpacket = mavparse.GenerateMAVLinkPacket20(MAVLink.MAVLINK_MSG_ID.ATTITUDE,
new MAVLink.mavlink_attitude_t()
{
pitch = 0, roll = 0, yaw = 0, time_boot_ms = (uint)DateTime.Now.Ticks
}, false, 1, 1, seqno++);
mavlinkudp.Send(sendpacket, sendpacket.Length, ipEP);
sendpacket = mavparse.GenerateMAVLinkPacket20(MAVLink.MAVLINK_MSG_ID.GLOBAL_POSITION_INT,
new MAVLink.mavlink_global_position_int_t()
{
lat = (int)(-35 * 1e7), lon = (int)(117.89 * 1e7), alt = (40 * 1000), hdg = 45 * 100,
time_boot_ms = (uint)DateTime.Now.Ticks
}, false, 1, 1, seqno++);
mavlinkudp.Send(sendpacket, sendpacket.Length, ipEP);
sendpacket = mavparse.GenerateMAVLinkPacket20(MAVLink.MAVLINK_MSG_ID.PARAM_VALUE,
new MAVLink.mavlink_param_value_t()
{
param_count = 0, param_index = 0, param_type = 0, param_value = 0
}, false, 1, 1, seqno++);
mavlinkudp.Send(sendpacket, sendpacket.Length, ipEP);
Thread.Sleep(100);
}
}
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
{
}
}
static void doread(object o)
{
lock (runlock)
{
running++;
}
var portname = (string)o;
Console.WriteLine("Scanning {0}", portname);
try
{
SerialPort port = new SerialPort();
{
port.PortName = portname;
foreach (var baud in bauds)
{
// try default baud
if (baud != 0)
{
port.BaudRate = baud;
}
port.Open();
port.DiscardInBuffer();
// let data flow in
Thread.Sleep(2000);
lock (runlock)
{
if (run == 0)
{
port.Close();
return;
}
}
int available = port.BytesToRead;
var buffer = new byte[available];
int read = port.Read(buffer, 0, available);
Console.WriteLine("{0} {1}", portname, read);
if (read > 0)
{
using (MemoryStream ms = new MemoryStream(buffer, 0, read))
{
MAVLink.MavlinkParse mav = new MAVLink.MavlinkParse();
try
{
again:
var packet = mav.ReadPacket(ms);
if (packet != null && packet.Length > 0)
{
port.Close();
Console.WriteLine("Found Mavlink on port {0} at {1}", port.PortName,
port.BaudRate);
foundport = true;
portinterface.Add(port);
if (connect)
{
doconnect(port);
}
break;
}
Console.WriteLine(portname + " crc: " + mav.badCRC);
goto again;
}
catch (Exception ex)
{
Console.WriteLine(portname + " " + ex.ToString());
}
}
}
try
{
port.Close();
}
catch (Exception ex)
{
Console.WriteLine(ex.ToString());
}
if (foundport)
{
break;
}
}
}
}
catch (Exception ex)
{
Console.WriteLine(portname + " " + ex.ToString());
}
finally
{
lock (runlock)
{
running--;
if (running == 0)
{
run = 0;
}
}
}
Console.WriteLine("Scan port {0} Finished!!", portname);
}
public static void tlog(string logfile)
{
List <MLArray> mlList = new List <MLArray>();
Hashtable datappl = new Hashtable();
using (Comms.CommsFile cf = new CommsFile(logfile))
using (CommsStream cs = new CommsStream(cf, cf.BytesToRead))
{
MAVLink.MavlinkParse parse = new MAVLink.MavlinkParse(true);
while (cs.Position < cs.Length)
{
MAVLink.MAVLinkMessage packet = parse.ReadPacket(cs);
if (packet == null)
{
continue;
}
object data = packet.data;
if (data == null)
{
continue;
}
if (data is MAVLink.mavlink_heartbeat_t)
{
if (((MAVLink.mavlink_heartbeat_t)data).type == (byte)MAVLink.MAV_TYPE.GCS)
{
continue;
}
}
Type test = data.GetType();
DateTime time = packet.rxtime;
double matlabtime = GetMatLabSerialDate(time);
try
{
foreach (var field in test.GetFields())
{
// field.Name has the field's name.
object fieldValue = field.GetValue(data); // Get value
if (field.FieldType.IsArray)
{
}
else
{
if (!datappl.ContainsKey(field.Name + "_" + field.DeclaringType.Name))
{
datappl[field.Name + "_" + field.DeclaringType.Name] = new List <double[]>();
}
List <double[]> list =
((List <double[]>)datappl[field.Name + "_" + field.DeclaringType.Name]);
object value = fieldValue;
if (value.GetType() == typeof(Single))
{
list.Add(new double[] { matlabtime, (double)(Single)field.GetValue(data) });
}
else if (value.GetType() == typeof(short))
{
list.Add(new double[] { matlabtime, (double)(short)field.GetValue(data) });
}
else if (value.GetType() == typeof(ushort))
{
list.Add(new double[] { matlabtime, (double)(ushort)field.GetValue(data) });
}
else if (value.GetType() == typeof(byte))
{
list.Add(new double[] { matlabtime, (double)(byte)field.GetValue(data) });
}
else if (value.GetType() == typeof(sbyte))
{
list.Add(new double[] { matlabtime, (double)(sbyte)field.GetValue(data) });
}
else if (value.GetType() == typeof(Int32))
{
list.Add(new double[] { matlabtime, (double)(Int32)field.GetValue(data) });
}
else if (value.GetType() == typeof(UInt32))
{
list.Add(new double[] { matlabtime, (double)(UInt32)field.GetValue(data) });
}
else if (value.GetType() == typeof(ulong))
{
list.Add(new double[] { matlabtime, (double)(ulong)field.GetValue(data) });
}
else if (value.GetType() == typeof(long))
{
list.Add(new double[] { matlabtime, (double)(long)field.GetValue(data) });
}
else if (value.GetType() == typeof(double))
{
list.Add(new double[] { matlabtime, (double)(double)field.GetValue(data) });
}
else
{
Console.WriteLine("Unknown data type");
}
}
}
}
catch
{
}
}
}
foreach (string item in datappl.Keys)
{
double[][] temp = ((List <double[]>)datappl[item]).ToArray();
MLArray dbarray = new MLDouble(item.Replace(" ", "_"), temp);
mlList.Add(dbarray);
}
try
{
MatFileWriter mfw = new MatFileWriter(logfile + ".mat", mlList, false);
}
catch (Exception err)
{
throw new Exception("There was an error when creating the MAT-file: \n" + err.ToString(), err);
}
}
private void ListenForData()
{
try {
socketConnection = new TcpClient("localhost", 14550);
Byte[] bytes = new Byte[1024];
Debug.Log("LISTENING!!!!");
using (NetworkStream stream = socketConnection.GetStream()) {
while (true)
{
// Get a stream object for reading
//int length;
MAVLink.MavlinkParse parser = new MAVLink.MavlinkParse();
MAVLink.MAVLinkMessage message = parser.ReadPacket(stream);
/*// Read incomming stream into byte arrary.
* if (message.data is MAVLink.mavlink_vfr_hud_t hud) {
* data["heading"] = hud.heading.ToString();
* data["airspeed"] = hud.airspeed.ToString();
* data["altitude"] = hud.alt;
*
* }
* if (message.data is MAVLink.mavlink_high_latency_t newData) {
* data["longitude"] = newData.longitude.ToString();
* }
* } */
switch (message.data)
{
case MAVLink.mavlink_vfr_hud_t newData:
data["heading"] = newData.heading.ToString();
data["airspeed"] = this.FormatNumber(newData.airspeed.ToString());
break;
case MAVLink.mavlink_global_position_int_t newData:
var longitudeString = newData.lon.ToString();
data["longitude"] = longitudeString.Substring(0, longitudeString.Length - 7) + "." + longitudeString.Substring(longitudeString.Length - 7);
var latitudeString = newData.lat.ToString();
data["latitude"] = latitudeString.Substring(0, latitudeString.Length - 7) + "." + latitudeString.Substring(latitudeString.Length - 7);
var altitudeString = newData.alt.ToString();
data["altitude"] = this.FormatNumber(altitudeString.Substring(0, altitudeString.Length - 4) + "." + altitudeString.Substring(altitudeString.Length - 4));
//var relString = newData.relative_alt.ToString();
//var rel = this.FormatNumber(relString.Substring(0, relString.Length - 4) + "." + relString.Substring(relString.Length - 4));
if (!initialPositionRecieved && get("longitude") != "" && get("latitude") != "" && get("altitude") != "")
{
initialPositionRecieved = true;
}
break;
case MAVLink.mavlink_attitude_t newData:
data["roll"] = this.FormatNumber(newData.roll.ToString());
data["pitch"] = this.FormatNumber(newData.pitch.ToString());
data["yaw"] = this.FormatNumber(newData.yaw.ToString());
break;
}
//Debug.Log(message.msgtypename);
}
}
}
catch (SocketException socketException) {
Debug.Log("Socket exception: " + socketException);
}
}
public static void Start()
{
var config = Settings.Instance[SettingsName];
if (config == null)
{
Settings.Instance[SettingsName] = connectionInfos.ToJSON();
config = Settings.Instance[SettingsName];
}
connectionInfos = config.FromJSON <List <ConnectionInfo> >();
foreach (var connectionInfo in connectionInfos)
{
if (connectionInfo.Enabled == false)
{
continue;
}
log.Info(connectionInfo.ToJSON());
if (connectionInfo.Format == ConnectionFormat.MAVLink)
{
if (connectionInfo.Protocol == ProtocolType.Udp && connectionInfo.Direction == Direction.Inbound)
{
try
{
var client = new UdpClient(connectionInfo.Port);
client.ExclusiveAddressUse = false;
client.BeginReceive(clientdataMAVLink, client);
}
catch (Exception ex)
{
log.Error(ex);
}
continue;
}
if (connectionInfo.Protocol == ProtocolType.Udp && connectionInfo.Direction == Direction.Outbound)
{
try
{
// create and set default dest
var client = new UdpClient(connectionInfo.ConfigString, connectionInfo.Port);
client.SendAsync(new byte[] { 0 }, 1);
client.BeginReceive(clientdataMAVLink, client);
}
catch (Exception ex)
{
log.Error(ex);
}
continue;
}
if (connectionInfo.Protocol == ProtocolType.Serial &&
connectionInfo.Direction == Direction.Outbound)
{
try
{
// try anything already connected
Task.Run(() =>
{
Parallel.ForEach(SerialPort.GetPortNames(), port =>
{
try
{
var serial = new SerialPort(port, connectionInfo.Port);
serial.ReadBufferSize = 1024 * 300;
serial.Open();
// sample 1.2seconds
Thread.Sleep(1200);
var btr = serial.BytesToRead;
var buffer = new byte[btr];
serial.Read(buffer, 0, buffer.Length);
serial.Close();
var parse = new MAVLink.MavlinkParse();
var st = buffer.ToMemoryStream();
while (st.Position < st.Length)
{
var packet = parse.ReadPacket(st);
if (packet != null)
{
if (packet.msgid == (int)MAVLink.MAVLINK_MSG_ID.HEARTBEAT)
{
NewMavlinkConnection?.BeginInvoke(null, serial, null, null);
return;
}
}
}
}
catch
{
}
});
});
}
catch (Exception ex)
{
log.Error(ex);
}
continue;
}
}
else if (connectionInfo.Format == ConnectionFormat.Video)
{
if (connectionInfo.Protocol == ProtocolType.Udp && connectionInfo.Direction == Direction.Inbound)
{
try
{
var client = new UdpClient(connectionInfo.Port, AddressFamily.InterNetwork);
client.BeginReceive(clientdataVideo, client);
}
catch (Exception ex)
{
log.Error(ex);
}
continue;
}
if (connectionInfo.Protocol == ProtocolType.Tcp && connectionInfo.Direction == Direction.Inbound)
{
try
{
var client = new TcpListener(IPAddress.Any, connectionInfo.Port);
client.Start();
client.BeginAcceptTcpClient(clientdatatcpvideo, client);
}
catch (Exception ex)
{
log.Error(ex);
}
continue;
}
if (connectionInfo.Direction == Direction.Outbound)
{
NewVideoStream?.BeginInvoke(null,
connectionInfos.First(a => a == connectionInfo).ConfigString, null, null);
continue;
}
}
}
}
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, string masterdestdir = "")
{
Parallel.ForEach(logs, logfile =>
//foreach (var logfile in logs)
{
if (masterdestdir == "")
{
masterdestdir = Path.GetDirectoryName(logfile);
}
try
{
FileInfo info;
info = new FileInfo(logfile);
// delete 0 size files
if (info.Length == 0)
{
try
{
File.Delete(logfile);
}
catch
{
}
return;
}
// 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
{
}
return;
}
bool issitl = false;
var sysid = 0;
var compid = 0;
var aptype = MAVLink.MAV_TYPE.GENERIC;
var packetsseen = 0;
var parse = new MAVLink.MavlinkParse(true);
using (var binfile = File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read))
{
var midpoint = binfile.Length / 8;
binfile.Seek(midpoint, SeekOrigin.Begin);
MAVLink.MAVLinkMessage packet;
List <MAVLink.MAVLinkMessage> hblist = new List <MAVLink.MAVLinkMessage>();
try
{
var length = binfile.Length;
while (binfile.Position < length)
{
packet = parse.ReadPacket(binfile);
// no gcs packets
if (packet == null || packet.compid == 190)
{
continue;
}
if (packet.msgid == (uint)MAVLink.MAVLINK_MSG_ID.SIMSTATE)
{
packetsseen++;
issitl = true;
}
else if (packet.msgid == (uint)MAVLink.MAVLINK_MSG_ID.HEARTBEAT)
{
packetsseen++;
hblist.Add(packet);
var hb = (MAVLink.mavlink_heartbeat_t)packet.data;
sysid = packet.sysid;
compid = packet.compid;
aptype = (MAVLink.MAV_TYPE)hb.type;
if (hblist.Count > 10)
{
break;
}
}
else if (packet != MAVLink.MAVLinkMessage.Invalid)
{
packetsseen++;
if (sysid == 0)
{
sysid = packet.sysid;
}
if (compid == 0)
{
compid = packet.compid;
}
}
}
} catch (EndOfStreamException) { }
catch { }
if (hblist.Count == 0)
{
binfile.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) + " " + info.Length);
MoveFileUsingMask(logfile,
masterdestdir + Path.DirectorySeparatorChar + "BAD" +
Path.DirectorySeparatorChar);
return;
}
// find most appropriate
if (hblist.GroupBy(a => a.sysid * 256 + a.compid).ToArray().Length > 1)
{
foreach (var mav in hblist)
{
var hb = (MAVLink.mavlink_heartbeat_t)mav.data;
if (hb.type == (byte)MAVLink.MAV_TYPE.ANTENNA_TRACKER)
{
continue;
}
if (hb.type == (byte)MAVLink.MAV_TYPE.GCS)
{
continue;
}
sysid = mav.sysid;
compid = mav.compid;
aptype = (MAVLink.MAV_TYPE)hb.type;
}
}
binfile.Close();
string destdir = masterdestdir + Path.DirectorySeparatorChar
+ aptype.ToString() + Path.DirectorySeparatorChar
+ sysid + Path.DirectorySeparatorChar;
if (issitl)
{
destdir = masterdestdir + Path.DirectorySeparatorChar
+ "SITL" + Path.DirectorySeparatorChar
+ aptype.ToString() + Path.DirectorySeparatorChar
+ sysid + Path.DirectorySeparatorChar;
}
if (!Directory.Exists(destdir))
{
Directory.CreateDirectory(destdir);
}
MoveFileUsingMask(logfile, destdir);
}
}
catch
{
return;
}
});
}
public static void anonymise(string logfile, string outputfile)
{
var latrandom = new Random().NextDouble();
//LOG
using (CollectionBuffer col = new CollectionBuffer(File.OpenRead(logfile)))
using (var outfilestream = File.Open(outputfile, FileMode.Create, FileAccess.ReadWrite, FileShare.Read))
{
foreach (var dfItem in col.GetEnumeratorTypeAll())
{
var index = col.dflog.FindMessageOffset(dfItem.msgtype, "lat");
if (index != -1)
{
dfItem.items[index] =
(Double.Parse(dfItem.items[index], CultureInfo.InvariantCulture) + latrandom).ToString(
CultureInfo.InvariantCulture);
}
var str = String.Join(",", dfItem.items) + "\r\n";
outfilestream.Write(ASCIIEncoding.ASCII.GetBytes(str), 0, str.Length);
}
}
return;
// TLOG
Comms.CommsFile tlogFile = new CommsFile();
tlogFile.Open(logfile);
using (var stream = new CommsStream(tlogFile, tlogFile.BytesToRead))
using (var outfilestream = File.Open(outputfile, FileMode.Create, FileAccess.ReadWrite, FileShare.Read))
{
MAVLink.MavlinkParse parse = new MAVLink.MavlinkParse();
double lat = 0;
double lng = 0;
while (stream.Position < stream.Length)
{
var packet = parse.ReadPacket(stream);
if (packet.msgid == (uint)MAVLink.MAVLINK_MSG_ID.GLOBAL_POSITION_INT)
{
packet.ToStructure <MAVLink.mavlink_global_position_int_t>();
}
if (packet.msgid == (uint)MAVLink.MAVLINK_MSG_ID.POSITION_TARGET_GLOBAL_INT)
{
}
if (packet.msgid == (uint)MAVLink.MAVLINK_MSG_ID.SET_HOME_POSITION)
{
}
if (packet.msgid == (uint)MAVLink.MAVLINK_MSG_ID.HOME_POSITION)
{
}
if (packet.msgid == (uint)MAVLink.MAVLINK_MSG_ID.SET_POSITION_TARGET_GLOBAL_INT)
{
}
if (packet.msgid == (uint)MAVLink.MAVLINK_MSG_ID.LOCAL_POSITION_NED)
{
}
if (packet.msgid == (uint)MAVLink.MAVLINK_MSG_ID.COMMAND_LONG)
{
}
if (packet.msgid == (uint)MAVLink.MAVLINK_MSG_ID.MISSION_ITEM)
{
}
if (packet.msgid == (uint)MAVLink.MAVLINK_MSG_ID.MISSION_ITEM_INT)
{
}
byte[] datearray =
BitConverter.GetBytes(
(UInt64)((DateTime.UtcNow - new DateTime(1970, 1, 1)).TotalMilliseconds * 1000));
Array.Reverse(datearray);
outfilestream.Write(datearray, 0, datearray.Length);
outfilestream.Write(packet.buffer, 0, packet.Length);
}
}
}
void RecvData()
{
IPEndPoint myGCS = new IPEndPoint(IPAddress.Parse("127.0.0.1"), 17500);
byte[] buf = new byte[MAVLink.MAVLINK_MAX_PACKET_LEN];
MAVLink.MavlinkParse mavlinkParse = new MAVLink.MavlinkParse();
Socket sock = new Socket(AddressFamily.InterNetwork, SocketType.Dgram, ProtocolType.Udp);
sock.ReceiveTimeout = 1000;
sock.Bind(new IPEndPoint(IPAddress.Loopback, 17501));
while (gogo)
{
int recvBytes = 0;
try
{
recvBytes = sock.Receive(buf);
}
catch (SocketException)
{
}
if (recvBytes > 0)
{
MAVLink.MAVLinkMessage msg = mavlinkParse.ReadPacket(buf);
if (msg != null && msg.data != null)
{
System.Type msg_type = msg.data.GetType();
//Debug.Log("recv "+msg_type);
if (msg_type == typeof(MAVLink.mavlink_heartbeat_t))
{
if (!gotPos)
{
MAVLink.mavlink_command_long_t msgOut = new MAVLink.mavlink_command_long_t()
{
target_system = 0,
command = (ushort)MAVLink.MAV_CMD.SET_MESSAGE_INTERVAL,
param1 = (float)MAVLink.MAVLINK_MSG_ID.LOCAL_POSITION_NED,
param2 = 100000
};
byte[] data = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, msgOut);
sock.SendTo(data, myGCS);
}
if (!gotAtt)
{
MAVLink.mavlink_command_long_t msgOut = new MAVLink.mavlink_command_long_t()
{
target_system = 0,
command = (ushort)MAVLink.MAV_CMD.SET_MESSAGE_INTERVAL,
param1 = (float)MAVLink.MAVLINK_MSG_ID.ATTITUDE_QUATERNION,
param2 = 100000
};
byte[] data = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, msgOut);
sock.SendTo(data, myGCS);
}
}
else if (msg_type == typeof(MAVLink.mavlink_local_position_ned_t))
{
if (!gotPos)
{
Debug.Log("local_position_ned received");
}
gotPos = true;
var data = (MAVLink.mavlink_local_position_ned_t)msg.data;
pos = new Vector3(-data.x, -data.z, data.y);
//Debug.Log("recv local_position_ned " + pos.ToString("F4"));
}
else if (msg_type == typeof(MAVLink.mavlink_attitude_quaternion_t))
{
if (!gotAtt)
{
Debug.Log("attitude_quaternion received");
}
gotAtt = true;
var data = (MAVLink.mavlink_attitude_quaternion_t)msg.data;
att = new Quaternion(-data.q2, -data.q4, data.q3, -data.q1);
}
}
}
}
}
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
{
}
}
public VehicleModel(Stream stream)
{
_mavlink_parse = new MAVLink.MavlinkParse();
_datastream = stream;
}
public static void ProcessFile(string logfile)
{
if (File.Exists(logfile + ".jpg"))
{
return;
}
double minx = 99999;
double maxx = -99999;
double miny = 99999;
double maxy = -99999;
bool sitl = false;
Dictionary <int, List <PointLatLngAlt> > loc_list = new Dictionary <int, List <PointLatLngAlt> >();
try
{
if (logfile.ToLower().EndsWith(".tlog"))
{
Comms.CommsFile cf = new CommsFile();
cf.Open(logfile);
using (CommsStream cs = new CommsStream(cf, cf.BytesToRead))
{
MAVLink.MavlinkParse parse = new MAVLink.MavlinkParse(true);
while (cs.Position < cs.Length)
{
try
{
MAVLink.MAVLinkMessage packet = parse.ReadPacket(cs);
if (packet == null || packet.Length < 5)
{
continue;
}
if (packet.msgid == (byte)MAVLink.MAVLINK_MSG_ID.SIM_STATE ||
packet.msgid == (byte)MAVLink.MAVLINK_MSG_ID.SIMSTATE)
{
sitl = true;
}
if (packet.msgid == (byte)MAVLink.MAVLINK_MSG_ID.GLOBAL_POSITION_INT)
{
var loc = packet.ToStructure <MAVLink.mavlink_global_position_int_t>();
if (loc.lat == 0 || loc.lon == 0)
{
continue;
}
var id = packet.sysid * 256 + packet.compid;
if (!loc_list.ContainsKey(id))
{
loc_list[id] = new List <PointLatLngAlt>();
}
loc_list[id].Add(new PointLatLngAlt(loc.lat / 10000000.0f, loc.lon / 10000000.0f));
minx = Math.Min(minx, loc.lon / 10000000.0f);
maxx = Math.Max(maxx, loc.lon / 10000000.0f);
miny = Math.Min(miny, loc.lat / 10000000.0f);
maxy = Math.Max(maxy, loc.lat / 10000000.0f);
}
}
catch { }
}
}
cf.Close();
}
else if (logfile.ToLower().EndsWith(".bin") || logfile.ToLower().EndsWith(".log"))
{
using (
DFLogBuffer colbuf =
new DFLogBuffer(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.ReadWrite))
)
{
loc_list[0] = new List <PointLatLngAlt>();
foreach (var item in colbuf.GetEnumeratorType("GPS"))
{
if (item.msgtype.StartsWith("GPS"))
{
if (!colbuf.dflog.logformat.ContainsKey("GPS"))
{
continue;
}
var status =
double.Parse(item.items[colbuf.dflog.FindMessageOffset(item.msgtype, "Status")]);
var lat = double.Parse(item.items[colbuf.dflog.FindMessageOffset(item.msgtype, "Lat")]);
var lon = double.Parse(item.items[colbuf.dflog.FindMessageOffset(item.msgtype, "Lng")]);
if (lat == 0 || lon == 0 || status < 3)
{
continue;
}
loc_list[0].Add(new PointLatLngAlt(lat, lon));
minx = Math.Min(minx, lon);
maxx = Math.Max(maxx, lon);
miny = Math.Min(miny, lat);
maxy = Math.Max(maxy, lat);
}
}
}
}
if (loc_list.Count > 0 && loc_list.First().Value.Count > 10)
{
// add a bit of buffer
var area = RectLatLng.FromLTRB(minx - 0.001, maxy + 0.001, maxx + 0.001, miny - 0.001);
using (var map = GetMap(area))
using (var grap = Graphics.FromImage(map))
{
if (sitl)
{
AddTextToMap(grap, "SITL");
}
Color[] colours =
{
Color.Red, Color.Orange, Color.Yellow, Color.Green, Color.Blue, Color.Indigo,
Color.Violet, Color.Pink
};
int a = 0;
foreach (var locs in loc_list.Values)
{
PointF lastpoint = new PointF();
var pen = new Pen(colours[a % (colours.Length - 1)]);
foreach (var loc in locs)
{
PointF newpoint = GetPixel(area, loc, map.Size);
if (!lastpoint.IsEmpty)
{
grap.DrawLine(pen, lastpoint, newpoint);
}
lastpoint = newpoint;
}
a++;
}
map.Save(logfile + ".jpg", SKEncodedImageFormat.Jpeg);
File.SetLastWriteTime(logfile + ".jpg", new FileInfo(logfile).LastWriteTime);
}
}
else
{
DoTextMap(logfile + ".jpg", "No gps data");
File.SetLastWriteTime(logfile + ".jpg", new FileInfo(logfile).LastWriteTime);
}
}
catch (Exception ex)
{
if (ex.ToString().Contains("Mavlink 0.9"))
{
DoTextMap(logfile + ".jpg", "Old log\nMavlink 0.9");
}
}
}
