public void Reset(TimeSpan totalGameTime, HelicopterScenario scenario, NavigationMap heightmap)
{
Console.WriteLine(@"Resetting helicopter.");
_scenario = scenario;
// TODO We would rather want to do Autopilot.Reset() than this fugly code
Autopilot.IsAtDestination = false;
Autopilot = Autopilot.Clone();
Autopilot.Task = scenario.Task.Clone();
Autopilot.Map = heightmap;
Vector3 startPosition = scenario.StartPosition;
Vector3 startVelocity = Vector3.Zero;
Vector3 startAcceleration = Vector3.Zero;
Quaternion startOrientation = Quaternion.Identity;
if (Task.HoldHeightAboveGround > 0)
{
startPosition.Y = Autopilot.Map.GetAltitude(VectorHelper.ToHorizontal(startPosition)) + Task.HoldHeightAboveGround;
}
var startPhysicalState = new PhysicalHeliState(
startOrientation, startPosition, startVelocity, startAcceleration);
var initialState = new SimulationStepResults(startPhysicalState, totalGameTime);
_physicalState = startPhysicalState;
// Re-create the state provider when resetting because some sensors will have to re-initialize.
_physics = new HeliPhysics(_collision, UseTerrainCollision);
_sensors = new SensorModel(_sensorSpecifications, Autopilot.Map, startPosition, startOrientation);
_perfectStateProvider = new PerfectState();
_estimatedStateProvider = new SensorEstimatedState(_sensors, startPhysicalState);
// Wait for state to become stable.
// while (!_perfectStateProvider.Ready)
// {
// TODO GPS will require N seconds of startup time
// _perfectStateProvider.Update(initialState, 0, 0, new JoystickOutput());
// Sensors.Update(initialState, new JoystickOutput());
// Thread.Sleep(100);
// }
// When resetting, use perfect state as starting point.
// TODO It should not be necessary to create this waypoint since it should never be used for navigation! Delete if safe.
// Use start position and start orientation instead.
const float defaultWaypointRadius = 5;
var startWaypoint = new Waypoint(startPosition, 0, WaypointType.Intermediate, defaultWaypointRadius);
_trueState = StateHelper.ToHeliState(startPhysicalState, GetHeightAboveGround(startPhysicalState.Position), startWaypoint, new JoystickOutput());
_estimatedState = _trueState;
Log.Clear();
}
/// <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 static PhysicalHeliState ToPhysical(HeliState state)
{
// TODO Why is roll angle inverted?
// var orientation = Quaternion.CreateFromYawPitchRoll(
// state.Radians.HeadingAngle,
// state.Radians.PitchAngle,
// -state.Radians.RollAngle);
return(new PhysicalHeliState(state.Orientation, state.Position, state.Velocity, state.Acceleration));
}
/// <summary>
///
/// </summary>
/// <param name="s"></param>
/// <param name="ticks">TimeSpan ticks (100ns).</param>
private void ProcessNavigation(HeliState s, long ticks)
{
// Note: Changing current waypoint must be done after input processing, because
// modifying the waypoint will not update the helicopter state until next time -
// including states about angle and distance to waypoint.
// Make sure we use 2D position (map) when holding height above terrain, in order to pass waypoints.
if (Task.HoldHeightAboveGround > 0)
{
float groundHeightAtWaypoint = Map.GetAltitude(VectorHelper.ToHorizontal(CurrentWaypoint.Position));
CurrentWaypoint.Position.Y = groundHeightAtWaypoint + Task.HoldHeightAboveGround;
}
bool isWithinRadius = IsTestMode
? IsTruePositionWithinRadius
: CurrentWaypoint.IsWithinRadius(s.Position);
if (isWithinRadius)
{
CurrentWaypoint.SecondsWaited += ticks / 1000.0f;
}
else
{
CurrentWaypoint.SecondsWaited = 0; // Seconds waited should be in an uninterrupted sequence
}
// Progress to next waypoint if currently at start point or have been sufficiently long at a stop point
if ( //CurrentWaypoint.Type == WaypointType.Start ||
CurrentWaypoint.Type == WaypointType.Hover && CurrentWaypoint.DoneWaiting ||
CurrentWaypoint.Type == WaypointType.Intermediate && isWithinRadius)
{
Task.Next();
}
else if (CurrentWaypoint.Type == WaypointType.TestDestination &&
isWithinRadius)
{
if (Task.Loop)
{
Task.Next();
}
else
{
IsAtDestination = true;
}
}
if (Task.Current == null)
{
throw new Exception("Waypoint should never be null! Always end task with an end-waypoint.");
}
// s.Waypoint = Task.Current;
// s.HPositionToGoal = VectorHelper.ToHorizontal(s.Waypoint.Position - s.Position);
}
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));
}
/// <summary>
/// TODO Temporary. We might later distinguish between physical state and navigation state.
/// </summary>
/// <param name="s"></param>
/// <param name="heightAboveGround"></param>
/// <param name="waypoint"></param>
/// <param name="output"></param>
/// <returns></returns>
public static HeliState ToHeliState(PhysicalHeliState s, float heightAboveGround, Waypoint waypoint, JoystickOutput output)
{
// Update state
var r = new HeliState
{
HeightAboveGround = heightAboveGround,
Orientation = s.Orientation,
Forward = s.Axes.Forward,
Up = s.Axes.Up,
Right = s.Axes.Right,
Output = output,
Position = s.Position,
Velocity = s.Velocity,
Acceleration = s.Acceleration,
Waypoint = waypoint,
HPositionToGoal = VectorHelper.ToHorizontal(waypoint.Position - s.Position),
};
// Update angles from current state
var radians = new Angles
{
PitchAngle = VectorHelper.GetPitchAngle(s.Orientation),
RollAngle = VectorHelper.GetRollAngle(s.Orientation),
HeadingAngle = VectorHelper.GetHeadingAngle(s.Orientation),
};
// Velocity can be zero, and thus have no direction (NaN angle)
radians.BearingAngle = (s.Velocity.Length() < 0.001f)
? radians.HeadingAngle
: VectorHelper.GetHeadingAngle(s.Velocity);
//GetAngle(VectorHelper.ToHorizontal(s.Velocity));
radians.GoalAngle = VectorHelper.GetAngle(r.HPositionToGoal);
radians.BearingErrorAngle = VectorHelper.DiffAngle(radians.BearingAngle, radians.GoalAngle);
// Store angles as both radians and degrees for ease-of-use later
r.Radians = radians;
r.Degrees = ToDegrees(radians);
return(r);
}
public HelicopterBase(Game game, TestConfiguration testConfiguration, TerrainCollision collision,
ICameraProvider cameraProvider, BasicEffect effect, SunlightParameters skyParams, HelicopterScenario scenario,
bool playEngineSound, bool isPlayerControlled, bool drawText)
: base(game)
{
if (game == null || cameraProvider == null || effect == null || skyParams == null)
{
throw new ArgumentNullException("", @"One or several of the necessary arguments were null!");
}
_game = game;
_testConfiguration = testConfiguration;
_sensorSpecifications = testConfiguration.Sensors;
_collision = collision;
_flyBySensors = testConfiguration.FlyBySensors;
if (_collision != null)
{
_collision.CollidedWithTerrain += gameTime => Crashed(gameTime);
}
_basicEffect = effect;
_skyParams = skyParams;
_scenario = scenario;
_drawText = drawText;
_cameraProvider = cameraProvider;
IsPlayerControlled = isPlayerControlled;
_playEngineSound = playEngineSound;
_estimatedState = new HeliState();
PIDSetup pidSetup = SimulatorResources.GetPIDSetups()[0];
Autopilot = new Autopilot(_scenario.Task, pidSetup);
Log = new List <HelicopterLogSnapshot>();
}
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
});
}
public override void Update(GameTime gameTime)
{
if (_playEngineSound && _engineSoundInst != null && _engineSoundInst.State != SoundState.Playing)
{
_engineSoundInst.Play();
}
long totalTicks = gameTime.TotalGameTime.Ticks;
JoystickOutput output = GetJoystickInput(totalTicks);
// Invert yaw because we want yaw rotation positive as clockwise seen from above
output.Yaw *= -1;
// Update physics and sensors + state estimators
PhysicalHeliState trueState, observedState, estimatedState, blindEstimatedState;
UpdatePhysicalState(gameTime, output, out trueState);
UpdateStateEstimators(_simulationState, gameTime, output, out estimatedState, out blindEstimatedState, out observedState);
float trueGroundAltitude = GetGroundAltitude(trueState.Position);
float estimatedGroundAltitude = GetGroundAltitude(trueState.Position);
float trueHeightAboveGround = GetHeightAboveGround(trueState.Position);
float gpsinsHeightAboveGround = GetHeightAboveGround(estimatedState.Position);
float rangefinderHeightAboveGround = _sensors.GroundRangeFinder.FlatGroundHeight;
float estimatedHeightAboveGround;
if (!_testConfiguration.UseGPS)
{
throw new NotImplementedException();
}
else if (!_testConfiguration.UseINS)
{
throw new NotImplementedException();
}
if (_testConfiguration.UseGPS && _testConfiguration.UseINS)
{
if (!_testConfiguration.UseRangeFinder)
{
estimatedHeightAboveGround = gpsinsHeightAboveGround;
}
else
{
// The range finder may be out of range (NaN) and typically requires <10 meters.
// In this case we need to fully trust INS/GPS estimate.
// Note GPS is easily out-bested by range finder, so no need to weight
estimatedHeightAboveGround = float.IsNaN(rangefinderHeightAboveGround)
? gpsinsHeightAboveGround
: rangefinderHeightAboveGround;
// : 0.2f*gpsinsHeightAboveGround + 0.8f*rangefinderHeightAboveGround;
}
}
else
{
throw new NotImplementedException();
}
// Override Kalman Filter estimate of altitude by the more accurate range finder.
// However, there is a problem that the estimated map altitude depends on the estimated horizontal position; which is inaccurate.
estimatedState.Position.Y = estimatedHeightAboveGround + estimatedGroundAltitude;
_physicalState = trueState;
_trueState = StateHelper.ToHeliState(trueState, trueHeightAboveGround, Autopilot.CurrentWaypoint, output);
_estimatedState = StateHelper.ToHeliState(estimatedState, estimatedHeightAboveGround, Autopilot.CurrentWaypoint, output);
// _observedState = observedState;
// Calculate current error in estimated state
_estimatedStateError = StateHelper.GetError(_physicalState, StateHelper.ToPhysical(_estimatedState));
// Add current simulation step to log
ForwardRightUp accelerometerData = _sensors.IMU.AccelerationLocal;
Log.Add(new HelicopterLogSnapshot(trueState, observedState, estimatedState, blindEstimatedState, accelerometerData, trueGroundAltitude, gameTime.TotalGameTime));
// Update visual appearances
UpdateEffects();
UpdateSound(output);
// If we have disabled Jitter we must detect collisions ourselves for test scenarios
if (!UseTerrainCollision)
{
if (IsCollidedWithTerrain())
{
if (Crashed != null)
{
Crashed(gameTime);
}
}
}
// Handle crashes, user input etc.. that cause the helicopter to reset
HandleResetting(gameTime);
base.Update(gameTime);
}
/// <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);
}
/// <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));
}
