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));
}
}
public bool Check(Command command)
{
if (!(command is FlightMoveCommand) && !(command is HoverModeCommand))
return true;
if(command is HoverModeCommand)
{
lastRollValue = 0;
lastPitchValue = 0;
lastYawValue = 0;
lastGazValue = 0;
return true;
}
var 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 SendUnqueuedCommand(Command command)
{
command.SequenceNumber = GetSequenceNumberForCommand();
SendMessage(command.CreateCommand(firmwareVersion));
}
private void UpdateCurrentCamera(Command command)
{
if (command is SwitchCameraCommand)
{
DroneCameraMode cameraMode = ((SwitchCameraCommand)command).CameraMode;
if (cameraMode == DroneCameraMode.NextMode)
{
switch (currentCameraMode)
{
case DroneCameraMode.FrontCamera:
cameraMode = DroneCameraMode.BottomCamera;
break;
case DroneCameraMode.BottomCamera:
cameraMode = DroneCameraMode.PictureInPictureFront;
break;
case DroneCameraMode.PictureInPictureFront:
cameraMode = DroneCameraMode.PictureInPictureBottom;
break;
case DroneCameraMode.PictureInPictureBottom:
cameraMode = DroneCameraMode.FrontCamera;
break;
}
}
currentCameraMode = cameraMode;
}
}
private void ChangeStatusAccordingToCommand(Command command)
{
if (command.HasOutcome(CommandStatusOutcome.SetFlying)) flying = true;
if (command.HasOutcome(CommandStatusOutcome.ClearFlying)) flying = false;
if (command.HasOutcome(CommandStatusOutcome.SetHovering)) hovering = true;
if (command.HasOutcome(CommandStatusOutcome.ClearHovering)) hovering = false;
if (command.HasOutcome(CommandStatusOutcome.SetEmergency)) emergency = true;
if (command.HasOutcome(CommandStatusOutcome.ClearEmergency)) emergency = false;
}
public void SendCommand(Command command)
{
UpdateCurrentCamera(command);
if (IsCommandPossible(command) && CheckFlightMoveCommand(command))
{
commandSender.SendQueuedCommand(command);
ChangeStatusAccordingToCommand(command);
}
}
public bool IsCommandPossible(Command command)
{
if (command.NeedsPrerequisite(CommandStatusPrerequisite.Connected) && !IsConnected) return false;
if (command.NeedsPrerequisite(CommandStatusPrerequisite.NotConnected) && IsConnected) return false;
if (command.NeedsPrerequisite(CommandStatusPrerequisite.Flying) && !IsFlying) return false;
if (command.NeedsPrerequisite(CommandStatusPrerequisite.NotFlying) && IsFlying) return false;
if (command.NeedsPrerequisite(CommandStatusPrerequisite.Hovering) && !IsHovering) return false;
if (command.NeedsPrerequisite(CommandStatusPrerequisite.NotHovering) && IsHovering) return false;
if (command.NeedsPrerequisite(CommandStatusPrerequisite.Emergency) && !IsEmergency) return false;
if (command.NeedsPrerequisite(CommandStatusPrerequisite.NotEmergency) && IsEmergency) return false;
return true;
}
private bool CheckFlightMoveCommand(Command command)
{
return checkFlightMoveCommandStrategy.Check(command);
}
