public PointLatLngAlt getRallyPoint(int no, ref int total)
{
MAVLinkMessage buffer;
giveComport = true;
PointLatLngAlt plla = new PointLatLngAlt();
mavlink_rally_fetch_point_t req = new mavlink_rally_fetch_point_t();
req.idx = (byte) no;
req.target_component = MAV.compid;
req.target_system = MAV.sysid;
// request point
generatePacket((byte) MAVLINK_MSG_ID.RALLY_FETCH_POINT, req);
DateTime start = DateTime.Now;
int retrys = 3;
while (true)
{
if (!(start.AddMilliseconds(700) > DateTime.Now))
{
if (retrys > 0)
{
log.Info("getRallyPoint Retry " + retrys + " - giv com " + giveComport);
generatePacket((byte) MAVLINK_MSG_ID.FENCE_FETCH_POINT, req);
start = DateTime.Now;
retrys--;
continue;
}
giveComport = false;
throw new TimeoutException("Timeout on read - getRallyPoint");
}
buffer = readPacket();
if (buffer.Length > 5)
{
if (buffer.msgid == (byte) MAVLINK_MSG_ID.RALLY_POINT)
{
mavlink_rally_point_t fp = buffer.ToStructure<mavlink_rally_point_t>();
if (req.idx != fp.idx)
{
generatePacket((byte) MAVLINK_MSG_ID.FENCE_FETCH_POINT, req);
continue;
}
plla.Lat = fp.lat/t7;
plla.Lng = fp.lng/t7;
plla.Tag = fp.idx.ToString();
plla.Alt = fp.alt;
total = fp.count;
giveComport = false;
return plla;
}
}
}
}