/// <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);
}
public JoystickOutput MoveRelatively(HeliState s, float wantedHVelocityForward,
float wantedHVelocityRight, float wantedYawAngleDeg, float wantedHeightAboveGround,
long totalTicks, out ControlGoal control)
{
// Max degrees to tilt the cyclic when accelerating/decelerating
const float maxCyclicAngleDeg = 10.0f;
Vector2 hVelocityVector = VectorHelper.ToHorizontal(s.Velocity);
Vector2 hForwardNorm = Vector2.Normalize(VectorHelper.ToHorizontal(s.Forward));
Vector2 hRightNorm = Vector2.Normalize(VectorHelper.ToHorizontal(s.Right));
float hVelocityForward = VectorHelper.Project(hVelocityVector, hForwardNorm);
float hVelocityRight = VectorHelper.Project(hVelocityVector, hRightNorm);
// Errors in velocity (derivative of position)
float hVelocityForwardError = hVelocityForward - wantedHVelocityForward;
float hVelocityRightError = hVelocityRight - wantedHVelocityRight;
// Too much velocity should trigger deceleration and vice versa.
// Forwards acceleration means pitching nose down, and rightwards acceleration means rolling right.
// -1 == max deceleration, +1 == max acceleration
float wantedForwardsAcceleration = PIDSetup.ForwardsAccel.ComputeExplicit(0, 0, hVelocityForwardError);
float wantedRightwardsAcceleration = PIDSetup.RightwardsAccel.ComputeExplicit(0, 0, hVelocityRightError);
// Determine the pitching/rolling angles to achieve wanted acceleration/deceleration in either direction
// Invert pitch; negative angle (nose down) to accelerate.
float wantedPitchAngleDeg = -wantedForwardsAcceleration * maxCyclicAngleDeg;
float wantedRollAngleDeg = wantedRightwardsAcceleration * maxCyclicAngleDeg;
// Determine the current errors in pitch and roll
// Then adjust the inputs accordingly
var moveToTargetInput = new JoystickOutput();
moveToTargetInput.Pitch = Pitch(s.Degrees.PitchAngle, wantedPitchAngleDeg, totalTicks);
moveToTargetInput.Roll = Roll(s.Degrees.RollAngle, wantedRollAngleDeg, totalTicks);
moveToTargetInput.Yaw = Yaw(s.Degrees.HeadingAngle, wantedYawAngleDeg, totalTicks);
moveToTargetInput.Throttle = (wantedHeightAboveGround > 0)
? Throttle(s.HeightAboveGround, wantedHeightAboveGround, totalTicks)
: Throttle(s.Position.Y, s.Waypoint.Position.Y, totalTicks);
// This is used for debugging and visual feedback
control = new ControlGoal
{
HVelocity = 0,//wantedVelocity,
PitchAngle = MathHelper.ToRadians(wantedPitchAngleDeg),
RollAngle = MathHelper.ToRadians(wantedRollAngleDeg),
HeadingAngle = 0,
};
return(moveToTargetInput);
}
/// <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));
}
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
});
}
/// <summary>
///
/// </summary>
/// <param name="maxHVelocity"></param>
/// <param name="control"></param>
/// <param name="s"></param>
/// <param name="totalTicks">TimeSpan ticks (100ns).</param>
/// <param name="holdHeightAboveGround">If larger than zero, this will override the throttle to hold an altitude above ground.</param>
/// <returns></returns>
public JoystickOutput MoveTowardsGoal(float maxHVelocity, out ControlGoal control, HeliState s, long totalTicks, float holdHeightAboveGround)
{
// Vector2 bearing = VectorHelper.ToHorizontal(s.Velocity);
float hGoalDistance = s.HPositionToGoal.Length();//VectorHelper.ToHorizontal(s.Position - s.Waypoint.Position).Length();
const float secondsToStop = 1.0f;
float metersToStop = secondsToStop * maxHVelocity;
float wantedHVelocity = MathHelper.Lerp(0, maxHVelocity, MathHelper.Clamp(hGoalDistance / metersToStop, 0, 1));
// When flying in sloped terrains we risk crashing if the terrain altitude changes quicker than
// the autopilot can follow at the current horizontal velocity.
// If that is the case, stop horizontal motion while trying to stabilize height above ground.
// if (moveToTargetInput.Throttle == 1 || moveToTargetInput.Throttle == 0)
// wantedHVelocity = 0;
Vector2 hForwardNorm = Vector2.Normalize(VectorHelper.ToHorizontal(s.Forward));
Vector2 hRightNorm = Vector2.Normalize(VectorHelper.ToHorizontal(s.Right));
Vector2 wantedHVelocityVector = wantedHVelocity * Vector2.Normalize(s.HPositionToGoal);
float wantedHVelocityForward = (wantedHVelocity != 0)
? VectorHelper.Project(wantedHVelocityVector, hForwardNorm) : 0;
float wantedHVelocityRight = (wantedHVelocity != 0)
? VectorHelper.Project(wantedHVelocityVector, hRightNorm) : 0;
// If helicopter is nearing a waypoint at which it should hover for a while, then
// let the helicopter yaw to the waypoint heading angle while hovering.
// Otherwise we want the helicopter to head in the bearing direction.
Vector2 hVelocityVector = VectorHelper.ToHorizontal(s.Velocity);
float hVelocity = hVelocityVector.Length();
float wantedYawAngleDeg = (s.Waypoint.Type == WaypointType.Hover && hVelocity < 1)
? MathHelper.ToDegrees(s.Waypoint.HeadingAngle)
: s.Degrees.GoalAngle;
return(MoveRelatively(s, wantedHVelocityForward, wantedHVelocityRight,
wantedYawAngleDeg, holdHeightAboveGround, totalTicks, out control));
}
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
});
}
public UserControlBoxes()
{
InitializeComponent();
control = new ControlGoal();
}
/// <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="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);
}
