private JoystickOutput GetOutput_Recovery(HeliState s, long totalTicks, out ControlGoal control)
{
Actions |= Actions.Hover;
Log("Crash imminent, trying to recover!");
control = new ControlGoal
{
HVelocity = 0,
PitchAngle = 0,
RollAngle = 0,
HeadingAngle = 0
};
return new JoystickOutput
{
Throttle = 1.0f,
Roll = Output.Roll(s.Degrees.RollAngle, control.RollAngle, totalTicks),
Pitch = Output.Pitch(s.Degrees.PitchAngle, control.PitchAngle, totalTicks),
Yaw = 0.0f
};
}
public JoystickOutput GetHoverOutput(HeliState s, long totalTicks, out ControlGoal control)
{
Actions |= Actions.Hover;
//Log("Hovering!");
control = new ControlGoal
{
HVelocity = 0,
PitchAngle = 0,
RollAngle = 0,
HeadingAngle = 0
};
return new JoystickOutput
{
// TODO We need to store an initial s.Position.Y instead of using the most current one, so we need a "Hover" command that does this
Throttle = Output.Throttle(s.Position.Y, s.Position.Y, totalTicks),
Roll = Output.Roll(s.Degrees.RollAngle, control.RollAngle, totalTicks),
Pitch = Output.Pitch(s.Degrees.PitchAngle, control.PitchAngle, totalTicks),
Yaw = 0.0f
};
}
/// <summary>
///
/// </summary>
/// <param name="userInput"></param>
/// <param name="s"></param>
/// <param name="totalTicks">TimeSpan ticks (100ns).</param>
/// <param name="control"></param>
/// <returns></returns>
private JoystickOutput GetOutput_AssistedAutopilot(JoystickOutput userInput, HeliState s, long totalTicks,
out ControlGoal control)
{
// Command the output controller based on the user input.
// Joystick commands:
// Forward/backward - Move forwards/backwards
// Right/left - Move rightwards/leftwards
// Throttle - Set the height above the ground to hold
float heightAboveGroundToHold = MyMathHelper.Lerp(userInput.Throttle, 0, 1, 1, 10);
float forwardsVelocity = MyMathHelper.Lerp(-userInput.Pitch, -1, 1, -10, 10);
float rightwardsVelocity = MyMathHelper.Lerp(userInput.Roll, -1, 1, -10, 10);
float headingDeg = s.Degrees.HeadingAngle - 15*userInput.Yaw;
return Output.MoveRelatively(s, forwardsVelocity, rightwardsVelocity, headingDeg, heightAboveGroundToHold,
totalTicks, out control);
}
/// <summary>
///
/// </summary>
/// <param name="s"></param>
/// <param name="totalTicks">TimeSpan ticks (100ns).</param>
/// <param name="control"></param>
/// <returns></returns>
private JoystickOutput GetOutput_EnRoute(HeliState s, long totalTicks, out ControlGoal control)
{
return Output.MoveTowardsGoal(MaxHVelocity, out control, s, totalTicks, Task.HoldHeightAboveGround);
}
/// <summary>
///
/// </summary>
/// <param name="userInput"></param>
/// <param name="s"></param>
/// <param name="ticks">TimeSpan ticks (100ns).</param>
/// <param name="control"></param>
/// <returns></returns>
public JoystickOutput GetAssistedOutput(JoystickOutput userInput, HeliState s, long ticks,
out ControlGoal control)
{
Navigation = NavigationState.AssistedAutopilot;
JoystickOutput result = GetOutput_AssistedAutopilot(userInput, s, ticks, out control);
ProcessNavigation(s, ticks);
return result;
}
/// <summary>
///
/// </summary>
/// <param name="s"></param>
/// <param name="ticks">TimeSpan ticks (100ns).</param>
/// <param name="control"></param>
/// <returns></returns>
public JoystickOutput GetOutput(HeliState s, long ticks, out ControlGoal control)
{
JoystickOutput result;
// Re-fill the list of actions that are being performed in this input
Actions = Actions.None;
// float hDistanceToGoal = Vector2.Distance(VectorHelper.ToHorizontal(s.Position),
// VectorHelper.ToHorizontal(CurrentWaypoint.Position));
// Simplified: Is the position in one second from now a crash?
// If not, are we at the destination?
// HeliState nextState = GetEstimatedState(s, TimeSpan.FromSeconds(1));
// Navigation = nextState.Position.Y < 0
// ? NavigationState.Recovery
// : NavigationState.EnRoute;
// TODO Insert some recovery code as well, commented out now because it triggers too often
Navigation = NavigationState.EnRoute;
// Navigation = (hDistanceToGoal < GoalDistanceTolerance)
// ? NavigationState.AtDestination
// : NavigationState.EnRoute;
// if (CurrentWaypoint.Type != WaypointType.Start) //&&
// if (CurrentWaypoint.Type == WaypointType.Land)
// {
// result = new JoystickInput();
// control = new ControlGoal();
// }
// else
{
switch (Navigation)
{
case NavigationState.Recovery:
Log("Nav: Recovery");
result = GetOutput_Recovery(s, ticks, out control);
break;
case NavigationState.EnRoute:
Log("Nav: EnRoute");
result = GetOutput_EnRoute(s, ticks, out control);
break;
case NavigationState.AtDestination:
Log("Nav: AtDestination");
result = GetHoverOutput(s, ticks, out control);
break;
default:
throw new NotImplementedException("Should return before here.");
}
Log("Actions: " + ActionsToString(Actions));
Log(String.Format("Input: PRYT {0},{1},{2},{3}",
(int) (result.Pitch*100),
(int) (result.Roll*100),
(int) (result.Yaw*100),
(int) (result.Throttle*100)));
}
ProcessNavigation(s, ticks);
return result;
}
