public MAV_MISSION_RESULT setWP(mavlink_mission_item_t req)
{
giveComport = true;
ushort index = req.seq;
log.InfoFormat("setWP {6} frame {0} cmd {1} p1 {2} x {3} y {4} z {5}", req.frame, req.command, req.param1,
req.x, req.y, req.z, index);
// request
generatePacket((byte) MAVLINK_MSG_ID.MISSION_ITEM, req);
DateTime start = DateTime.Now;
int retrys = 10;
while (true)
{
if (!(start.AddMilliseconds(400) > DateTime.Now))
{
if (retrys > 0)
{
log.Info("setWP Retry " + retrys);
generatePacket((byte) MAVLINK_MSG_ID.MISSION_ITEM, req);
start = DateTime.Now;
retrys--;
continue;
}
giveComport = false;
throw new TimeoutException("Timeout on read - setWP");
}
MAVLinkMessage buffer = readPacket();
if (buffer.Length > 5)
{
if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_ACK)
{
var ans = buffer.ToStructure<mavlink_mission_ack_t>();
log.Info("set wp " + index + " ACK 47 : " + buffer.msgid + " ans " +
Enum.Parse(typeof (MAV_MISSION_RESULT), ans.type.ToString()));
giveComport = false;
if (req.current == 2)
{
MAV.GuidedMode = req;
}
else if (req.current == 3)
{
}
else
{
MAV.wps[req.seq] = req;
}
return (MAV_MISSION_RESULT) ans.type;
}
else if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_REQUEST)
{
var ans = buffer.ToStructure<mavlink_mission_request_t>();
if (ans.seq == (index + 1))
{
log.Info("set wp doing " + index + " req " + ans.seq + " REQ 40 : " + buffer.msgid);
giveComport = false;
if (req.current == 2)
{
MAV.GuidedMode = req;
}
else if (req.current == 3)
{
}
else
{
MAV.wps[req.seq] = req;
}
return MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED;
}
else
{
log.InfoFormat(
"set wp fail doing " + index + " req " + ans.seq + " ACK 47 or REQ 40 : " + buffer.msgid +
" seq {0} ts {1} tc {2}", req.seq, req.target_system, req.target_component);
// resend point now
start = DateTime.MinValue;
}
}
else
{
//Console.WriteLine(DateTime.Now + " PC setwp " + buffer.msgid);
}
}
}
// return MAV_MISSION_RESULT.MAV_MISSION_INVALID;
}