public bool doCommand(byte sysid, byte compid, MAV_CMD actionid, float p1, float p2, float p3, float p4, float p5, float p6, float p7, bool requireack = true)
{
giveComport = true;
MAVLinkMessage 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;
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, sysid, compid);
if (!requireack)
{
giveComport = false;
return true;
}
DateTime GUI = DateTime.Now;
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
giveComport = false;
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, sysid, compid);
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, sysid, compid);
giveComport = false;
return true;
}
else if (actionid == MAV_CMD.GET_HOME_POSITION)
{
giveComport = false;
return true;
}
while (true)
{
if (DateTime.Now > GUI.AddMilliseconds(100))
{
GUI = DateTime.Now;
if (!MainV2.instance.InvokeRequired)
{
try
{
MainV2.instance.Invalidate();
MainV2.instance.FlightData.Invalidate();
MainV2.instance.Update();
}
catch { }
}
}
if (!(start.AddMilliseconds(timeout) > DateTime.Now))
{
if (retrys > 0)
{
log.Info("doCommand Retry " + retrys);
generatePacket((byte) MAVLINK_MSG_ID.COMMAND_LONG, req, sysid, compid);
start = DateTime.Now;
retrys--;
continue;
}
giveComport = false;
throw new TimeoutException("Timeout on read - doCommand");
}
buffer = readPacket();
if (buffer.Length > 5)
{
if (buffer.msgid == (byte) MAVLINK_MSG_ID.COMMAND_ACK && buffer.sysid == sysid && buffer.compid == compid)
{
var ack = buffer.ToStructure<mavlink_command_ack_t>();
if (ack.command != req.command)
{
log.InfoFormat("doCommand cmd resp {0} - {1} - Commands dont match", (MAV_CMD)ack.command, (MAV_RESULT)ack.result);
continue;
}
log.InfoFormat("doCommand cmd resp {0} - {1}", (MAV_CMD)ack.command, (MAV_RESULT)ack.result);
if (ack.result == (byte) MAV_RESULT.ACCEPTED)
{
giveComport = false;
return true;
}
else
{
giveComport = false;
return false;
}
}
}
}
}