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(); } }
public void Send(ATCommand command) { _commandQueue.Enqueue(command); }