void ProcessCommand(Command command)
{
if (command is FlightMoveCommand)
{
FlightMoveCommand moveCommand = (FlightMoveCommand)command;
}
}
private bool CheckFlightMoveCommand(Command command)
{
if (!(command is FlightMoveCommand))
{
return(true);
}
FlightMoveCommand moveCommand = (FlightMoveCommand)command;
if (Math.Abs(moveCommand.Roll - lastRollValue) >= thresholdBetweenSettingCommands ||
Math.Abs(moveCommand.Pitch - lastPitchValue) >= thresholdBetweenSettingCommands ||
Math.Abs(moveCommand.Yaw - lastYawValue) >= thresholdBetweenSettingCommands ||
Math.Abs(moveCommand.Gaz - lastGazValue) >= thresholdBetweenSettingCommands)
{
lastRollValue = moveCommand.Roll;
lastPitchValue = moveCommand.Pitch;
lastYawValue = moveCommand.Yaw;
lastGazValue = moveCommand.Gaz;
return(true);
}
else if (moveCommand.Roll == 0.0f && moveCommand.Pitch == 0.0f &&
moveCommand.Yaw == 0.0f && moveCommand.Gaz == 0.0f)
{
return(true);
}
else
{
return(false);
}
}
private void Navigate(float roll, float pitch, float yaw, float gaz)
{
FlightMoveCommand flightMoveCommand = new FlightMoveCommand(roll, pitch, yaw, gaz);
if (droneControl.IsCommandPossible(flightMoveCommand))
{
droneControl.SendCommand(flightMoveCommand);
}
}
public void SendQueuedCommand(Command command)
{
//command.SequenceNumber = GetSequenceNumberForCommand();
//commandsToSend.Add(command.CreateCommand(FirmwareVersion));
//if (command is SetConfigurationCommand)
//{
// SetControlModeCommand controlModeCommand = new SetControlModeCommand(DroneControlMode.LogControlMode);
// controlModeCommand.SequenceNumber = GetSequenceNumberForCommand();
// commandsToSend.Add(controlModeCommand.CreateCommand(FirmwareVersion));
//}
if (command is FlightMoveCommand)
{
FlightMoveCommand moveCommand = (FlightMoveCommand)command;
this.SetFlightParameters(moveCommand.Roll, moveCommand.Pitch, moveCommand.Yaw, moveCommand.Gaz);
return;
}
queueCommands.Enqueue(command);
}
