private void ChangeState(LearnForm.State state)
{
if (_nowState != state)
{
switch (state)
{
case LearnForm.State.TakeOff:
DroneSingleton._droneClient.Takeoff();
_commandStartTime = DateTime.Now;
_isCounting = true;
break;
case LearnForm.State.Hover:
DroneSingleton._droneClient.Hover();
_commandStartTime = DateTime.Now;
_isCounting = true;
break;
case LearnForm.State.Up:
DroneSingleton._droneClient.Progress(FlightMode.Progressive, gaz: 0.25f);
_commandStartTime = DateTime.Now;
_isCounting = true;
break;
case LearnForm.State.Down:
DroneSingleton._droneClient.Progress(FlightMode.Progressive, gaz: -0.25f);
_commandStartTime = DateTime.Now;
_isCounting = true;
break;
case LearnForm.State.Forward:
_commandStartTime = DateTime.Now;
DroneSingleton._droneClient.Progress(FlightMode.Progressive, pitch: -0.05f);
_isForwarding = true;
_isCounting = true;
break;
case LearnForm.State.Left:
DroneSingleton._droneClient.Progress(FlightMode.Progressive, roll: -0.05f);
CountTime();
break;
case LearnForm.State.Right:
DroneSingleton._droneClient.Progress(FlightMode.Progressive, roll: 0.05f);
CountTime();
break;
case LearnForm.State.TurnLeft:
DroneSingleton._droneClient.Progress(FlightMode.Progressive, yaw: -0.25f);
break;
case LearnForm.State.TurnRight:
DroneSingleton._droneClient.Progress(FlightMode.Progressive, yaw: 0.25f);
break;
case LearnForm.State.Wait:
DroneSingleton._droneClient.Hover();
_waitStartTime = DateTime.Now;
_waitting = true;
break;
}
_nowState = state;
}
else
{
if (_isCounting && _nowCommand == _nowState)
{
_commandEndTime = DateTime.Now;
TimeSpan costTime = _commandEndTime.Subtract(_commandStartTime);
if (_totalTime.CompareTo(costTime) <= 0)
{
_isCounting = false;
if (_isReturn)
_commandIndex--;
else
_commandIndex++;
ReadCommand();
return;
}
}
if (_waitting)
{
_waitEndTime = DateTime.Now;
TimeSpan costTime = _waitEndTime.Subtract(_waitStartTime);
if (_waitTimeLimit.CompareTo(costTime) <= 0)
{
if (_backHomeWait)
{
_isForwarding = false;
_turning = false;
_needWait = true;
_backHomeWait = false;
_turnBack = true;
}
_waitting = false;
_needWait = false;
return;
}
}
}
}
private void ReadCommand()
{
_isForwarding = false;
_turning = false;
_needWait = true;
_nowCommand = _commandList[_commandIndex];
if (_nowCommand == LearnForm.State.TakeOff)
{
if (_isReturn)
{
DroneSingleton._droneClient.Land();
_controling = false;
_haveLastLine = false;
_nowState = LearnForm.State.Land;
_isForwarding = false;
InitializeChildPanel();
_planeStatePanel._comboBox.IsEnabled = true;
}
else
{
NavdataBag navdataBag;
if (DroneSingleton._navigationPacket.Data != null && NavdataBagParser.TryParse(ref DroneSingleton._navigationPacket, out navdataBag))
{
_heading = navdataBag.magneto.heading_fusion_unwrapped;
_heading = CheckAngle(_heading);
}
_totalTime = _timeList[_commandIndex];
}
}
else if (_nowCommand == LearnForm.State.Land)
{
DroneSingleton._droneClient.Land();
_controling = false;
_haveLastLine = false;
_nowState = LearnForm.State.Land;
_isForwarding = false;
InitializeChildPanel();
_planeStatePanel._comboBox.IsEnabled = true;
}
else if (_nowCommand == LearnForm.State.Hover)
{
_totalTime = _timeList[_commandIndex];
}
else if (_nowCommand == LearnForm.State.Forward)
{
_totalTime = _timeList[_commandIndex];
}
else if (_nowCommand == LearnForm.State.Up || _nowCommand == LearnForm.State.Down)
{
_totalTime = _timeList[_commandIndex];
}
else
{
NavdataBag navdataBag;
if (DroneSingleton._navigationPacket.Data != null && NavdataBagParser.TryParse(ref DroneSingleton._navigationPacket, out navdataBag))
{
_heading = navdataBag.magneto.heading_fusion_unwrapped;
_heading = CheckAngle(_heading);
}
if (_isReturn)
_targetAngle = _angleList[_commandIndex] - 180;
else
_targetAngle = _angleList[_commandIndex];
_turning = true;
}
}
public void OnPlaneStatePanelStartAutoPatrol()
{
_isPatroling = true;
//FLY START
_commandList = _planeStatePanel._mapImage.CommandList;
_timeList = _planeStatePanel._mapImage.TimeList;
_angleList = _planeStatePanel._mapImage.AngleList;
_nowState = LearnForm.State.Land;
_controling = true;
_haveLastLine = false;
_backHomeWait = false;
_isReturn = false;
_turnBack = false;
_commandIndex = 0;
ReadCommand();
}
