/// <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>
/// 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 void 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;
Console.WriteLine("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(500) > DateTime.Now))
{
if (retrys > 0)
{
Console.WriteLine("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)
{
__mavlink_mission_ack_t ans = new __mavlink_mission_ack_t();
object temp = (object)ans;
ByteArrayToStructure(buffer, ref temp, 6);
ans = (__mavlink_mission_ack_t)(temp);
Console.WriteLine("set wp " + index + " ACK 47 : " + buffer[5] + " ans " + Enum.Parse(typeof(MAV_MISSION_RESULT), ans.type.ToString()));
break;
}
else if (buffer[5] == MAVLINK_MSG_ID_MISSION_REQUEST)
{
__mavlink_mission_request_t ans = new __mavlink_mission_request_t();
object temp = (object)ans;
ByteArrayToStructure(buffer, ref temp, 6);
ans = (__mavlink_mission_request_t)(temp);
if (ans.seq == (index + 1))
{
Console.WriteLine("set wp doing " + index + " req " + ans.seq + " REQ 40 : " + buffer[5]);
MainV2.givecomport = false;
break;
}
else
{
Console.WriteLine("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
Console.WriteLine("set wp " + index + " ACK 47 : " + buffer[5]);
break;
}
else if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT_REQUEST)
{
__mavlink_waypoint_request_t ans = new __mavlink_waypoint_request_t();
object temp = (object)ans;
//Array.Copy(buffer, 6, buffer, 0, buffer.Length - 6);
ByteArrayToStructure(buffer, ref temp, 6);
ans = (__mavlink_waypoint_request_t)(temp);
if (ans.seq == (index + 1))
{
Console.WriteLine("set wp doing " + index + " req " + ans.seq + " REQ 40 : " + buffer[5]);
MainV2.givecomport = false;
break;
}
else
{
Console.WriteLine("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
}
}
}
public void setPositionTargetGlobalInt(byte sysid, byte compid, bool pos, bool vel, bool acc, MAV_FRAME frame, double lat, double lng, double alt, double vx, double vy, double vz)
{
// for mavlink SET_POSITION_TARGET messages
const ushort MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE = ((1 << 0) | (1 << 1) | (1 << 2));
const ushort MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE = ((1 << 3) | (1 << 4) | (1 << 5));
const ushort MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE = ((1 << 6) | (1 << 7) | (1 << 8));
mavlink_set_position_target_global_int_t target = new mavlink_set_position_target_global_int_t()
{
target_system = sysid,
target_component = compid,
alt = (float) alt,
lat_int = (int) (lat*1e7),
lon_int = (int) (lng*1e7),
coordinate_frame = (byte) frame,
vx = (float) vx,
vy = (float) vy,
vz = (float) vz
};
target.type_mask = 7 + 56 + 448;
if (pos)
target.type_mask -= 7;
if (vel)
target.type_mask -= 56;
if (acc)
target.type_mask -= 448;
bool pos_ignore = (target.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE)>0;
bool vel_ignore = (target.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE)>0;
bool acc_ignore = (target.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE)>0;
generatePacket((byte)MAVLINK_MSG_ID.SET_POSITION_TARGET_GLOBAL_INT, target, sysid, compid);
}