public void GoToTargetPoint(PointLatLngAlt target)
{
Locationwp gotohere = new Locationwp();
gotohere.id = (byte)MAVLink.MAV_CMD.WAYPOINT;
gotohere.alt = target.Alt; // back to m
gotohere.lat = target.Lat;
gotohere.lng = target.Lng;
MAVLink.MAV_MISSION_RESULT ans = this.setWP(gotohere, 0, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT, (byte)2);
this.ListMessage("正在发送直达航点...");
}
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 static async Task upload(MAVLinkInterface port, byte sysid, byte compid, MAVLink.MAV_MISSION_TYPE type, List <Locationwp> commandlist, Action <int, string> progress = null)
{
try
{
if (!port.BaseStream.IsOpen)
{
throw new Exception("Please connect first!");
}
bool use_supportfence = (port.MAVlist[sysid, compid].cs.capabilities & (uint)MAVLink.MAV_PROTOCOL_CAPABILITY.MISSION_FENCE) > 0;
bool use_int = (port.MAVlist[sysid, compid].cs.capabilities & (uint)MAVLink.MAV_PROTOCOL_CAPABILITY.MISSION_INT) > 0;
if (!use_supportfence && !port.MAVlist[sysid, compid].mavlinkv2 && type != MAVLink.MAV_MISSION_TYPE.MISSION)
{
throw new Exception("Mission type only supported under mavlink2");
}
int a;
await port.setWPTotalAsync(sysid, compid, (ushort)commandlist.Count, type).ConfigureAwait(false);
// process commandlist to the mav
for (a = 0; a < commandlist.Count; a++)
{
var temp = commandlist[a];
// handle current wp upload number
int uploadwpno = a;
progress?.Invoke(((a * 100) / commandlist.Count), "Uploading WP " + a);
// try send the wp
MAVLink.MAV_MISSION_RESULT ans = await port.setWPAsync(sysid, compid, temp, (ushort)(uploadwpno), (MAVLink.MAV_FRAME) temp.frame, 0, 1, use_int, type).ConfigureAwait(false);
// 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
await port.setWPPartialUpdateAsync(sysid, compid, (ushort)(uploadwpno), (ushort)commandlist.Count, type).ConfigureAwait(false);
// reupload this point.
ans = await port.setWPAsync(sysid, compid, temp, (ushort)(uploadwpno),
(MAVLink.MAV_FRAME) temp.frame, 0, 1, use_int, type).ConfigureAwait(false);
}
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 = await port.getRequestedWPNoAsync(sysid, compid).ConfigureAwait(false) - 1;
}
catch
{
// resend for partial upload
await port.setWPPartialUpdateAsync(sysid, compid, (ushort)(uploadwpno), (ushort)commandlist.Count, type).ConfigureAwait(false);
// reupload this point.
}
continue;
}
if (ans != MAVLink.MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED)
{
throw new Exception("Upload " + type.ToString() + " failed " + Enum.Parse(typeof(MAVLink.MAV_CMD), temp.id.ToString()) +
" " + Enum.Parse(typeof(MAVLink.MAV_MISSION_RESULT), ans.ToString()));
}
}
port.setWPACK(sysid, compid, type);
}
catch (Exception ex)
{
log.Error(ex);
throw;
}
}
