private static void HandlePacket(object packet)
{
if (packet.GetType() == typeof(MAVLink.mavlink_heartbeat_t))
{
MAVLink.mavlink_heartbeat_t parsed = (MAVLink.mavlink_heartbeat_t)packet;
droneState = (MAVLink.MAV_STATE)parsed.system_status;
//MAVLink.MAV_MODE
mode = (APMModes.Quadrotor)parsed.custom_mode;
}
else if (packet.GetType() == typeof(MAVLink.mavlink_global_position_int_t))
{
MAVLink.mavlink_global_position_int_t gps = (MAVLink.mavlink_global_position_int_t)packet;
// dronePosition.lat = (double)gps.lat / 10000000;
// dronePosition.lng = (double)gps.lon / 10000000;
// dronePosition.alt = (double)gps.alt / 1000;
dronePosition.altFromGround = (double)gps.relative_alt / 1000;
dronePosition.heading = (double)gps.hdg / 100;
}
else if (packet.GetType() == typeof(MAVLink.mavlink_gps_raw_int_t))
{
MAVLink.mavlink_gps_raw_int_t gps = (MAVLink.mavlink_gps_raw_int_t)packet;
GPSHdop = (double)gps.eph / 100;
dronePosition.lat = (double)gps.lat / 10000000;
dronePosition.lng = (double)gps.lon / 10000000;
dronePosition.alt = (double)gps.alt / 1000;
satCount = gps.satellites_visible;
IsDroneReady();
}
else if (packet.GetType() == typeof(MAVLink.mavlink_mission_count_t))
{
MAVLink.mavlink_mission_count_t paramValue = (MAVLink.mavlink_mission_count_t)packet;
missionWaypointsCount = paramValue.count;
missionItems = new SMissionItem[missionWaypointsCount];
}
else if (packet.GetType() == typeof(MAVLink.mavlink_mission_item_t))
{
MAVLink.mavlink_mission_item_t paramValue = (MAVLink.mavlink_mission_item_t)packet;
SMissionItem missionItem = new SMissionItem();
missionItem.command = (MAVLink.MAV_CMD)paramValue.command;
// if (missionItem.command == MAVLink.MAV_CMD.TAKEOFF)
// // || (missionItem.command == MAVLink.MAV_CMD.RETURN_TO_LAUNCH))
// {
// missionItem.lat = dronePosition.lat;
// missionItem.lng = dronePosition.lng;
// }
// else
{
missionItem.lat = paramValue.x;
missionItem.lng = paramValue.y;
}
missionItem.p1 = paramValue.param1;
missionItem.p2 = paramValue.param2;
missionItem.p3 = paramValue.param3;
missionItem.p4 = paramValue.param4;
missionItem.alt = paramValue.z;
// Is this the home?
missionItem.IsHome = (paramValue.seq == 0);
missionItems[paramValue.seq] = missionItem;
}
else if (packet.GetType() == typeof(MAVLink.mavlink_mission_request_t))
{
MAVLink.mavlink_mission_request_t missionReq = (MAVLink.mavlink_mission_request_t)packet;
SetMissionItem(missionReq.seq);
IsDroneReady();
}
else if (packet.GetType() == typeof(MAVLink.mavlink_param_value_t))
{
MAVLink.mavlink_param_value_t paramValue = (MAVLink.mavlink_param_value_t)packet;
string name = ASCIIEncoding.ASCII.GetString(paramValue.param_id);
name = name.Trim().Replace("\0", "");
double val = paramValue.param_value;
if (OnParamUpdate != null)
{
OnParamUpdate(name, val, paramValue.param_index);
}
}
else if (packet.GetType() == typeof(MAVLink.mavlink_ekf_status_report_t))
{
MAVLink.mavlink_ekf_status_report_t ekf = (MAVLink.mavlink_ekf_status_report_t)packet;
// Check that all estimations are good
EKFStatus = (float)Math.Max(ekf.pos_vert_variance, Math.Max(ekf.compass_variance, Math.Max(ekf.pos_horiz_variance, Math.Max(ekf.pos_vert_variance, ekf.terrain_alt_variance))));
EKFFlags = ekf.flags;
IsDroneReady();
}
}
public static bool LoadMission(string filename, SMission mission)
{
if (!File.Exists(filename))
{
return(false);
}
StreamReader reader = new StreamReader(filename);
string header = reader.ReadLine();
// Ignore the first line
if (header == null || !header.Contains("QGC WPL"))
{
return(false);
}
while (!reader.EndOfStream)
{
string line = reader.ReadLine();
// Ignore comments
if (line.StartsWith("#"))
{
continue;
}
string[] items = line.Split(new[] { '\t', ' ', ',' }, StringSplitOptions.RemoveEmptyEntries);
if (items.Length <= 9)
{
continue;
}
try
{
SMissionItem missionItem = new SMissionItem();
// First point is home
if (items[0] == "0")
{
missionItem.IsHome = true;
}
// I'm not sure why we do this, but this is what mission planner does
if (items[2] == "3")
{
// abs MAV_FRAME_GLOBAL_RELATIVE_ALT=3
missionItem.frame = (MAVLink.MAV_FRAME.LOCAL_NED);
}
else
{
missionItem.frame = (MAVLink.MAV_FRAME.GLOBAL);
}
missionItem.command = (MAVLink.MAV_CMD)Enum.Parse(typeof(MAVLink.MAV_CMD), items[3], false);
missionItem.p1 = float.Parse(items[4], new CultureInfo("en-US"));
missionItem.alt = (float)(double.Parse(items[10], new CultureInfo("en-US")));
missionItem.lat = (double.Parse(items[8], new CultureInfo("en-US")));
missionItem.lng = (double.Parse(items[9], new CultureInfo("en-US")));
missionItem.p2 = (float)(double.Parse(items[5], new CultureInfo("en-US")));
missionItem.p3 = (float)(double.Parse(items[6], new CultureInfo("en-US")));
missionItem.p4 = (float)(double.Parse(items[7], new CultureInfo("en-US")));
mission.Items.Add(missionItem);
}
catch
{
return(false);
}
}
reader.Close();
return(true);
}
public static List <SMissionItem> ReadWaypointFile(string file, bool append = false)
{
// ReadQGC110wpfile
int wp_count = 0;
bool error = false;
List <SMissionItem> cmds = new List <SMissionItem>();
try
{
StreamReader reader = new StreamReader(file); //"defines.h"
string header = reader.ReadLine();
if (header == null || !header.Contains("QGC WPL"))
{
return(null);
}
while (!error && !reader.EndOfStream)
{
string line = reader.ReadLine();
// Waypoints
if (line.StartsWith("#"))
{
continue;
}
// Waypoint file format:
// <INDEX> <CURRENT WP> <COORD FRAME> <COMMAND> <PARAM1> <PARAM2> <PARAM3> <PARAM4> <PARAM5/X/LONGITUDE> <PARAM6/Y/LATITUDE> <PARAM7/Z/ALTITUDE> <AUTOCONTINUE>
string[] items = line.Split(new[] { '\t', ' ', ',' }, StringSplitOptions.RemoveEmptyEntries);
if (items.Length <= 9)
{
continue;
}
try
{
SMissionItem temp = new SMissionItem();
if (items[2] == "3")
{ // abs MAV_FRAME_GLOBAL_RELATIVE_ALT=3
temp.frame = MAVLink.MAV_FRAME.LOCAL_NED;
}
else
{
temp.frame = MAVLink.MAV_FRAME.GLOBAL;
}
temp.command = (MAVLink.MAV_CMD)Enum.Parse(typeof(MAVLink.MAV_CMD), items[3], false);
temp.p1 = float.Parse(items[4], new CultureInfo("en-US"));
if ((byte)temp.command == 99)
{
temp.command = 0;
}
temp.alt = (float)(double.Parse(items[10], new CultureInfo("en-US")));
temp.lat = (double.Parse(items[8], new CultureInfo("en-US")));
temp.lng = (double.Parse(items[9], new CultureInfo("en-US")));
temp.p2 = (float)(double.Parse(items[5], new CultureInfo("en-US")));
temp.p3 = (float)(double.Parse(items[6], new CultureInfo("en-US")));
temp.p4 = (float)(double.Parse(items[7], new CultureInfo("en-US")));
cmds.Add(temp);
wp_count++;
}
catch
{
return(null);
}
}
reader.Close();
}
catch //(Exception ex)
{
return(null);
}
return(cmds);
}