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>
/// Returns WP count
/// </summary>
/// <returns></returns>
public ushort getWPCount()
{
giveComport = true;
MAVLinkMessage buffer;
mavlink_mission_request_list_t req = new mavlink_mission_request_list_t();
req.target_system = MAV.sysid;
req.target_component = MAV.compid;
// request list
generatePacket((byte) MAVLINK_MSG_ID.MISSION_REQUEST_LIST, req);
DateTime start = DateTime.Now;
int retrys = 6;
while (true)
{
if (!(start.AddMilliseconds(700) > DateTime.Now))
{
if (retrys > 0)
{
log.Info("getWPCount Retry " + retrys + " - giv com " + giveComport);
generatePacket((byte) MAVLINK_MSG_ID.MISSION_REQUEST_LIST, req);
start = DateTime.Now;
retrys--;
continue;
}
giveComport = false;
//return (byte)int.Parse(param["WP_TOTAL"].ToString());
throw new TimeoutException("Timeout on read - getWPCount");
}
buffer = readPacket();
if (buffer.Length > 5)
{
if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_COUNT)
{
var count = buffer.ToStructure<mavlink_mission_count_t>();
log.Info("wpcount: " + count.count);
giveComport = false;
return count.count; // should be ushort, but apm has limited wp count < byte
}
else
{
log.Info(DateTime.Now + " PC wpcount " + buffer.msgid + " need " + MAVLINK_MSG_ID.MISSION_COUNT);
}
}
}
}
/// <summary>
/// Returns WP count
/// </summary>
/// <returns></returns>
public byte getWPCount()
{
MainV2.giveComport = true;
byte[] buffer;
#if MAVLINK10
mavlink_mission_request_list_t req = new mavlink_mission_request_list_t();
req.target_system = sysid;
req.target_component = compid;
// request list
generatePacket(MAVLINK_MSG_ID_MISSION_REQUEST_LIST, req);
DateTime start = DateTime.Now;
int retrys = 6;
while (true)
{
if (!(start.AddMilliseconds(500) > DateTime.Now))
{
if (retrys > 0)
{
log.Info("getWPCount Retry " + retrys + " - giv com " + MainV2.giveComport);
generatePacket(MAVLINK_MSG_ID_MISSION_REQUEST_LIST, req);
start = DateTime.Now;
retrys--;
continue;
}
MainV2.giveComport = false;
//return (byte)int.Parse(param["WP_TOTAL"].ToString());
throw new Exception("Timeout on read - getWPCount");
}
buffer = readPacket();
if (buffer.Length > 5)
{
if (buffer[5] == MAVLINK_MSG_ID_MISSION_COUNT)
{
var count = buffer.ByteArrayToStructure<mavlink_mission_count_t>(6);
log.Info("wpcount: " + count.count);
MainV2.giveComport = false;
return (byte)count.count; // should be ushort, but apm has limited wp count < byte
}
else
{
log.Info(DateTime.Now + " PC wpcount " + buffer[5] + " need " + MAVLINK_MSG_ID_MISSION_COUNT);
}
}
}
#else
mavlink_waypoint_request_list_t req = new mavlink_waypoint_request_list_t();
req.target_system = sysid;
req.target_component = compid;
// request list
generatePacket(MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST, req);
DateTime start = DateTime.Now;
int retrys = 6;
while (true)
{
if (!(start.AddMilliseconds(500) > DateTime.Now))
{
if (retrys > 0)
{
log.Info("getWPCount Retry " + retrys + " - giv com " + MainV2.giveComport);
generatePacket(MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST, req);
start = DateTime.Now;
retrys--;
continue;
}
MainV2.giveComport = false;
//return (byte)int.Parse(param["WP_TOTAL"].ToString());
throw new Exception("Timeout on read - getWPCount");
}
buffer = readPacket();
if (buffer.Length > 5)
{
if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT_COUNT)
{
log.Info("wpcount: " + buffer[9]);
MainV2.giveComport = false;
return buffer[9]; // should be ushort, but apm has limited wp count < byte
}
else
{
log.Info(DateTime.Now + " PC wpcount " + buffer[5] + " need " + MAVLINK_MSG_ID_WAYPOINT_COUNT + " " + this.BaseStream.BytesToRead);
}
}
}
#endif
}
