public double p4; // param 4
/// <summary>
/// this constructor is used only for reading mission files
/// </summary>
/// <param name="lw"></param>
internal LocationWp(Locationwp lw)
{
waypointState = WaypointState.None;
number = lw.number;
id = (MAV_CMD)Enum.Parse(typeof(MAV_CMD), lw.id.ToString()); // command id
isHome = lw.ishome != 0;
coordinateFrameOption = lw.options == 1 ? CoordinateFrameOption.MAV_FRAME_GLOBAL_RELATIVE_ALT : CoordinateFrameOption.MAV_FRAME_GLOBAL;
// alt is in meters, can be above the ground (AGL) or above mean sea level (MSL), depending on coordinateFrameOption
geoPosition = new GeoPosition(lw.lng, lw.lat, lw.alt);
// p1-p4 are just float point numbers that can be added to waypoints and be interpreted by behaviors. We pass them all directly.
p1 = lw.p1;
p2 = lw.p2;
p3 = lw.p3;
p4 = lw.p4;
}
/// <summary>
/// this constructor is used only for reading mission files
/// </summary>
/// <param name="lw"></param>
internal LocationWp(Locationwp lw)
{
waypointState = WaypointState.None;
number = lw.number;
id = (MAV_CMD)Enum.Parse(typeof(MAV_CMD), lw.id.ToString()); // command id
isHome = lw.ishome != 0;
coordinateFrameOption = lw.options == 1 ? CoordinateFrameOption.MAV_FRAME_GLOBAL_RELATIVE_ALT : CoordinateFrameOption.MAV_FRAME_GLOBAL;
// alt is in meters, can be above the ground (AGL) or above mean sea level (MSL), depending on coordinateFrameOption
geoPosition = new GeoPosition(lw.lng, lw.lat, lw.alt);
// p1-p4 are just float point numbers that can be added to waypoints and be interpreted by behaviors. We pass them all directly.
p1 = lw.p1;
p2 = lw.p2;
p3 = lw.p3;
p4 = lw.p4;
}
public bool doCommand(MAV_CMD actionid, float p1, float p2, float p3, float p4, float p5, float p6, float p7)
{
giveComport = true;
MAVLinkMessage buffer;
mavlink_command_long_t req = new mavlink_command_long_t();
req.target_system = MAV.sysid;
req.target_component = MAV.compid;
req.command = (ushort) actionid;
req.param1 = p1;
req.param2 = p2;
req.param3 = p3;
req.param4 = p4;
req.param5 = p5;
req.param6 = p6;
req.param7 = p7;
log.InfoFormat("doCommand cmd {0} {1} {2} {3} {4} {5} {6} {7}", actionid.ToString(), p1, p2, p3, p4, p5, p6,
p7);
generatePacket((byte) MAVLINK_MSG_ID.COMMAND_LONG, req);
DateTime start = DateTime.Now;
int retrys = 3;
int timeout = 2000;
// imu calib take a little while
if (actionid == MAV_CMD.PREFLIGHT_CALIBRATION && p5 == 1)
{
// this is for advanced accel offsets, and blocks execution
return true;
}
else if (actionid == MAV_CMD.PREFLIGHT_CALIBRATION)
{
retrys = 1;
timeout = 25000;
}
else if (actionid == MAV_CMD.PREFLIGHT_REBOOT_SHUTDOWN)
{
generatePacket((byte) MAVLINK_MSG_ID.COMMAND_LONG, req);
giveComport = false;
return true;
}
else if (actionid == MAV_CMD.COMPONENT_ARM_DISARM)
{
// 10 seconds as may need an imu calib
timeout = 10000;
}
else if (actionid == MAV_CMD.PREFLIGHT_CALIBRATION && p6 == 1)
{
// compassmot
// send again just incase
generatePacket((byte) MAVLINK_MSG_ID.COMMAND_LONG, req);
giveComport = false;
return true;
}
else if (actionid == MAV_CMD.GET_HOME_POSITION)
{
return true;
}
while (true)
{
if (!(start.AddMilliseconds(timeout) > DateTime.Now))
{
if (retrys > 0)
{
log.Info("doAction Retry " + retrys);
generatePacket((byte) MAVLINK_MSG_ID.COMMAND_LONG, req);
start = DateTime.Now;
retrys--;
continue;
}
giveComport = false;
throw new Exception("Timeout on read - doAction");
}
buffer = readPacket();
if (buffer.Length > 5)
{
if (buffer.msgid == (byte) MAVLINK_MSG_ID.COMMAND_ACK)
{
var ack = buffer.ToStructure<mavlink_command_ack_t>();
if (ack.result == (byte) MAV_RESULT.ACCEPTED)
{
giveComport = false;
return true;
}
else
{
giveComport = false;
return false;
}
}
}
}
}
public bool doCommand(MAV_CMD actionid, float p1, float p2, float p3, float p4, float p5, float p6, float p7)
{
MainV2.givecomport = true;
byte[] buffer;
__mavlink_command_long_t req = new __mavlink_command_long_t();
req.target_system = sysid;
req.target_component = compid;
req.command = (ushort)actionid;
req.param1 = p1;
req.param2 = p2;
req.param3 = p3;
req.param4 = p4;
req.param5 = p5;
req.param6 = p6;
req.param7 = p7;
generatePacket(MAVLINK_MSG_ID_COMMAND_LONG, req);
DateTime start = DateTime.Now;
int retrys = 3;
int timeout = 2000;
// imu calib take a little while
if (actionid == MAV_CMD.PREFLIGHT_CALIBRATION)
{
retrys = 1;
timeout = 6000;
}
while (true)
{
if (!(start.AddMilliseconds(timeout) > DateTime.Now))
{
if (retrys > 0)
{
Console.WriteLine("doAction Retry " + retrys);
generatePacket(MAVLINK_MSG_ID_COMMAND_LONG, req);
start = DateTime.Now;
retrys--;
continue;
}
MainV2.givecomport = false;
throw new Exception("Timeout on read - doAction");
}
buffer = readPacket();
if (buffer.Length > 5)
{
if (buffer[5] == MAVLINK_MSG_ID_COMMAND_ACK)
{
__mavlink_command_ack_t ack = new __mavlink_command_ack_t();
object temp = (object)ack;
ByteArrayToStructure(buffer, ref temp, 6);
ack = (__mavlink_command_ack_t)(temp);
if (ack.result == (byte)MAV_RESULT.MAV_RESULT_ACCEPTED)
{
MainV2.givecomport = false;
return true;
}
else
{
MainV2.givecomport = false;
return false;
}
}
}
}
#else
MainV2.givecomport = true;
byte[] buffer;
__mavlink_waypoint_set_current_t req = new __mavlink_waypoint_set_current_t();
req.target_system = sysid;
req.target_component = compid;
req.seq = index;
generatePacket(MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT, req);
DateTime start = DateTime.Now;
int retrys = 5;
while (true)
{
if (!(start.AddMilliseconds(2000) > DateTime.Now))
{
if (retrys > 0)
{
Console.WriteLine("setWPCurrent Retry " + retrys);
generatePacket(MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT, req);
start = DateTime.Now;
retrys--;
continue;
}
MainV2.givecomport = false;
throw new Exception("Timeout on read - setWPCurrent");
}
buffer = readPacket();
if (buffer.Length > 5)
{
if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT_CURRENT)
{
MainV2.givecomport = false;
return true;
}
}
}
}
public bool doCommand(MAV_CMD actionid, float p1, float p2, float p3, float p4, float p5, float p6, float p7)
{
giveComport = true;
byte[] buffer;
mavlink_command_long_t req = new mavlink_command_long_t();
req.target_system = MAV.sysid;
req.target_component = MAV.compid;
if (actionid == MAV_CMD.COMPONENT_ARM_DISARM)
{
req.target_component = (byte)MAV_COMPONENT.MAV_COMP_ID_SYSTEM_CONTROL;
}
req.command = (ushort)actionid;
req.param1 = p1;
req.param2 = p2;
req.param3 = p3;
req.param4 = p4;
req.param5 = p5;
req.param6 = p6;
req.param7 = p7;
generatePacket((byte)MAVLINK_MSG_ID.COMMAND_LONG, req);
DateTime start = DateTime.Now;
int retrys = 3;
int timeout = 2000;
// imu calib take a little while
if (actionid == MAV_CMD.PREFLIGHT_CALIBRATION && p5 == 1)
{
// this is for advanced accel offsets, and blocks execution
return true;
}
else if (actionid == MAV_CMD.PREFLIGHT_CALIBRATION)
{
retrys = 1;
timeout = 25000;
}
else if (actionid == MAV_CMD.PREFLIGHT_REBOOT_SHUTDOWN)
{
generatePacket((byte)MAVLINK_MSG_ID.COMMAND_LONG, req);
giveComport = false;
return true;
}
else if (actionid == MAV_CMD.COMPONENT_ARM_DISARM)
{
// 10 seconds as may need an imu calib
timeout = 10000;
}
while (true)
{
if (!(start.AddMilliseconds(timeout) > DateTime.Now))
{
if (retrys > 0)
{
log.Info("doAction Retry " + retrys);
generatePacket((byte)MAVLINK_MSG_ID.COMMAND_LONG, req);
start = DateTime.Now;
retrys--;
continue;
}
giveComport = false;
throw new Exception("Timeout on read - doAction");
}
buffer = readPacket();
if (buffer.Length > 5)
{
if (buffer[5] == (byte)MAVLINK_MSG_ID.COMMAND_ACK)
{
var ack = buffer.ByteArrayToStructure<mavlink_command_ack_t>(6);
if (ack.result == (byte)MAV_RESULT.ACCEPTED)
{
giveComport = false;
return true;
}
else
{
giveComport = false;
return false;
}
}
}
}
}
public bool doCommand(MAV_CMD actionid, float p1, float p2, float p3, float p4, float p5, float p6, float p7,
bool requireack = true)
{
return doCommand(MAV.sysid, MAV.compid, actionid, p1, p2, p3, p4, p5, p6, p7, requireack);
}
