public bool setWPCurrent(ushort index)
{
giveComport = true;
MAVLinkMessage buffer;
mavlink_mission_set_current_t req = new mavlink_mission_set_current_t();
req.target_system = MAV.sysid;
req.target_component = MAV.compid;
req.seq = index;
generatePacket((byte) MAVLINK_MSG_ID.MISSION_SET_CURRENT, req);
DateTime start = DateTime.Now;
int retrys = 5;
while (true)
{
if (!(start.AddMilliseconds(2000) > DateTime.Now))
{
if (retrys > 0)
{
log.Info("setWPCurrent Retry " + retrys);
generatePacket((byte) MAVLINK_MSG_ID.MISSION_SET_CURRENT, req);
start = DateTime.Now;
retrys--;
continue;
}
giveComport = false;
throw new TimeoutException("Timeout on read - setWPCurrent");
}
buffer = readPacket();
if (buffer.Length > 5)
{
if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_CURRENT)
{
giveComport = false;
return true;
}
}
}
}