private CommandAck fetchCommandAck(MAVLink.MAV_CMD cmdType)
{
DateTime deadline = DateTime.Now.AddMilliseconds(cmdtimeout_ms);
while (DateTime.Now < deadline)
{
if (drone.commandAckStacks.ContainsKey(cmdType))
{
lock (drone.commandAckStacks[cmdType])
{
Stack <CommandAck> stack = drone.commandAckStacks[cmdType];
if (stack.Count > 0)
{
CommandAck result = stack.Pop();
logger.Debug("Command Ack received for {0} command with status {1}", cmdType, result.result.ToString());
return(result);
}
}
}
// sleep
Thread.Sleep(cmdpoll_ms);
}
logger.Error("Error: timeout waiting for command result for {0}", cmdType);
return(null);
}
public SMissionItem Set(double lat, double lng, double alt, MAVLink.MAV_CMD command, bool isHome = false)
{
this.lat = lat;
this.lng = lng;
this.alt = (float)alt;
this.command = command;
IsHome = isHome;
return(this);
}
public Mavlink_Command(MAVLink.MAV_CMD _actionid, float _p1, float _p2, float _p3, float _p4, float _p5, float _p6, float _p7, bool _requireack = true)
{
actionid = _actionid;
p1 = _p1;
p2 = _p2;
p3 = _p3;
p4 = _p4;
p5 = _p5;
p6 = _p6;
p7 = _p7;
requireack = _requireack;
}
private CommandAck runCommand(MAVLink.MAV_CMD command, Int32 param1,
Int32 param2,
Int32 param3,
Int32 param4,
Int32 param5,
Int32 param6,
Int32 param7)
{
lock (commandLock)
{
logger.Debug("Running {0} command.", command);
connection.sendCommand((ushort)command, param1, param2,
param3, param4, param5, param6,
param7);
return(fetchCommandAck(command));
}
}
private static void CommandLong(MAVLink.MAV_CMD command, float param1, float param2, float param3, float param4)
{
MAVLink.mavlink_command_long_t cmd = new MAVLink.mavlink_command_long_t();
cmd.target_system = sysID;
cmd.target_component = compID;
cmd.command = (ushort)command;
cmd.param1 = param1;
cmd.param2 = param2;
cmd.param3 = param3;
cmd.param4 = param4;
byte[] commandBytes = mavlink.GenerateMAVLinkPacket(MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, cmd);
SendPacket(commandBytes);
}
/// <summary>
/// add wp to command queue - dont upload to mav
/// </summary>
/// <param name="cmd"></param>
/// <param name="p1"></param>
/// <param name="p2"></param>
/// <param name="p3"></param>
/// <param name="p4"></param>
/// <param name="x"></param>
/// <param name="y"></param>
/// <param name="z"></param>
public void AddWPtoList(MAVLink.MAV_CMD cmd, double p1, double p2, double p3, double p4, double x, double y, double z)
{
MainV2.instance.FlightPlanner.AddCommand(cmd, p1, p2, p3, p4, x, y, z);
}
public void InsertWP(int idx, MAVLink.MAV_CMD cmd, double p1, double p2, double p3, double p4, double x,
double y,
double z)
{
InsertWP(idx, cmd, p1, p2, p3, p4, x, y, z, null);
}
public int AddWPtoList(MAVLink.MAV_CMD cmd, double p1, double p2, double p3, double p4, double x, double y,
double z)
{
return(AddWPtoList(cmd, p1, p2, p3, p4, x, y, z, null));
}
public void InsertWP(int idx, MAVLink.MAV_CMD cmd, double p1, double p2, double p3, double p4, double x,
double y,
double z, object tag = null)
{
MainV2.instance.FlightPlanner.InsertCommand(idx, cmd, p1, p2, p3, p4, x, y, z);
}
/// <summary>
/// add wp to command queue - dont upload to mav
/// </summary>
/// <param name="cmd"></param>
/// <param name="p1"></param>
/// <param name="p2"></param>
/// <param name="p3"></param>
/// <param name="p4"></param>
/// <param name="x"></param>
/// <param name="y"></param>
/// <param name="z"></param>
public int AddWPtoList(MAVLink.MAV_CMD cmd, double p1, double p2, double p3, double p4, double x, double y,
double z, object tag = null)
{
return(MainV2.instance.FlightPlanner.AddCommand(cmd, p1, p2, p3, p4, x, y, z, tag));
}
/// <summary>
/// add wp to command queue - dont upload to mav
/// </summary>
/// <param name="cmd"></param>
/// <param name="p1"></param>
/// <param name="p2"></param>
/// <param name="p3"></param>
/// <param name="p4"></param>
/// <param name="x"></param>
/// <param name="y"></param>
/// <param name="z"></param>
public void AddWPtoList(MAVLink.MAV_CMD cmd, float p1, float p2, float p3, float p4, float x, float y, float z)
{
MainV2.instance.FlightPlanner.AddCommand(cmd, p1, p2, p3, p4, x, y, z);
}
private static void CommandLong(MAVLink.MAV_CMD command, float param1, float param2)
{
CommandLong(command, param1, param2, 0, 0);
}
public static SMissionItem Create(double lat, double lng, double alt, MAVLink.MAV_CMD command, bool isHome = false)
{
return(new SMissionItem().Set(lat, lng, alt, command, isHome));
}
