| public GenerateMAVLinkPacket10 ( MAVLINK_MSG_ID messageType, object indata ) : byte[] | ||
| messageType | MAVLINK_MSG_ID | |
| indata | object | |
| return | byte[] | |
public void TakeOff()
{
MAVLink.mavlink_command_long_t cmd = new MAVLink.mavlink_command_long_t
{
command = (ushort)MAVLink.MAV_CMD.TAKEOFF,
param7 = 1f
};
byte[] data = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, cmd);
sock.SendTo(data, myproxy);
}
private void SendHome()
{
var cl = new MAVLink.mavlink_command_long_t(0, 0, 0, 0, (float)(HomeLoc.Lat * 1e7),
(float)(HomeLoc.Lng * 1e7),
(float)(HomeLoc.Alt * 1e2),
(ushort)MAVLink.MAV_CMD.DO_SET_HOME, 1, 1, 1);
var home = mavlink.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, cl, 255, 190, sequence++);
ATStream.Write(home, 0, home.Length);
}
public void GuidedClick()
{
//Debug.Log("clicked");
MAVLink.mavlink_set_mode_t cmd = new MAVLink.mavlink_set_mode_t
{
base_mode = (byte)MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED,
target_system = 0,
custom_mode = 4
};
byte[] data = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.SET_MODE, cmd);
sock.SendTo(data, ep);
StartCoroutine(SetGPSOrigin());
}
void TestSend1(object sender, DoWorkEventArgs e)//sender与e为开启单独线程所用
{
while (serialPort1.IsOpen)
{
if (DateTime.Now > Time4TestSend)
{
MAVLink.mavlink_command_long_t req1 = new MAVLink.mavlink_command_long_t();
req1.target_system = sysid;
req1.target_component = compid;
req1.command = (ushort)2;
byte[] packet1 = mavlink.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, req1);
serialPort1.Write(packet1, 0, packet1.Length);
Time4TestSend = DateTime.Now.AddSeconds(1);
}
}
}
public void doMotorTest(int motor, int throttle = 0, int duration = 0)
{
// deklarasi untuk pengiriman data paket.
MAVLink.mavlink_command_long_t request = new MAVLink.mavlink_command_long_t();
// mengarahkan dari aplikasi ke apm, tidak ke perangkat lain.
request.target_system = 1;
request.target_component = 1;
// menentukan target command dari indeks DO_MOTOR_TEST (209).
request.command = (ushort)MAVLink.MAV_CMD.DO_MOTOR_TEST;
request.param1 = motor;
request.param2 = (byte)MAVLink.MOTOR_TEST_THROTTLE_TYPE.MOTOR_TEST_THROTTLE_PERCENT;
request.param3 = throttle;
request.param4 = duration;
byte[] packet = mavlink.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, request);
serialPort1.Write(packet, 0, packet.Length);
}
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
{
}
}
void bgw_DoWork(object sender, DoWorkEventArgs e)
{
while (serialPort1.IsOpen)
{
try
{
MAVLink.MAVLinkMessage packet;
lock (readlock)
{
// read any valid packet from the port
packet = mavlink.ReadPacket(serialPort1.BaseStream);
// check its valid
if (packet == null || packet.data == null)
{
continue;
}
}
// check to see if its a hb packet from the comport
if (packet.data.GetType() == typeof(MAVLink.mavlink_heartbeat_t))
{
var hb = (MAVLink.mavlink_heartbeat_t)packet.data;
// save the sysid and compid of the seen MAV
sysid = packet.sysid;
compid = packet.compid;
// request streams at 2 hz
var buffer = mavlink.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.REQUEST_DATA_STREAM,
new MAVLink.mavlink_request_data_stream_t()
{
req_message_rate = 2,
req_stream_id = (byte)MAVLink.MAV_DATA_STREAM.ALL,
start_stop = 1,
target_component = compid,
target_system = sysid
});
serialPort1.Write(buffer, 0, buffer.Length);
buffer = mavlink.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.HEARTBEAT, hb);
serialPort1.Write(buffer, 0, buffer.Length);
}
// from here we should check the the message is addressed to us
if (sysid != packet.sysid || compid != packet.compid)
{
continue;
}
Console.WriteLine(packet.msgtypename);
if (packet.msgid == (byte)MAVLink.MAVLINK_MSG_ID.ATTITUDE)
//or
//if (packet.data.GetType() == typeof(MAVLink.mavlink_attitude_t))
{
var att = (MAVLink.mavlink_attitude_t)packet.data;
Console.WriteLine(att.pitch * 57.2958 + " " + att.roll * 57.2958);
/* custom code(Deriech)
* Idea: To test if GUI ca be updated while packets are updating as well.
* Exception: Cross-thread operation not valid: Control "Lab_roll" accessed from
* a thread other than the one it was created on
* Solution: Using invoked methods online (Methods will have comments such as these)
* Update(1/28/19 11:49PM): added delagate classes, but error on line 16.
* Update(1/30/19 12:42PM): deleted said methods and adding my own, due to more understanding of
* what delagates are
*/
roll_yaw = att.pitch * 57.2958 + " " + att.roll * 57.2958;
if (label_att.InvokeRequired)
{
this.Invoke(setYawAndRollDelagate);
}
}
if (packet.msgid == (byte)MAVLink.MAVLINK_MSG_ID.GLOBAL_POSITION_INT)
//or
//if (packet.data.GetType() == typeof(MAVLink.mavlink_attitude_t))
{
var alt = (MAVLink.mavlink_global_position_int_t)packet.data;
Console.WriteLine(alt.alt * 0.00328084);
altitude = alt.alt * 0.00328084 + "";
if (label_gps.InvokeRequired)
{
this.Invoke(setAltitudeDelagate);
}
}
}
catch
{
}
System.Threading.Thread.Sleep(1);
}
}
void bgw_DoWork(object sender, DoWorkEventArgs e)
{
while (serialPort1.IsOpen)
{
try//首先需要申请全部数据包，否则只能收到心跳包！*****
{
//只有此处申请66号信息包才能获得全部参数！
//放在监听中无法实时触发！
MAVLink.mavlink_request_data_stream_t r = new MAVLink.mavlink_request_data_stream_t();
r.req_message_rate = 2;
r.req_stream_id = (byte)MAVLink.MAV_DATA_STREAM.ALL;
r.start_stop = 1;
r.target_component = compid;
r.target_system = sysid;
byte[] packetRequest = mavlink.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.REQUEST_DATA_STREAM, r);
serialPort1.Write(packetRequest, 0, packetRequest.Length);
MAVLink.MAVLinkMessage packet; //定义一个mavlink消息解析类
lock (readlock) //锁定读取端口的当前数据当前数据
{
//读取端口的数据流
packet = mavlink.ReadPacket(serialPort1.BaseStream);
if (packet == null || packet.data == null)
{
continue;
}
}
//判断是否为心跳包，如果是向向飞控申请数据流
//
if (packet.data.GetType() == typeof(MAVLink.mavlink_heartbeat_t))
{
var hb = (MAVLink.mavlink_heartbeat_t)packet.data;
// 保存包sysid和compid参数
sysid = packet.sysid;
compid = packet.compid;
Console.WriteLine("飞控数据接收成功：");
//向飞控申请数据流
/* mavlink.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.REQUEST_DATA_STREAM,
* //飞控数据流的格式要求
* new MAVLink.mavlink_request_data_stream_t()
* {//Mavlink的一个结构体数组
* req_message_rate = 2,//信息频率2hz
* //数据流ID，ALL枚举=0，代表获得全部信息
* req_stream_id = (byte)MAVLink.MAV_DATA_STREAM.ALL,
*
* start_stop = 1,//1开始发送，0停止发送
* target_component = compid,
* target_system = sysid
* });*/
}
// 核对发送的信息
if (sysid != packet.sysid || compid != packet.compid)
{
continue;
}
// Console.WriteLine("packet.msgid = " + packet.msgid);
if (packet.msgid == 0)
{
var mode1 = (MAVLink.mavlink_heartbeat_t)packet.data;
Console.WriteLine("当前模式" + mode1.base_mode);
string s = mode1.base_mode.ToString();
int n = Convert.ToInt32(s);
/* MAV_MODE ****
* 0 MAV_MODE_PREFLIGHT
* 80 MAV_MODE_STABILIZE_DISARMED
* 208 MAV_MODE_STABILIZE_ARMED
* 88 MAV_MODE_GUIDED_DISARMED
* 216 MAV_MODE_GUIDED_ARMED
*/
string str = null;
switch (n)
{
case 1: str = "设备故障"; break;
case 81: str = "自稳锁定"; break;
case 209: str = "自稳解锁"; break;
case 89: str = "引导锁定"; break;
case 217: str = "引导解锁"; break;
default: str = "其他模式"; break;
}
if (fly.Text.Equals("已起飞") && str.Equals("自稳锁定"))
{
fly.Text = "等待起飞..";
}
textBox1.Text = str;
}
if (packet.msgid == (byte)MAVLink.MAVLINK_MSG_ID.ALTITUDE)
//#141号信息包
{
var alti = (MAVLink.mavlink_altitude_t)packet.data;
Console.WriteLine("当前高度" + alti.altitude_relative);
}
if (packet.msgid == (byte)MAVLink.MAVLINK_MSG_ID.GPS_RAW_INT)
//#24号信息包
{
var sta = (MAVLink.mavlink_gps_raw_int_t)packet.data;
Console.WriteLine("地速度" + sta.vel / 100.0);
Console.WriteLine("卫星数量" + sta.satellites_visible);
int n = (int)(sta.satellites_visible & 0xFF);
gps.Text = Convert.ToString(n);
}
if (packet.msgid == (byte)MAVLink.MAVLINK_MSG_ID.GLOBAL_POSITION_INT)
//#33号信息包
{
var glo = (MAVLink.mavlink_global_position_int_t)packet.data;
Console.WriteLine("垂直速度" + glo.vz / 100.0);
Console.WriteLine("相对高度" + glo.relative_alt / 1000.0);
}
if (packet.msgid == 62)
//#62号信息包 NAV_CONTROLLER_OUTPUT
{
var nav = (MAVLink.mavlink_nav_controller_output_t)packet.data;
Console.WriteLine("高度误差" + nav.alt_error);
error.Text = nav.alt_error.ToString("f2");
}
if (packet.msgid == 74)
//#74号信息包 VFR_HUD
{
var vfr = (MAVLink.mavlink_vfr_hud_t)packet.data;
Console.WriteLine("空速" + vfr.airspeed);
airv.Text = Convert.ToString(vfr.airspeed);
Console.WriteLine("地速" + vfr.groundspeed);
grov.Text = Convert.ToString(vfr.groundspeed);
Console.WriteLine("高度" + vfr.alt);
alt.Text = Convert.ToString(vfr.alt);
Console.WriteLine("升降速度" + vfr.climb);
clime.Text = Convert.ToString(vfr.climb);
}
}
catch
{
}
System.Threading.Thread.Sleep(1);//while循环通常加这句，防止堵死
}
}
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);
}
}
}
}
}
void bgw_DoWork(object sender, DoWorkEventArgs e)
{
while (serialPort1.IsOpen)
{
try
{
MAVLink.MAVLinkMessage packet;
lock (readlock)
{
// read any valid packet from the port
packet = mavlink.ReadPacket(serialPort1.BaseStream);
// check its valid
if (packet == null || packet.data == null)
{
continue;
}
}
// check to see if its a hb packet from the comport
if (packet.data.GetType() == typeof(MAVLink.mavlink_heartbeat_t))
{
var hb = (MAVLink.mavlink_heartbeat_t)packet.data;
// save the sysid and compid of the seen MAV
sysid = packet.sysid;
compid = packet.compid;
// request streams at 2 hz
var buffer = mavlink.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.REQUEST_DATA_STREAM,
new MAVLink.mavlink_request_data_stream_t()
{
req_message_rate = 2,
req_stream_id = (byte)MAVLink.MAV_DATA_STREAM.ALL,
start_stop = 1,
target_component = compid,
target_system = sysid
});
serialPort1.Write(buffer, 0, buffer.Length);
buffer = mavlink.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.HEARTBEAT, hb);
serialPort1.Write(buffer, 0, buffer.Length);
}
// from here we should check the the message is addressed to us
if (sysid != packet.sysid || compid != packet.compid)
{
continue;
}
//Console.WriteLine(packet.msgtypename);
listBox1.Invoke(new Action(() =>
{
if (!listBox1.Items.Contains(packet.msgtypename))
{
mav_messages_counter++;
if (mav_messages_counter > 0)
{
mav_messages_Label.ForeColor = Color.Green;
}
listBox1.Items.Add(packet.msgtypename);
mav_messages_Label.Text = "Found " + mav_messages_counter + " messages.";
}
}));
//detect gps packet
if (packet.msgid == (byte)MAVLink.MAVLINK_MSG_ID.GLOBAL_POSITION_INT)
{
marker.IsVisible = true;
var global_position_int = (MAVLink.mavlink_global_position_int_t)packet.data;
//get longitude and latitude info from packet
var lon = global_position_int.lon / 10000000.0;
var lat = global_position_int.lat / 10000000.0;
gmap.Invoke(new Action(() => //access gui thread
{
//draw on map
gmap.Position = new PointLatLng(lat, lon);
GMapMarker line = new GMarkerGoogle(new PointLatLng(lat, lon), new Bitmap(dot));
marker.Position = new PointLatLng(lat, lon);
lines.Markers.Add(line);
gmap.Overlays.Add(lines);
}));
}
//detect system status packet
if (packet.msgid == (byte)MAVLink.MAVLINK_MSG_ID.SYS_STATUS)
{
progressBar1.Invoke(new Action(() =>
{
var sys_status = (MAVLink.mavlink_sys_status_t)packet.data;
//get battery remaining
if (sys_status.battery_remaining < 15)
{
SetState(progressBar1, 2);
Console.Beep(600, 100);
}
else if (sys_status.battery_remaining < 50)
{
if (!half_battery_signal)
{
SetState(progressBar1, 3);
half_battery_signal = true;
Console.Beep(600, 1000);
}
}
else
{
SetState(progressBar1, 1);
}
progressBar1.Value = sys_status.battery_remaining;
}));
}
//get heartbeat packet
if (packet.msgid == (byte)MAVLink.MAVLINK_MSG_ID.HEARTBEAT)
{
var heartbeat = (MAVLink.mavlink_heartbeat_t)packet.data;
txt_vehicleType.Invoke(new Action(() => //access gui thread
{
//print information
txt_vehicleType.Text = ((MAVLink.MAV_TYPE)heartbeat.autopilot).ToString();
txt_autopilot.Text = ((MAVLink.MAV_AUTOPILOT)heartbeat.autopilot).ToString();
txt_baseMode.Text = ((MAVLink.MAV_MODE_FLAG)heartbeat.base_mode).ToString();
txt_system.Text = ((MAVLink.MAV_STATE)heartbeat.system_status).ToString();
}));
}
}
catch (Exception ex)
{
Console.WriteLine("--------------------------------------");
Console.WriteLine(ex.ToString());
Console.WriteLine("--------------------------------------");
}
System.Threading.Thread.Sleep(1);
}
}
//static uint GPS_LEAPSECONDS_MILLIS = 18000;
static void processFrameData(NatNetML.FrameOfMocapData data)
{
bool data_gogo = true;
/* Parsing Rigid Body Frame Data */
for (int i = 0; i < mRigidBodies.Count; i++)
{
int rbID = mRigidBodies[i].ID; // Fetching rigid body IDs from the saved descriptions
for (int j = 0; j < data.nRigidBodies; j++)
{
if (rbID == data.RigidBodies[j].ID) // When rigid body ID of the descriptions matches rigid body ID of the frame data.
{
NatNetML.RigidBody rb = mRigidBodies[i]; // Saved rigid body descriptions
NatNetML.RigidBodyData rbData = data.RigidBodies[j]; // Received rigid body descriptions
if (rbData.Tracked == true)
{
#if DEBUG_MSG
Console.WriteLine("\tRigidBody ({0}):", rb.Name);
Console.WriteLine("\t\tpos ({0:N3}, {1:N3}, {2:N3})", rbData.x, rbData.y, rbData.z);
// Rigid Body Euler Orientation
float[] quat = new float[4] {
rbData.qx, rbData.qy, rbData.qz, rbData.qw
};
float[] eulers = new float[3];
eulers = NatNetClientML.QuatToEuler(quat, NATEulerOrder.NAT_XYZr); //Converting quat orientation into XYZ Euler representation.
double xrot = RadiansToDegrees(eulers[0]);
double yrot = RadiansToDegrees(eulers[1]);
double zrot = RadiansToDegrees(eulers[2]);
Console.WriteLine("\t\tori ({0:N3}, {1:N3}, {2:N3})", xrot, yrot, zrot);
#endif
if (drones.ContainsKey(rb.Name))
{
DroneData drone = drones[rb.Name];
drone.lost_count = 0;
long cur_ms = stopwatch.ElapsedMilliseconds;
if (drone.send_count > 0)
{
drone.send_count--;
}
else if (data_gogo)
{
drone.send_count = 10;
data_gogo = false;
MAVLink.mavlink_att_pos_mocap_t att_pos = new MAVLink.mavlink_att_pos_mocap_t();
att_pos.time_usec = (ulong)(cur_ms * 1000);
att_pos.x = rbData.x; //north
att_pos.y = rbData.z; //east
att_pos.z = -rbData.y; //down
att_pos.q = new float[4] {
rbData.qw, rbData.qx, rbData.qz, -rbData.qy
};
byte[] pkt = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.ATT_POS_MOCAP, att_pos);
if (drone.lastTime >= 0)
{
MAVLink.mavlink_vision_speed_estimate_t vis_speed = new MAVLink.mavlink_vision_speed_estimate_t();
float total_s = (cur_ms - drone.lastTime) * 0.001f;
vis_speed.x = (rbData.x - drone.lastPosN) / total_s;
vis_speed.y = (rbData.z - drone.lastPosE) / total_s;
vis_speed.z = (-rbData.y - drone.lastPosD) / total_s;
vis_speed.usec = (ulong)(cur_ms * 1000);
byte[] pkt2 = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.VISION_SPEED_ESTIMATE, vis_speed);
int total_len = pkt.Length + pkt2.Length;
byte[] uwb_data = new byte[10 + total_len + 1];
uwb_data[0] = 0x54;
uwb_data[1] = 0xf1;
uwb_data[2] = 0xff;
uwb_data[3] = 0xff;
uwb_data[4] = 0xff;
uwb_data[5] = 0xff;
uwb_data[6] = 2;
uwb_data[7] = drone.uwb_tag_id;
uwb_data[8] = (byte)(total_len & 0xff);
uwb_data[9] = (byte)(total_len >> 16);
Array.Copy(pkt, 0, uwb_data, 10, pkt.Length);
Array.Copy(pkt2, 0, uwb_data, 10 + pkt.Length, pkt2.Length);
byte chk_sum = 0;
foreach (byte uwb_data_byte in uwb_data)
{
chk_sum += uwb_data_byte;
}
uwb_data[uwb_data.Length - 1] = chk_sum;
mavSock.SendTo(uwb_data, drone.ep);
}
}
drone.lastTime = cur_ms;
drone.lastPosN = rbData.x;
drone.lastPosE = rbData.z;
drone.lastPosD = -rbData.y;
}
}
else
{
Console.WriteLine("\t{0} is not tracked in current frame", rb.Name);
/*if (drones.ContainsKey(rb.Name))
* {
* DroneData drone = drones[rb.Name];
* drone.lost_count++;
* if (drone.lost_count > 3)
* {
* drone.lost_count = 0;
* MAVLink.mavlink_gps_input_t gps_input = new MAVLink.mavlink_gps_input_t();
* gps_input.gps_id = 0;
* gps_input.fix_type = (byte)MAVLink.GPS_FIX_TYPE.NO_FIX;
* byte[] pkt = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.GPS_INPUT, gps_input);
* mavSock.SendTo(pkt, drone.ep);
* }
* }*/
}
}
}
}
/* Parsing Skeleton Frame Data */
for (int i = 0; i < mSkeletons.Count; i++) // Fetching skeleton IDs from the saved descriptions
{
int sklID = mSkeletons[i].ID;
for (int j = 0; j < data.nSkeletons; j++)
{
if (sklID == data.Skeletons[j].ID) // When skeleton ID of the description matches skeleton ID of the frame data.
{
NatNetML.Skeleton skl = mSkeletons[i]; // Saved skeleton descriptions
NatNetML.SkeletonData sklData = data.Skeletons[j]; // Received skeleton frame data
Console.WriteLine("\tSkeleton ({0}):", skl.Name);
Console.WriteLine("\t\tSegment count: {0}", sklData.nRigidBodies);
/* Now, for each of the skeleton segments */
for (int k = 0; k < sklData.nRigidBodies; k++)
{
NatNetML.RigidBodyData boneData = sklData.RigidBodies[k];
/* Decoding skeleton bone ID */
int skeletonID = HighWord(boneData.ID);
int rigidBodyID = LowWord(boneData.ID);
int uniqueID = skeletonID * 1000 + rigidBodyID;
int key = uniqueID.GetHashCode();
NatNetML.RigidBody bone = (RigidBody)mHtSkelRBs[key]; //Fetching saved skeleton bone descriptions
//Outputting only the hip segment data for the purpose of this sample.
if (k == 0)
{
Console.WriteLine("\t\t{0:N3}: pos({1:N3}, {2:N3}, {3:N3})", bone.Name, boneData.x, boneData.y, boneData.z);
}
}
}
}
}
/* Parsing Force Plate Frame Data */
for (int i = 0; i < mForcePlates.Count; i++)
{
int fpID = mForcePlates[i].ID; // Fetching force plate IDs from the saved descriptions
for (int j = 0; j < data.nForcePlates; j++)
{
if (fpID == data.ForcePlates[j].ID) // When force plate ID of the descriptions matches force plate ID of the frame data.
{
NatNetML.ForcePlate fp = mForcePlates[i]; // Saved force plate descriptions
NatNetML.ForcePlateData fpData = data.ForcePlates[i]; // Received forceplate frame data
Console.WriteLine("\tForce Plate ({0}):", fp.Serial);
// Here we will be printing out only the first force plate "subsample" (index 0) that was collected with the mocap frame.
for (int k = 0; k < fpData.nChannels; k++)
{
Console.WriteLine("\t\tChannel {0}: {1}", fp.ChannelNames[k], fpData.ChannelData[k].Values[0]);
}
}
}
}
#if DEBUG_MSG
Console.WriteLine("\n");
#endif
}
void bgw_DoWork(object sender, DoWorkEventArgs e)
{
while (serialPort1.IsOpen)
{
try
{
MAVLink.MAVLinkMessage packet;
lock (readlock)
{
// read any valid packet from the port
// 从端口读取任何有效的数据包
packet = mavlink.ReadPacket(serialPort1.BaseStream);
// check its valid
// 检查它的有效
if (packet == null || packet.data == null)
{
continue;
}
}
// check to see if its a hb packet from the comport
// 看看它的心跳包从表现
if (packet.data.GetType() == typeof(MAVLink.mavlink_heartbeat_t))
{
var hb = (MAVLink.mavlink_heartbeat_t)packet.data;
// save the sysid and compid of the seen MAV
// 保存系统ID和目标飞行器的计算机ID
sysid = packet.sysid;
compid = packet.compid;
// request streams at 2 hz
// 请求的数据流在2Hz
mavlink.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.REQUEST_DATA_STREAM,
new MAVLink.mavlink_request_data_stream_t()
{
req_message_rate = 2,
req_stream_id = (byte)MAVLink.MAV_DATA_STREAM.ALL,
start_stop = 1,
target_component = compid,
target_system = sysid
});
// 设置mavlink数据缓冲区格式
}
// from here we should check the the message is addressed to us
// 从这里我们应该检查的消息是写给我们（直）
if (sysid != packet.sysid || compid != packet.compid)
{
continue;
}
if (packet.msgid == (byte)MAVLink.MAVLINK_MSG_ID.ATTITUDE)
//or
//或
//if (packet.data.GetType() == typeof(MAVLink.mavlink_attitude_t))
{
//var att = (MAVLink.mavlink_attitude_t)packet.data;// 读取一帧姿态包
//Console.WriteLine(att.pitch*57.2958 + " " + att.roll*57.2958);
var imm = (MAVLink.mavlink_attitude_t)packet.data;
Console.WriteLine("Roll:" + imm.roll + ";Pitch:" + imm.pitch + ";Yaw:" + imm.yaw + ";");
}
}
catch
{
}
System.Threading.Thread.Sleep(1);
}
}
// Update is called once per frame
void Update()
{
if (sock.Available > 0)
{
int recvBytes = 0;
try
{
recvBytes = sock.Receive(buf);
}
catch (SocketException e)
{
Debug.LogWarning("socket err " + e.ErrorCode);
}
if (recvBytes > 0)
{
MAVLink.MAVLinkMessage msg = mavlinkParse.ReadPacket(buf);
if (msg != null)
{
if (msg.msgid == (uint)MAVLink.MAVLINK_MSG_ID.STATUSTEXT)
{
var status_txt = (MAVLink.mavlink_statustext_t)msg.data;
Debug.Log(System.Text.Encoding.ASCII.GetString(status_txt.text));
}
else if (msg.msgid == (uint)MAVLink.MAVLINK_MSG_ID.HEARTBEAT)
{
var heartbeat = (MAVLink.mavlink_heartbeat_t)msg.data;
apm_mode = (int)heartbeat.custom_mode;
if (apm_mode != (int)MAVLink.COPTER_MODE.GUIDED)
{
wp.SetActive(false);
}
if ((heartbeat.base_mode & (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED) == 0)
{
if (armed)
{
MyDroneModel.GetComponent <DroneAnime>().PropellerRun = false;
}
armed = false;
}
else
{
if (!armed)
{
MyDroneModel.GetComponent <DroneAnime>().PropellerRun = true;
}
armed = true;
}
system_status = heartbeat.system_status;
if (!pos_tgt_local_rcved && (apm_mode == (int)MAVLink.COPTER_MODE.GUIDED))
{
MAVLink.mavlink_command_long_t cmd = new MAVLink.mavlink_command_long_t
{
command = (ushort)MAVLink.MAV_CMD.SET_MESSAGE_INTERVAL,
param1 = (float)MAVLink.MAVLINK_MSG_ID.POSITION_TARGET_LOCAL_NED,
param2 = 1000000
};
byte[] data = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, cmd);
sock.SendTo(data, myproxy);
}
if (!mis_cur_rcved && (apm_mode == (int)MAVLink.COPTER_MODE.AUTO))
{
MAVLink.mavlink_command_long_t cmd = new MAVLink.mavlink_command_long_t
{
command = (ushort)MAVLink.MAV_CMD.SET_MESSAGE_INTERVAL,
param1 = (float)MAVLink.MAVLINK_MSG_ID.MISSION_CURRENT,
param2 = 1000000
};
byte[] data = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, cmd);
sock.SendTo(data, myproxy);
}
if (mission_count < 0)
{
MAVLink.mavlink_mission_request_list_t cmd = new MAVLink.mavlink_mission_request_list_t
{
target_system = 0,
target_component = 0,
mission_type = 0
};
byte[] data = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.MISSION_REQUEST_LIST, cmd);
sock.SendTo(data, myproxy);
Debug.Log("send MISSION_REQUEST_LIST");
}
if ((wait_mission_seq >= 0) && (wait_mission_seq < mission_count))
{
MAVLink.mavlink_mission_request_int_t cmd = new MAVLink.mavlink_mission_request_int_t
{
target_system = 0,
target_component = 0,
seq = (ushort)wait_mission_seq,
mission_type = 0
};
byte[] data = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.MISSION_REQUEST_INT, cmd);
sock.SendTo(data, myproxy);
Debug.Log("send MISSION_REQUEST_INT " + wait_mission_seq);
}
}
else if (msg.msgid == (uint)MAVLink.MAVLINK_MSG_ID.POSITION_TARGET_LOCAL_NED)
{
pos_tgt_local_rcved = true;
var pos_tgt = (MAVLink.mavlink_position_target_local_ned_t)msg.data;
if (((pos_tgt.type_mask & 0x1000) == 0x1000) || system_status != (byte)MAVLink.MAV_STATE.ACTIVE)
{
wp.SetActive(false);
}
else
{
wp.transform.position = new Vector3(-pos_tgt.x, -pos_tgt.z, pos_tgt.y);
wp.SetActive(true);
}
}
else if (msg.msgid == (uint)MAVLink.MAVLINK_MSG_ID.MISSION_CURRENT)
{
mis_cur_rcved = true;
cur_mis_seq = ((MAVLink.mavlink_mission_current_t)msg.data).seq;
//Debug.Log("rcv MISSION_CURRENT " + cur_mis_seq);
if (cur_mis_seq < nxt_wp_seq)
{
MAVLink.mavlink_mission_set_current_t cmd = new MAVLink.mavlink_mission_set_current_t
{
target_system = 0,
target_component = 0,
seq = (ushort)nxt_wp_seq
};
byte[] data = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.MISSION_SET_CURRENT, cmd);
sock.SendTo(data, myproxy);
}
else if (mission_chk_points.Contains(cur_mis_seq))
{
waiting_in_chk_point = true;
}
else
{
waiting_in_chk_point = false;
}
}
else if (msg.msgid == (uint)MAVLink.MAVLINK_MSG_ID.MISSION_COUNT)
{
mission_count = ((MAVLink.mavlink_mission_count_t)msg.data).count;
if (mission_count > 0)
{
MAVLink.mavlink_mission_request_int_t cmd = new MAVLink.mavlink_mission_request_int_t
{
target_system = 0,
target_component = 0,
seq = 0,
mission_type = 0
};
byte[] data = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.MISSION_REQUEST_INT, cmd);
sock.SendTo(data, myproxy);
wait_mission_seq = 0;
Debug.Log("send MISSION_REQUEST_INT 0");
}
}
else if (msg.msgid == (uint)MAVLink.MAVLINK_MSG_ID.MISSION_ITEM_INT)
{
var item_int = (MAVLink.mavlink_mission_item_int_t)msg.data;
if (item_int.seq == wait_mission_seq)
{
if (item_int.command == 93) //MAV_CMD_NAV_DELAY
{
mission_chk_points.Add(item_int.seq);
}
wait_mission_seq += 1;
if (wait_mission_seq < mission_count)
{
MAVLink.mavlink_mission_request_int_t cmd = new MAVLink.mavlink_mission_request_int_t
{
target_system = 0,
target_component = 0,
seq = (ushort)wait_mission_seq,
mission_type = 0
};
byte[] data = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.MISSION_REQUEST_INT, cmd);
sock.SendTo(data, myproxy);
Debug.Log("send MISSION_REQUEST_INT " + wait_mission_seq);
}
else
{
MAVLink.mavlink_mission_ack_t cmd = new MAVLink.mavlink_mission_ack_t
{
target_system = 0,
target_component = 0,
type = 0,
mission_type = 0
};
byte[] data = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.MISSION_ACK, cmd);
sock.SendTo(data, myproxy);
Debug.Log("send MISSION_ACK");
}
}
}
/*else if (msg.msgid == (uint)MAVLink.MAVLINK_MSG_ID.MISSION_ITEM_REACHED)
* {
* Debug.Log("rcv MISSION_ITEM_REACHED "+((MAVLink.mavlink_mission_item_reached_t)msg.data).seq);
* int seq = ((MAVLink.mavlink_mission_item_reached_t)msg.data).seq;
* if (mis_chk_points.Contains(seq))
* {
* MAVLink.mavlink_mission_set_current_t cmd = new MAVLink.mavlink_mission_set_current_t
* {
* target_system = 0,
* target_component = 0,
* seq = (ushort)(seq + 2)
* };
* byte[] data = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.MISSION_SET_CURRENT, cmd);
* sock.SendTo(data, myproxy);
* }
* }*/
}
}
}
if (lastPos.Equals(transform.localPosition))
{
tracking = false;
}
else
{
tracking = true;
vel_update_cd -= Time.deltaTime;
if (vel_update_cd < 0)
{
vel_update_cd = 0.5f;
vel = (transform.localPosition - lastPos) / Time.deltaTime;
}
}
lastPos = transform.localPosition;
if (!tracking)
{
if (MyDroneModel.activeSelf)
{
MyDroneModel.SetActive(false);
GetComponent <BoxCollider>().enabled = false;
}
}
else if (!MyDroneModel.activeSelf)
{
MyDroneModel.SetActive(true);
GetComponent <BoxCollider>().enabled = true;
}
}
private void Jobs(object sender, DoWorkEventArgs args)
{
while (autopilot.IsConnected())
{
try
{
MAVLink.MAVLinkMessage packet;
lock (readlock)
{
// 从飞控读取有效的packet
packet = autopilot.GetPacket();
if (packet == null || packet.data == null)
{
continue; // 如果是无效的packet则直接进入下一次循环
}
}
// 检查是否是来自端口的一个心跳包
if (packet.data.GetType() == typeof(MAVLink.mavlink_heartbeat_t))
{
var hb = (MAVLink.mavlink_heartbeat_t)packet.data;
// 保留无人机的系统和组件ID
sysid = packet.sysid;
compid = packet.compid;
// 创建请求数据流的MAVLINK消息包，这里令req_message_rate=7，调高些可以提高数据的实时性，但对硬件有更高要求
var buffer = mavlink.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.REQUEST_DATA_STREAM,
new MAVLink.mavlink_request_data_stream_t()
{
req_message_rate = 7,
req_stream_id = (byte)MAVLink.MAV_DATA_STREAM.ALL,
start_stop = 1,
target_component = compid,
target_system = sysid
});
// 向飞控发送请求数据流的MAVLINK消息包
autopilot.ReceiveBytes(buffer);
// 创建心跳包
buffer = mavlink.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.HEARTBEAT, hb);
// 向飞控发送心跳包
autopilot.ReceiveBytes(buffer);
}
// 检查消息是否来自相应飞控
if (sysid != packet.sysid || compid != packet.compid)
{
continue; // 不是就不处理
}
// 将来自飞控的MAVLINK消息包转发给消息处理器进一步处理。
messageHandler(packet);
}
catch
{
// try catch是必要的，否则会因为抛出无关紧要的异常而结束线程。
// 偶尔抛出System.IO.IOException: 由于线程退出或应用程序请求，已中止 I/O 操作。
}
System.Threading.Thread.Sleep(1); // 防止单个线程将CPU占满。
}
}
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
{
}
}
void bgw_DoWork(object sender, DoWorkEventArgs e)
{
while (serialPort1.IsOpen)
{
try
{
MAVLink.MAVLinkMessage packet;
lock (readlock)
{
// read any valid packet from the port
packet = mavlink.ReadPacket(serialPort1.BaseStream);
// check its valid
if (packet == null || packet.data == null)
{
continue;
}
}
// check to see if its a hb packet from the comport
if (packet.data.GetType() == typeof(MAVLink.mavlink_heartbeat_t))
{
var hb = (MAVLink.mavlink_heartbeat_t)packet.data;
// save the sysid and compid of the seen MAV
sysid = packet.sysid;
compid = packet.compid;
// request streams at 2 hz
var buffer = mavlink.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.REQUEST_DATA_STREAM,
new MAVLink.mavlink_request_data_stream_t()
{
req_message_rate = 2,
req_stream_id = (byte)MAVLink.MAV_DATA_STREAM.ALL,
start_stop = 1,
target_component = compid,
target_system = sysid
});
serialPort1.Write(buffer, 0, buffer.Length);
buffer = mavlink.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.HEARTBEAT, hb);
serialPort1.Write(buffer, 0, buffer.Length);
}
// from here we should check the the message is addressed to us
if (sysid != packet.sysid || compid != packet.compid)
{
continue;
}
Console.WriteLine(packet.msgtypename);
if (packet.msgid == (byte)MAVLink.MAVLINK_MSG_ID.ATTITUDE)
//or
//if (packet.data.GetType() == typeof(MAVLink.mavlink_attitude_t))
{
var att = (MAVLink.mavlink_attitude_t)packet.data;
Console.WriteLine(att.pitch * 57.2958 + " " + att.roll * 57.2958);
}
}
catch
{
}
System.Threading.Thread.Sleep(1);
}
}
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
{
}
}