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();
}
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>();
}
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);
}
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
};
}
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="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="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="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);
}
/// <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;
}
/// <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 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);
}
