public void Process( RideOnMotion.Inputs.InputState inputState )
{
if ( inputState.Land && _drone.CanLand )
_drone.Land();
else if ( inputState.TakeOff && _drone.CanTakeoff )
_drone.Takeoff();
if ( inputState.Hover && _drone.CanEnterHoverMode )
_drone.EnterHoverMode();
else if ( inputState.Hover && _drone.CanLeaveHoverMode )
_drone.LeaveHoverMode();
if ( inputState.CameraSwap )
_drone.ChangeCamera();
if ( inputState.Emergency )
_drone.Emergency();
else if ( inputState.FlatTrim )
_drone.FlatTrim();
if ( inputState.SpecialAction )
_drone.PlayLED();
float roll = inputState.Roll / 3.0f;
inputState.Roll = roll;
float pitch = inputState.Pitch / 3.0f;
inputState.Pitch = pitch;
float yaw = inputState.Yaw / 2.0f;
inputState.Yaw = yaw;
float gaz = inputState.Gaz / 3.0f;
inputState.Gaz = gaz;
RelativeDirection = true;
float roll2;
float pitch2;
if ( RelativeDirection )
{
roll2 = roll;
pitch2 = pitch;
}
else
{
roll2 = (( (float)Math.Cos( ( Math.PI / 180 ) * DroneOrientationDifference ) ) * roll )
+ (( (float)Math.Sin( ( Math.PI / 180 ) * DroneOrientationDifference ) ) * -pitch );
pitch2 = ( ( (float)Math.Cos( ( Math.PI / 180 ) * DroneOrientationDifference ) ) * pitch )
+ ( ( (float)Math.Sin( ( Math.PI / 180 ) * DroneOrientationDifference ) ) * roll );
inputState.Roll = roll2;
inputState.Pitch = pitch2;
// for debugging purpose
//Logger.Instance.NewEntry( CKLogLevel.Info, CKTraitTags.ARDrone, "Navigate with : roll2 : " + roll2 + " , pitch2 : " + pitch2);
}
_drone.Navigate( roll2, pitch2, yaw, gaz );
Logger.Instance.NewEntry( CKLogLevel.Info, CKTraitTags.ARDrone, inputState.ToString());
}
public RideOnMotion.Inputs.InputState GetCurrentControlInput(RideOnMotion.Inputs.InputState _lastInputState)
{
// TODO test
if (roll != _lastInputState.Roll || pitch != _lastInputState.Pitch || yaw != _lastInputState.Yaw || gaz != _lastInputState.Gaz || cameraSwap != _lastInputState.CameraSwap || takeOff != _lastInputState.TakeOff ||
land != _lastInputState.Land || hover != _lastInputState.Hover || emergency != _lastInputState.Emergency || flatTrim != _lastInputState.FlatTrim || specialActionButton != _lastInputState.SpecialAction)
{
RideOnMotion.Inputs.InputState newInputState = new RideOnMotion.Inputs.InputState(roll, pitch, yaw, gaz, cameraSwap, takeOff, land, hover, emergency, flatTrim, specialActionButton);
return newInputState;
}
else
{
return null;
}
}