private double GetHomeAlt(MAVLink.MAV_FRAME altmode, double homealt, double lat, double lng)
{
if (altmode == MAVLink.MAV_FRAME.GLOBAL_INT || altmode == MAVLink.MAV_FRAME.GLOBAL)
{
return(0); // for absolute we dont need to add homealt
}
if (altmode == MAVLink.MAV_FRAME.GLOBAL_TERRAIN_ALT_INT || altmode == MAVLink.MAV_FRAME.GLOBAL_TERRAIN_ALT)
{
return(srtm.getAltitude(lat, lng).alt);
}
return(homealt);
}
private double GetHomeAlt(MAVLink.MAV_FRAME altmode, double homealt, double lat, double lng)
{
if (altmode == MAVLink.MAV_FRAME.GLOBAL_INT || altmode == MAVLink.MAV_FRAME.GLOBAL)
{
return(0); // for absolute we dont need to add homealt
}
if (altmode == MAVLink.MAV_FRAME.GLOBAL_TERRAIN_ALT_INT || altmode == MAVLink.MAV_FRAME.GLOBAL_TERRAIN_ALT)
{
var sralt = srtm.getAltitude(lat, lng);
if (sralt.currenttype == srtm.tiletype.invalid)
{
return(-999);
}
return(sralt.alt);
}
return(homealt);
}
private MAVLink.MAV_MISSION_RESULT setWP(Locationwp loc, ushort index, MAVLink.MAV_FRAME frame, byte current = 0, byte autocontinue = 1)
{
MAVLink.mavlink_mission_item_t req = new MAVLink.mavlink_mission_item_t();
req.target_system = TARGET_SYSTEM_ID;
req.target_component = TARGET_SYS_COMPID; // MSG_NAMES.MISSION_ITEM
req.command = loc.id;
req.current = current;
req.autocontinue = autocontinue;
req.frame = (byte)frame;
req.y = (float)(loc.lng);
req.x = (float)(loc.lat);
req.z = (float)(loc.alt);
req.param1 = loc.p1;
req.param2 = loc.p2;
req.param3 = loc.p3;
req.param4 = loc.p4;
req.seq = index;
return(setWP(req));
}
public void CreateOverlay(MAVLink.MAV_FRAME altmode, PointLatLngAlt home, List <Locationwp> missionitems, double wpradius, double loiterradius)
{
overlay.Clear();
double maxlat = -180;
double maxlong = -180;
double minlat = 180;
double minlong = 180;
Func <double, double, double> gethomealt = (lat, lng) => GetHomeAlt(altmode, home.Alt, lat, lng);
home.Tag = "H";
home.Tag2 = altmode.ToString();
pointlist.Add(home);
fullpointlist.Add(pointlist[pointlist.Count - 1]);
addpolygonmarker("H", home.Lng, home.Lat, home.Alt, null, 0);
int a = 0;
foreach (var itemtuple in missionitems.PrevNowNext())
{
var itemprev = itemtuple.Item1;
var item = itemtuple.Item2;
var itemnext = itemtuple.Item3;
ushort command = item.id;
// invalid locationwp
if (command == 0)
{
continue;
}
if (command < (ushort)MAVLink.MAV_CMD.LAST &&
command != (ushort)MAVLink.MAV_CMD.TAKEOFF && // doesnt have a position
command != (ushort)MAVLink.MAV_CMD.VTOL_TAKEOFF && // doesnt have a position
command != (ushort)MAVLink.MAV_CMD.RETURN_TO_LAUNCH &&
command != (ushort)MAVLink.MAV_CMD.CONTINUE_AND_CHANGE_ALT &&
command != (ushort)MAVLink.MAV_CMD.DELAY &&
command != (ushort)MAVLink.MAV_CMD.GUIDED_ENABLE ||
command == (ushort)MAVLink.MAV_CMD.DO_SET_ROI)
{
// land can be 0,0 or a lat,lng
if (command == (ushort)MAVLink.MAV_CMD.LAND && item.lat == 0 && item.lng == 0)
{
continue;
}
if (command == (ushort)MAVLink.MAV_CMD.DO_SET_ROI)
{
pointlist.Add(new PointLatLngAlt(item.lat, item.lng,
item.alt + gethomealt(item.lat, item.lng), "ROI" + (a + 1))
{
color = Color.Red
});
// do set roi is not a nav command. so we dont route through it
//fullpointlist.Add(pointlist[pointlist.Count - 1]);
GMarkerGoogle m =
new GMarkerGoogle(new PointLatLng(item.lat, item.lng),
GMarkerGoogleType.red);
m.ToolTipMode = MarkerTooltipMode.Always;
m.ToolTipText = (a + 1).ToString();
m.Tag = (a + 1).ToString();
GMapMarkerRect mBorders = new GMapMarkerRect(m.Position);
{
mBorders.InnerMarker = m;
mBorders.Tag = "Dont draw line";
}
// check for clear roi, and hide it
if (m.Position.Lat != 0 && m.Position.Lng != 0)
{
// order matters
overlay.Markers.Add(m);
overlay.Markers.Add(mBorders);
}
}
else if (command == (ushort)MAVLink.MAV_CMD.LOITER_TIME ||
command == (ushort)MAVLink.MAV_CMD.LOITER_TURNS ||
command == (ushort)MAVLink.MAV_CMD.LOITER_UNLIM)
{
pointlist.Add(new PointLatLngAlt(item.lat, item.lng,
item.alt + gethomealt(item.lat, item.lng), (a + 1).ToString())
{
color = Color.LightBlue
});
// exit at tangent
if (item.p4 == 1)
{
var from = pointlist.Last();
var to = itemnext.lat != 0 && itemnext.lng != 0
? new PointLatLngAlt(itemnext)
{
Alt = itemnext.alt + gethomealt(item.lat, item.lng)
}
: from;
var bearing = from.GetBearing(to);
var dist = from.GetDistance(to);
if (dist > loiterradius)
{
fullpointlist.Add(pointlist[pointlist.Count - 1]);
var offset = from.newpos(bearing + 90, loiterradius);
fullpointlist.Add(offset);
}
else
{
fullpointlist.Add(pointlist[pointlist.Count - 1]);
}
}
else
{
fullpointlist.Add(pointlist[pointlist.Count - 1]);
}
addpolygonmarker((a + 1).ToString(), item.lng, item.lat,
item.alt, Color.LightBlue, loiterradius);
}
else if (command == (ushort)MAVLink.MAV_CMD.SPLINE_WAYPOINT)
{
pointlist.Add(new PointLatLngAlt(item.lat, item.lng,
item.alt + gethomealt(item.lat, item.lng), (a + 1).ToString())
{
Tag2 = "spline"
});
fullpointlist.Add(pointlist[pointlist.Count - 1]);
addpolygonmarker((a + 1).ToString(), item.lng, item.lat,
item.alt, Color.Green, wpradius);
}
else
{
pointlist.Add(new PointLatLngAlt(item.lat, item.lng,
item.alt + gethomealt(item.lat, item.lng), (a + 1).ToString()));
fullpointlist.Add(pointlist[pointlist.Count - 1]);
addpolygonmarker((a + 1).ToString(), item.lng, item.lat,
item.alt, null, wpradius);
}
maxlong = Math.Max(item.lng, maxlong);
maxlat = Math.Max(item.lat, maxlat);
minlong = Math.Min(item.lng, minlong);
minlat = Math.Min(item.lat, minlat);
}
else if (command == (ushort)MAVLink.MAV_CMD.DO_JUMP) // fix do jumps into the future
{
pointlist.Add(null);
int wpno = (int)item.p1;
int repeat = (int)item.p2;
List <PointLatLngAlt> list = new List <PointLatLngAlt>();
// cycle through reps
for (int repno = repeat; repno > 0; repno--)
{
// cycle through wps
for (int no = wpno; no <= a; no++)
{
if (pointlist[no] != null)
{
list.Add(pointlist[no]);
}
}
}
fullpointlist.AddRange(list);
}
else
{
pointlist.Add(null);
}
a++;
}
RegenerateWPRoute(fullpointlist, home);
}
public static void upload(MAVLinkInterface port, MAVLink.MAV_MISSION_TYPE type, List <Locationwp> commandlist, MAVLink.MAV_FRAME frame = MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT)
{
try
{
if (!port.BaseStream.IsOpen)
{
throw new Exception("Please connect first!");
}
int a;
bool use_int = (port.MAV.cs.capabilities & (uint)MAVLink.MAV_PROTOCOL_CAPABILITY.MISSION_INT) > 0;
port.setWPTotal((ushort)commandlist.Count);
// process commandlist to the mav
for (a = 0; a < commandlist.Count; a++)
{
var temp = commandlist[a];
// handle current wp upload number
int uploadwpno = a;
// try send the wp
MAVLink.MAV_MISSION_RESULT ans = port.setWP(temp, (ushort)(uploadwpno), (MAVLink.MAV_FRAME)temp.frame, 0, 1, use_int, type);
// we timed out while uploading wps/ command wasnt replaced/ command wasnt added
if (ans == MAVLink.MAV_MISSION_RESULT.MAV_MISSION_ERROR)
{
// resend for partial upload
port.setWPPartialUpdate((ushort)(uploadwpno), (ushort)commandlist.Count, type);
// reupload this point.
ans = port.setWP(temp, (ushort)(uploadwpno), frame, 0, 1, use_int, type);
}
if (ans == MAVLink.MAV_MISSION_RESULT.MAV_MISSION_NO_SPACE)
{
throw new Exception("Upload failed, please reduce the number of wp's");
}
if (ans == MAVLink.MAV_MISSION_RESULT.MAV_MISSION_INVALID)
{
throw new Exception(
"Upload failed, mission was rejected by the Mav,\n item had a bad option wp# " + a + " " +
ans);
}
if (ans == MAVLink.MAV_MISSION_RESULT.MAV_MISSION_INVALID_SEQUENCE)
{
// invalid sequence can only occur if we failed to see a response from the apm when we sent the request.
// or there is io lag and we send 2 mission_items and get 2 responces, one valid, one a ack of the second send
// the ans is received via mission_ack, so we dont know for certain what our current request is for. as we may have lost the mission_request
// get requested wp no - 1;
try
{
a = port.getRequestedWPNo() - 1;
}
catch
{
// resend for partial upload
port.setWPPartialUpdate((ushort)(uploadwpno), (ushort)commandlist.Count, type);
// reupload this point.
}
continue;
}
if (ans != MAVLink.MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED)
{
throw new Exception("Upload wps failed " + Enum.Parse(typeof(MAVLink.MAV_CMD), temp.id.ToString()) +
" " + Enum.Parse(typeof(MAVLink.MAV_MISSION_RESULT), ans.ToString()));
}
}
port.setWPACK();
}
catch (Exception ex)
{
log.Error(ex);
throw;
}
}
public void sendWPS(List <PointLatLngAlt> Points)
{
MAVLink.MAV_FRAME frame = MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT;
Locationwp home = new Locationwp();
try
{
home.id = (byte)MAVLink.MAV_CMD.WAYPOINT;
home.lat = 23.1647061918178;
home.lng = 113.455638885498;
home.alt = 20; // use saved home
}
catch { throw new Exception("Your home location is invalid"); }
ushort count = (ushort)(Points.Count + 1);//含家地址
UpLoadForm upform = new UpLoadForm();
upform.StartPosition = FormStartPosition.Manual;
upform.Location = new System.Drawing.Point((this.form.Width - upform.Width) / 2, (this.form.Height - upform.Height) / 2);
// this.upform.Visible = true;
// Application.DoEvents();
upform.Show();
// Application.DoEvents();
upform.SetMessage(0, count);
// this.upform.ShowDialog();
// Thread.Sleep(100);
setWPTotal(count);
this.ListMessage("已发送航点数量...");
upform.SetMessage(1, 0);
bool mission_uploading = true;
Locationwp[] wp_location = new Locationwp[count - 1];
int i = 0;
foreach (PointLatLngAlt point in Points)
{
wp_location[i].id = (byte)MAVLink.MAV_CMD.WAYPOINT;
wp_location[i].lat = point.Lat;
wp_location[i].lng = point.Lng;
wp_location[i].alt = point.Alt;
i++;
}
int seq = 0;
bool isSendTotalSuccess = false;
int cnt = 0;
while (mission_uploading)
{
Program.mav_msg_handler.mission_request = false;
Program.mav_msg_handler.mission_ack = false;
Program.mav_msg_handler.SetTimeout(5);
while (true)
{
if (Program.mav_msg_handler.Wait())
{
if (isSendTotalSuccess)
{
ListMessage("发送航点超时出错");
mission_uploading = false;
break;
}
else
{
cnt++;
if (cnt >= 3)
{
ListMessage("发送航点数量超时出错");
mission_uploading = false;
break;
}
else
{
ListMessage("发送航点数量超时出错，准备重发...");
setWPTotal(count);
break;
}
}
}
if (upform.isCancel)
{
this.ListMessage("您已经取消航点发送...");
upform.Close();
return;
}
if (Program.mav_msg_handler.mission_ack)
{
// this.ListMessage("收到ACK...");
if (Program.mav_msg_handler.mavlink_mission_ack.type != 0)
{
ListMessage("发送航点出错");
}
else
{
ListMessage("航点上传完毕");
}
mission_uploading = false;
break;
}
if (Program.mav_msg_handler.mission_request == true)
{
isSendTotalSuccess = true;
// this.ListMessage("收到REQUEST...");
seq = Program.mav_msg_handler.mavlink_mission_request.seq;
if (seq == 0)
{
setWP(home, 0, MAVLink.MAV_FRAME.GLOBAL, 0);
ListMessage("正在发送HOME...");
}
else
{
setWP(wp_location[seq - 1], (ushort)(seq), frame, 0);
ListMessage("正在发送航点" + seq.ToString() + "...");
upform.SetMessage(2, seq);
upform.SetCount(seq, count);
}
break;
}
Application.DoEvents();
}
}
Thread.Sleep(500);
upform.SetMessage(3, 0);
Thread.Sleep(500);
setWPACK();
upform.Close();
}
