protected void GenerateDroneCommand()
{
lock (ThisLock)
{
if (FlyVector.IsNull())
{
return;
}
CommandTuple cmdTuple = new CommandTuple(1, 0, 2);
int roll = Clamp(FlyVector.Roll, VectorMin, VectorMax);
int pitch = Clamp(FlyVector.Pitch, VectorMin, VectorMax);
int yaw = Clamp(FlyVector.Yaw, VectorMin, VectorMax);
int gaz = Clamp(FlyVector.Gaz, VectorMin, VectorMax);
CommandParam cmdParam = new CommandParam();
cmdParam.AddData((byte)FlyVector.Flag);
cmdParam.AddData((sbyte)roll);
cmdParam.AddData((sbyte)pitch);
cmdParam.AddData((sbyte)yaw);
cmdParam.AddData((sbyte)gaz);
cmdParam.AddData((uint)0);
SendSinglePcmd(cmdTuple, cmdParam);
Thread.Sleep(100);
}
}
protected bool?SendParam(CommandTuple cmdTuple, CommandParam cmdParam, bool ack = true)
{
// ReSharper disable once InconsistentNaming
string ACK = ack ? "SEND_WITH_ACK" : "SEND_NO_ACK";
// ReSharper disable once InconsistentNaming
string DataACK = ack ? "DATA_WITH_ACK" : "DATA_NO_ACK";
// ReSharper disable once StringLiteralTypo
string fmt = "<BBBIBBH" + cmdParam.Format();
SequenceCounter[ACK] = (SequenceCounter[ACK] + 1) % 256;
Command cmd = new Command();
cmd.InsertData((byte)DataTypesByName[DataACK]);
cmd.InsertData((byte)BufferIds[ACK]);
cmd.InsertData((byte)SequenceCounter[ACK]);
cmd.InsertData((uint)StructConverter.PacketSize(fmt));
cmd.InsertTuple(cmdTuple);
cmd.InsertParam(cmdParam);
if (ack)
{
return(SendCommandAck(cmd.Export(fmt), SequenceCounter["SEND_WITH_ACK"]));
}
SendCommandNoAck(cmd.Export(fmt));
return(null);
}
public void Land(int timeout)
{
// We do not want to fly while landing
lock (ThisLock)
{
FlyVector.ResetVector();
Logger.Debug("Landing...");
CommandTuple cmdTuple = new CommandTuple(1, 0, 3);
SendNoParam(cmdTuple);
Stopwatch sw = new Stopwatch();
sw.Start();
while (FlyingState.GetState() != FlyingState.State.Landed && sw.ElapsedMilliseconds < timeout)
{
if (FlyingState.GetState() == FlyingState.State.Emergency)
{
break;
}
SmartSleep(100);
}
sw.Stop();
}
}
public void TakeOff(int timeout)
{
Logger.Debug("Performing takeoff...");
CommandTuple cmdTuple = new CommandTuple(1, 0, 1);
if (FlyingState.GetState() == FlyingState.State.Landed || FlyingState.GetState() == FlyingState.State.UnKn0wn)
{
SendNoParam(cmdTuple);
}
else
{
return;
}
Stopwatch sw = new Stopwatch();
sw.Start();
while (FlyingState.GetState() != FlyingState.State.Hovering && FlyingState.GetState() != FlyingState.State.Flying && sw.ElapsedMilliseconds < timeout)
{
if (FlyingState.GetState() == FlyingState.State.Emergency)
{
break;
}
SmartSleep(100);
}
sw.Stop();
}
public void StopVideo()
{
CommandTuple cmdTuple = new CommandTuple(1, 21, 0);
CommandParam cmdParam = new CommandParam();
cmdParam.AddData((byte)0); // Disable Video
SendParam(cmdTuple, cmdParam);
}
protected bool SendNoParam(CommandTuple cmdTuple)
{
SequenceCounter["SEND_WITH_ACK"] = (SequenceCounter["SEND_WITH_ACK"] + 1) % 256;
// ReSharper disable once StringLiteralTypo
const string fmt = "<BBBIBBH";
Command cmd = new Command();
cmd.InsertData((byte)DataTypesByName["DATA_WITH_ACK"]);
cmd.InsertData((byte)BufferIds["SEND_WITH_ACK"]);
cmd.InsertData((byte)SequenceCounter["SEND_WITH_ACK"]);
cmd.InsertData((uint)StructConverter.PacketSize(fmt));
cmd.InsertTuple(cmdTuple);
return(SendCommandAck(cmd.Export(fmt), SequenceCounter["SEND_WITH_ACK"]));
}
// ReSharper disable once IdentifierTypo
protected bool SendSinglePcmd(CommandTuple cmdTuple, CommandParam cmdParam)
{
SequenceCounter["SEND_NO_ACK"] = (SequenceCounter["SEND_NO_ACK"] + 1) % 256;
// ReSharper disable once StringLiteralTypo
string fmt = "<BBBIBBH" + cmdParam.Format();
Command cmd = new Command();
cmd.InsertData((byte)DataTypesByName["DATA_NO_ACK"]);
cmd.InsertData((byte)BufferIds["SEND_NO_ACK"]);
cmd.InsertData((byte)SequenceCounter["SEND_NO_ACK"]);
cmd.InsertData((uint)StructConverter.PacketSize(fmt));
cmd.InsertTuple(cmdTuple);
cmd.InsertParam(cmdParam);
return(SafeSend(cmd.Export(fmt)));
}
public void FlatTrim(int duration)
{
if (duration > 0)
{
FlatTrimChanged.Reset();
}
CommandTuple cmdTuple = new CommandTuple(1, 0, 0);
SendNoParam(cmdTuple);
// Wait for FlatTrimChanged or duration
if (duration > 0)
{
Stopwatch sw = new Stopwatch();
sw.Start();
while (!FlatTrimChanged.Updated && sw.ElapsedMilliseconds < duration)
{
SmartSleep(100);
}
sw.Stop();
}
}
public void AskForStateUpdate()
{
CommandTuple cmdTuple = new CommandTuple(0, 4, 0);
SendNoParam(cmdTuple);
}
