/// <summary>
/// Gets specfied WP
/// </summary>
/// <param name="index"></param>
/// <returns>WP</returns>
public Locationwp getWP(ushort index)
{
while (giveComport)
System.Threading.Thread.Sleep(100);
giveComport = true;
Locationwp loc = new Locationwp();
mavlink_mission_request_t req = new mavlink_mission_request_t();
req.target_system = MAV.sysid;
req.target_component = MAV.compid;
req.seq = index;
//Console.WriteLine("getwp req "+ DateTime.Now.Millisecond);
// request
generatePacket((byte)MAVLINK_MSG_ID.MISSION_REQUEST, req);
DateTime start = DateTime.Now;
int retrys = 5;
while (true)
{
if (!(start.AddMilliseconds(800) > DateTime.Now)) // apm times out after 1000ms
{
if (retrys > 0)
{
log.Info("getWP Retry " + retrys);
generatePacket((byte)MAVLINK_MSG_ID.MISSION_REQUEST, req);
start = DateTime.Now;
retrys--;
continue;
}
giveComport = false;
throw new Exception("Timeout on read - getWP");
}
//Console.WriteLine("getwp read " + DateTime.Now.Millisecond);
byte[] buffer = readPacket();
//Console.WriteLine("getwp readend " + DateTime.Now.Millisecond);
if (buffer.Length > 5)
{
if (buffer[5] == (byte)MAVLINK_MSG_ID.MISSION_ITEM)
{
//Console.WriteLine("getwp ans " + DateTime.Now.Millisecond);
var wp = buffer.ByteArrayToStructure<mavlink_mission_item_t>(6);
// received a packet, but not what we requested
if (req.seq != 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
{
log.Info(DateTime.Now + " PC getwp " + buffer[5]);
}
}
}
giveComport = false;
return loc;
}
/// <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;
}
/// <summary>
/// Gets specfied WP
/// </summary>
/// <param name="index"></param>
/// <returns>WP</returns>
public Locationwp getWP(ushort index)
{
giveComport = true;
Locationwp loc = new Locationwp();
mavlink_mission_request_t req = new mavlink_mission_request_t();
req.target_system = MAV.sysid;
req.target_component = MAV.compid;
req.seq = index;
//Console.WriteLine("getwp req "+ DateTime.Now.Millisecond);
// request
generatePacket((byte)MAVLINK_MSG_ID.MISSION_REQUEST, req);
DateTime start = DateTime.Now;
int retrys = 5;
while (true)
{
if (!(start.AddMilliseconds(800) > DateTime.Now)) // apm times out after 1000ms
{
if (retrys > 0)
{
log.Info("getWP Retry " + retrys);
generatePacket((byte)MAVLINK_MSG_ID.MISSION_REQUEST, req);
start = DateTime.Now;
retrys--;
continue;
}
giveComport = false;
throw new Exception("Timeout on read - getWP");
}
//Console.WriteLine("getwp read " + DateTime.Now.Millisecond);
byte[] buffer = readPacket();
//Console.WriteLine("getwp readend " + DateTime.Now.Millisecond);
if (buffer.Length > 5)
{
if (buffer[5] == (byte)MAVLINK_MSG_ID.MISSION_ITEM)
{
//Console.WriteLine("getwp ans " + DateTime.Now.Millisecond);
//Array.Copy(buffer, 6, buffer, 0, buffer.Length - 6);
var wp = buffer.ByteArrayToStructure<mavlink_mission_item_t>(6);
loc.options = (byte)(wp.frame & 0x1);
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));
/*
if (MAV.cs.firmware == MainV2.Firmwares.ArduPlane)
{
switch (loc.id)
{ // Switch to map APM command fields inot MAVLink command fields
case (byte)MAV_CMD.LOITER_TURNS:
case (byte)MAV_CMD.TAKEOFF:
case (byte)MAV_CMD.DO_SET_HOME:
//case (byte)MAV_CMD.DO_SET_ROI:
loc.alt = (float)((wp.z));
loc.lat = (float)((wp.x));
loc.lng = (float)((wp.y));
loc.p1 = (float)wp.param1;
break;
case (byte)MAV_CMD.CONDITION_CHANGE_ALT:
loc.lat = (int)wp.param1;
loc.p1 = 0;
break;
case (byte)MAV_CMD.LOITER_TIME:
if (MainV2.APMFirmware == MainV2.Firmwares.ArduPlane)
{
loc.p1 = (byte)(wp.param1 / 10); // APM loiter time is in ten second increments
}
else
{
loc.p1 = (byte)wp.param1;
}
break;
case (byte)MAV_CMD.CONDITION_DELAY:
case (byte)MAV_CMD.CONDITION_DISTANCE:
loc.lat = (int)wp.param1;
break;
case (byte)MAV_CMD.DO_JUMP:
loc.lat = (int)wp.param2;
loc.p1 = (byte)wp.param1;
break;
case (byte)MAV_CMD.DO_REPEAT_SERVO:
loc.lng = (int)wp.param4;
goto case (byte)MAV_CMD.DO_CHANGE_SPEED;
case (byte)MAV_CMD.DO_REPEAT_RELAY:
case (byte)MAV_CMD.DO_CHANGE_SPEED:
loc.lat = (int)wp.param3;
loc.alt = (int)wp.param2;
loc.p1 = (byte)wp.param1;
break;
case (byte)MAV_CMD.DO_SET_PARAMETER:
case (byte)MAV_CMD.DO_SET_RELAY:
case (byte)MAV_CMD.DO_SET_SERVO:
loc.alt = (int)wp.param2;
loc.p1 = (byte)wp.param1;
break;
case (byte)MAV_CMD.WAYPOINT:
loc.p1 = (byte)wp.param1;
break;
}
}
*/
log.InfoFormat("getWP {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[5]);
}
}
}
giveComport = false;
return loc;
}
