public byte[] gotolocation(float longitude, float latitude, float altitude)
{
mavlink_mission_item_t req = new mavlink_mission_item_t();
req.target_system = 1;
req.target_component = 1;
req.command = (ushort)MAVLink.MAV_CMD.WAYPOINT;
req.current = 2;
req.autocontinue = 1;
req.frame = (byte)MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT;
req.y = (float)longitude; //lng
req.x = (float)latitude; //lat
req.z = (float)altitude; //alt
req.param1 = 0;
req.param2 = 0;
req.param3 = 0;
req.param4 = 0;
req.seq = 0;
byte[] packet = mavlink.GenerateMAVLinkPacket_APM(MAVLink.MAVLINK_MSG_ID.MISSION_ITEM, req);//把req数据包，按照mavlink协议打包成（定义了临时变量packet）
return(packet);
}
public byte[] pause()
{
float NAN = (((float)((1e+300) * (1e+300))) * 0.0F);
mavlink_mission_item_t req = new mavlink_mission_item_t();
req.target_system = 1;
req.target_component = 1;
req.command = (ushort)MAVLink.MAV_CMD.WAYPOINT;
req.current = 2;
req.autocontinue = 1;
req.frame = (byte)MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT;
req.y = NAN; //lng
req.x = NAN; //lat
req.z = NAN; //alt
req.param1 = 0;
req.param2 = 0;
req.param3 = 0;
req.param4 = 0;
req.seq = 0;
byte[] packet = mavlink.GenerateMAVLinkPacket_APM(MAVLink.MAVLINK_MSG_ID.MISSION_ITEM, req);//把req数据包，按照mavlink协议打包成（定义了临时变量packet）
return(packet);
}
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;
}
/// <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, bool use_int = false)
{
if (use_int)
{
mavlink_mission_item_int_t req = new mavlink_mission_item_int_t();
req.target_system = MAV.sysid;
req.target_component = MAV.compid;
req.command = loc.id;
req.current = current;
req.autocontinue = autocontinue;
req.frame = (byte) frame;
req.y = (int) (loc.lng*1.0e7);
req.x = (int) (loc.lat*1.0e7);
req.z = (float) (loc.alt);
req.param1 = loc.p1;
req.param2 = loc.p2;
req.param3 = loc.p3;
req.param4 = loc.p4;
req.seq = index;
return setWP(req);
}
else
{
mavlink_mission_item_t req = new mavlink_mission_item_t();
req.target_system = MAV.sysid;
req.target_component = MAV.compid;
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;
return setWP(req);
}
}
/// <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()));
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;
}
/// <summary>
/// Used to extract mission from log file
/// </summary>
/// <param name="buffer">packet</param>
void getWPsfromstream(ref byte[] buffer)
{
if (buffer[5] == MAVLINK_MSG_ID_MISSION_COUNT)
{
// clear old
wps.Clear();
//new PointLatLngAlt[wps.Length];
}
if (buffer[5] == MAVLink.MAVLINK_MSG_ID_MISSION_ITEM)
{
mavlink_mission_item_t wp = buffer.ByteArrayToStructure<mavlink_mission_item_t>(6);
if (wp.current == 2)
{
// guide mode wp
GuidedMode = wp;
}
else
{
wps[wp.seq] = wp;
}
Console.WriteLine("WP # {7} cmd {8} p1 {0} p2 {1} p3 {2} p4 {3} x {4} y {5} z {6}", wp.param1, wp.param2, wp.param3, wp.param4, wp.x, wp.y, wp.z, wp.seq, wp.command);
}
}
/// <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)
{
MainV2.giveComport = true;
#if MAVLINK10
mavlink_mission_item_t req = new mavlink_mission_item_t();
#else
mavlink_waypoint_t req = new mavlink_waypoint_t();
#endif
req.target_system = sysid;
req.target_component = compid; // MAVLINK_MSG_ID_MISSION_ITEM
req.command = loc.id;
req.param1 = loc.p1;
req.current = current;
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;
/*
if (MainV2.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:
req.param1 = loc.p1;
break;
case (byte)MAV_CMD.DO_SET_HOME:
req.param1 = loc.p1;
break;
case (byte)MAV_CMD.CONDITION_CHANGE_ALT:
req.param1 = loc.lat;
req.x = 0;
req.y = 0;
break;
case (byte)MAV_CMD.LOITER_TIME:
req.param1 = loc.p1 * 10; // APM loiter time is in ten second increments
break;
case (byte)MAV_CMD.CONDITION_DELAY:
case (byte)MAV_CMD.CONDITION_DISTANCE:
req.param1 = loc.lat;
break;
case (byte)MAV_CMD.DO_JUMP:
req.param2 = loc.lat;
req.param1 = loc.p1;
break;
case (byte)MAV_CMD.DO_REPEAT_SERVO:
req.param4 = loc.lng;
goto case (byte)MAV_CMD.DO_CHANGE_SPEED;
case (byte)MAV_CMD.DO_REPEAT_RELAY:
case (byte)MAV_CMD.DO_CHANGE_SPEED:
req.param3 = loc.lat;
req.param2 = loc.alt;
req.param1 = loc.p1;
break;
case (byte)MAV_CMD.DO_SET_PARAMETER:
case (byte)MAV_CMD.DO_SET_RELAY:
case (byte)MAV_CMD.DO_SET_SERVO:
req.param2 = loc.alt;
req.param1 = loc.p1;
break;
}
}
*/
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
#if MAVLINK10
generatePacket(MAVLINK_MSG_ID_MISSION_ITEM, req);
#else
generatePacket(MAVLINK_MSG_ID_WAYPOINT, req);
#endif
DateTime start = DateTime.Now;
int retrys = 6;
while (true)
{
if (!(start.AddMilliseconds(200) > DateTime.Now))
{
if (retrys > 0)
{
log.Info("setWP Retry " + retrys);
#if MAVLINK10
generatePacket(MAVLINK_MSG_ID_MISSION_ITEM, req);
#else
generatePacket(MAVLINK_MSG_ID_WAYPOINT, req);
#endif
start = DateTime.Now;
retrys--;
continue;
}
MainV2.giveComport = false;
throw new Exception("Timeout on read - setWP");
}
byte[] buffer = readPacket();
if (buffer.Length > 5)
{
#if MAVLINK10
if (buffer[5] == 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()));
return (MAV_MISSION_RESULT)ans.type;
}
else if (buffer[5] == 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]);
MainV2.giveComport = false;
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);
//break;
}
}
else
{
//Console.WriteLine(DateTime.Now + " PC setwp " + buffer[5]);
}
#else
if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT_ACK)
{ //mavlink_waypoint_request_t
log.Info("set wp " + index + " ACK 47 : " + buffer[5]);
break;
}
else if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT_REQUEST)
{
mavlink_waypoint_request_t ans = buffer.ByteArrayToStructure<mavlink_waypoint_request_t>(6);
if (ans.seq == (index + 1))
{
log.Info("set wp doing " + index + " req " + ans.seq + " REQ 40 : " + buffer[5]);
MainV2.giveComport = false;
break;
}
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);
//break;
}
}
else
{
//Console.WriteLine(DateTime.Now + " PC setwp " + buffer[5]);
}
#endif
}
}
// return MAV_MISSION_RESULT.MAV_MISSION_INVALID;
}
