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)
{
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;
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(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 = 25000;
}
while (true)
{
if (!(start.AddMilliseconds(timeout) > DateTime.Now))
{
if (retrys > 0)
{
log.Info("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)
{
var ack = buffer.ByteArrayToStructure<mavlink_command_ack_t>(6);
if (ack.result == (byte)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)
{
log.Info("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;
}
}
}
}
