/// <summary>
/// Save wp to eeprom
/// </summary>
/// <param name="loc">location struct</param>
/// <param name="index">wp no</param>
/// <param name="frame">global or relative</param>
/// <param name="current">0 = no , 2 = guided mode</param>
public MAV_MISSION_RESULT setWP(Locationwp loc, ushort index, MAV_FRAME frame, byte current = 0, byte autocontinue = 1)
{
giveComport = true;
mavlink_mission_item_t req = new mavlink_mission_item_t();
req.target_system = MAV.sysid;
req.target_component = MAV.compid; // MSG_NAMES.MISSION_ITEM
req.command = loc.id;
req.current = current;
req.autocontinue = autocontinue;
req.frame = (byte)frame;
req.y = (float)(loc.lng);
req.x = (float)(loc.lat);
req.z = (float)(loc.alt);
req.param1 = loc.p1;
req.param2 = loc.p2;
req.param3 = loc.p3;
req.param4 = loc.p4;
req.seq = index;
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 Exception("Timeout on read - setWP");
}
byte[] buffer = readPacket();
if (buffer.Length > 5)
{
if (buffer[5] == (byte)MAVLINK_MSG_ID.MISSION_ACK)
{
var ans = buffer.ByteArrayToStructure<mavlink_mission_ack_t>(6);
log.Info("set wp " + index + " ACK 47 : " + buffer[5] + " 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[5] == (byte)MAVLINK_MSG_ID.MISSION_REQUEST)
{
var ans = buffer.ByteArrayToStructure<mavlink_mission_request_t>(6);
if (ans.seq == (index + 1))
{
log.Info("set wp doing " + index + " req " + ans.seq + " REQ 40 : " + buffer[5]);
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[5] + " 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[5]);
}
}
}
// return MAV_MISSION_RESULT.MAV_MISSION_INVALID;
}
public void setNewWPAlt(Locationwp gotohere)
{
giveComport = true;
try
{
gotohere.id = (byte)MAV_CMD.WAYPOINT;
MAV_MISSION_RESULT ans = setWP(gotohere, 0, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT, (byte)3);
if (ans != MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED)
throw new Exception("Alt Change Failed");
}
catch (Exception ex) { giveComport = false; log.Error(ex); throw; }
giveComport = false;
}
public void setGuidedModeWP(Locationwp gotohere)
{
if (gotohere.alt == 0 || gotohere.lat == 0 || gotohere.lng == 0)
return;
giveComport = true;
try
{
gotohere.id = (byte)MAV_CMD.WAYPOINT;
// fix for followme change
if (MAV.cs.mode.ToUpper() != "GUIDED")
setMode("GUIDED");
MAV_MISSION_RESULT ans = setWP(gotohere, 0, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT, (byte)2);
if (ans != MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED)
throw new Exception("Guided Mode Failed");
}
catch (Exception ex) { log.Error(ex); }
giveComport = false;
}
/// <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;
}
public PointLatLngAlt(Locationwp locwp)
{
this.Lat = locwp.lat;
this.Lng = locwp.lng;
this.Alt = locwp.alt;
}
public PointLatLngAlt(Locationwp locwp)
{
this.Lat = locwp.lat;
this.Lng = locwp.lng;
this.Alt = locwp.alt;
}
