public void Send(ATCommand command)
{
_commandQueue.Enqueue(command);
}
public void Send(ATCommand command)
{
_commandQueue.Enqueue(command);
}
private void ProcessRequestedState()
{
switch (_requestedState)
{
case RequestedState.None:
return;
case RequestedState.Initialize:
ATCommand cmdNavdataDemo = _configuration.General.NavdataDemo.Set(false).ToCommand();
_commandQueue.Enqueue(cmdNavdataDemo);
_commandQueue.Enqueue(new ControlCommand(ControlMode.NoControlMode));
_requestedState = RequestedState.GetConfiguration;
return;
case RequestedState.GetConfiguration:
_configurationAcquisitionWorker.Start();
if (_navigationData.State.HasFlag(NavigationState.Command))
{
_commandQueue.Enqueue(new ControlCommand(ControlMode.AckControlMode));
}
else
{
_commandQueue.Enqueue(new ControlCommand(ControlMode.CfgGetControlMode));
_requestedState = RequestedState.None;
}
break;
case RequestedState.Emergency:
if (_navigationData.State.HasFlag(NavigationState.Flying))
{
_commandQueue.Enqueue(new RefCommand(RefMode.Emergency));
}
else
{
_requestedState = RequestedState.None;
}
break;
case RequestedState.ResetEmergency:
_commandQueue.Enqueue(new RefCommand(RefMode.Emergency));
_requestedState = RequestedState.None;
break;
case RequestedState.Land:
if (_navigationData.State.HasFlag(NavigationState.Flying) &&
_navigationData.State.HasFlag(NavigationState.Landing) == false)
{
_commandQueue.Enqueue(new RefCommand(RefMode.Land));
}
else
{
_requestedState = RequestedState.None;
}
break;
case RequestedState.Fly:
if (_navigationData.State.HasFlag(NavigationState.Landed) &&
_navigationData.State.HasFlag(NavigationState.Takeoff) == false &&
_navigationData.State.HasFlag(NavigationState.Emergency) == false &&
_navigationData.Battery.Low == false)
{
_commandQueue.Enqueue(new RefCommand(RefMode.Takeoff));
}
else
{
_requestedState = RequestedState.None;
}
break;
default:
throw new ArgumentOutOfRangeException();
}
}
private void ProcessTransition()
{
if (_initializationRequested == false)
{
_initializationRequested = true;
_stateRequest = StateRequest.Initialization;
}
switch (_stateRequest)
{
case StateRequest.None:
return;
case StateRequest.Initialization:
_commandQueue.Flush();
ATCommand cmdNavdataDemo = _droneConfiguration.General.NavdataDemo.Set(false).ToCommand();
Send(cmdNavdataDemo);
Send(new ControlCommand(ControlMode.NoControlMode));
_stateRequest = StateRequest.Configuration;
return;
case StateRequest.Configuration:
_configurationAcquisition.Start();
if (_navigationData.State.HasFlag(NavigationState.Command))
{
Send(new ControlCommand(ControlMode.AckControlMode));
}
else
{
Send(new ControlCommand(ControlMode.CfgGetControlMode));
_stateRequest = StateRequest.None;
}
break;
case StateRequest.Emergency:
if (_navigationData.State.HasFlag(NavigationState.Flying))
{
Send(new RefCommand(RefMode.Emergency));
}
else
{
_stateRequest = StateRequest.None;
}
break;
case StateRequest.ResetEmergency:
Send(new RefCommand(RefMode.Emergency));
_stateRequest = StateRequest.None;
break;
case StateRequest.Land:
if (_navigationData.State.HasFlag(NavigationState.Flying) &&
_navigationData.State.HasFlag(NavigationState.Landing) == false)
{
Send(new RefCommand(RefMode.Land));
}
else
{
_stateRequest = StateRequest.None;
}
break;
case StateRequest.Fly:
if (_navigationData.State.HasFlag(NavigationState.Landed) &&
_navigationData.State.HasFlag(NavigationState.Takeoff) == false &&
_navigationData.State.HasFlag(NavigationState.Emergency) == false &&
_navigationData.Battery.Low == false)
{
Send(new RefCommand(RefMode.Takeoff));
}
else
{
_stateRequest = StateRequest.None;
}
break;
default:
throw new ArgumentOutOfRangeException();
}
}