public async Task GetWayPoints(IDrone drone, ISerialTelemetryLink link)
{
var req = new mavlink_mission_request_list_t();
req.target_system = drone.SystemId;
req.target_component = drone.ComponentId;
var result = await link.RequestDataAsync <mavlink_mission_count_t>(drone, MAVLINK_MSG_ID.MISSION_REQUEST_LIST, req, MAVLINK_MSG_ID.MISSION_COUNT, TimeSpan.FromSeconds(1000));
if (result.Successful)
{
for (ushort idx = 0; idx < result.Result.count; ++idx)
{
var reqf = new mavlink_mission_request_int_t();
reqf.target_system = drone.SystemId;
reqf.target_component = drone.ComponentId;
reqf.seq = idx;
var wpResult = await link.RequestDataAsync <mavlink_mission_item_t>(drone, MAVLINK_MSG_ID.MISSION_REQUEST_INT, reqf, MAVLINK_MSG_ID.MISSION_ITEM, TimeSpan.FromSeconds(1000));
if (wpResult.Successful)
{
Debug.WriteLine(wpResult.Result.x + " " + wpResult.Result.y + " " + wpResult.Result.z);
}
else
{
Debug.WriteLine($"No joy on {idx}");
}
}
Debug.WriteLine($"Get go our response {result.Result.count}");
}
else
{
Debug.WriteLine($"No joy");
}
}
/// <summary>
/// Gets specfied WP
/// </summary>
/// <param name="index"></param>
/// <returns>WP</returns>
public Locationwp getWP(ushort index)
{
while (giveComport)
System.Threading.Thread.Sleep(100);
bool use_int = (MAV.cs.capabilities & MAV_PROTOCOL_CAPABILITY.MISSION_INT) > 0;
object req;
if (use_int)
{
mavlink_mission_request_int_t reqi = new mavlink_mission_request_int_t();
reqi.target_system = MAV.sysid;
reqi.target_component = MAV.compid;
reqi.seq = index;
// request
generatePacket((byte)MAVLINK_MSG_ID.MISSION_REQUEST_INT, reqi);
req = reqi;
}
else
{
mavlink_mission_request_t reqf = new mavlink_mission_request_t();
reqf.target_system = MAV.sysid;
reqf.target_component = MAV.compid;
reqf.seq = index;
// request
generatePacket((byte)MAVLINK_MSG_ID.MISSION_REQUEST, reqf);
req = reqf;
}
giveComport = true;
Locationwp loc = new Locationwp();
DateTime start = DateTime.Now;
int retrys = 5;
while (true)
{
if (!(start.AddMilliseconds(3500) > DateTime.Now)) // apm times out after 5000ms
{
if (retrys > 0)
{
log.Info("getWP Retry " + retrys);
if (use_int)
generatePacket((byte)MAVLINK_MSG_ID.MISSION_REQUEST_INT, req);
else
generatePacket((byte)MAVLINK_MSG_ID.MISSION_REQUEST, req);
start = DateTime.Now;
retrys--;
continue;
}
giveComport = false;
throw new TimeoutException("Timeout on read - getWP");
}
//Console.WriteLine("getwp read " + DateTime.Now.Millisecond);
MAVLinkMessage buffer = readPacket();
//Console.WriteLine("getwp readend " + DateTime.Now.Millisecond);
if (buffer.Length > 5)
{
if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_ITEM)
{
//Console.WriteLine("getwp ans " + DateTime.Now.Millisecond);
var wp = buffer.ToStructure<mavlink_mission_item_t>();
// received a packet, but not what we requested
if (index != wp.seq)
{
generatePacket((byte) MAVLINK_MSG_ID.MISSION_REQUEST, req);
continue;
}
loc.options = (byte) (wp.frame);
loc.id = (byte) (wp.command);
loc.p1 = (wp.param1);
loc.p2 = (wp.param2);
loc.p3 = (wp.param3);
loc.p4 = (wp.param4);
loc.alt = ((wp.z));
loc.lat = ((wp.x));
loc.lng = ((wp.y));
log.InfoFormat("getWP {0} {1} {2} {3} {4} opt {5}", loc.id, loc.p1, loc.alt, loc.lat, loc.lng,
loc.options);
break;
}
else if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_ITEM_INT)
{
//Console.WriteLine("getwp ans " + DateTime.Now.Millisecond);
var wp = buffer.ToStructure<mavlink_mission_item_int_t>();
// received a packet, but not what we requested
if (index != wp.seq)
{
generatePacket((byte)MAVLINK_MSG_ID.MISSION_REQUEST_INT, req);
continue;
}
loc.options = (byte)(wp.frame);
loc.id = (byte)(wp.command);
loc.p1 = (wp.param1);
loc.p2 = (wp.param2);
loc.p3 = (wp.param3);
loc.p4 = (wp.param4);
loc.alt = ((wp.z));
loc.lat = ((wp.x / 1.0e7));
loc.lng = ((wp.y / 1.0e7));
log.InfoFormat("getWPint {0} {1} {2} {3} {4} opt {5}", loc.id, loc.p1, loc.alt, loc.lat, loc.lng,
loc.options);
break;
}
else
{
//log.Info(DateTime.Now + " PC getwp " + buffer.msgid);
}
}
}
giveComport = false;
return loc;
}
