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();
}
private void UpdatePhysicalState(GameTime gameTime, JoystickOutput output, out PhysicalHeliState trueState)
{
_simulationState = _physics.PerformTimestep(_physicalState, output, gameTime.ElapsedGameTime,
gameTime.TotalGameTime);
TimestepResult final = _simulationState.Result;
// We need to use the second last simulation substep to obtain the current acceleration used
// because the entire substep has constant acceleration and it makes no sense
// to use the acceleration calculated for the state after the timestep because no animation will occur after it.
// TODO Support substeps
Vector3 currentAcceleration = _simulationState.StartingCondition.Acceleration;
trueState = new PhysicalHeliState(final.Orientation, final.Position, final.Velocity, currentAcceleration);
}
private void UpdateStateEstimators(SimulationStepResults simulationState, GameTime gameTime, JoystickOutput output,
out PhysicalHeliState estimatedState,
out PhysicalHeliState blindEstimatedState,
out PhysicalHeliState observedState)
{
var dtMillis = (long)gameTime.ElapsedGameTime.TotalMilliseconds;
var totalMillis = (long)gameTime.TotalGameTime.TotalMilliseconds;
// TimeSpan totalSimulationTime = gameTime.TotalGameTime;
// Update sensors prior to using the state provider,
// because they are typically dependent on the state of the sensors.
Sensors.Update(_simulationState, output);
// Update states
((SensorEstimatedState)_estimatedStateProvider).CheatingTrueState = simulationState; // TODO Temp cheating
_estimatedStateProvider.Update(simulationState, dtMillis, totalMillis, output);
// TODO Separate physical and navigational states
_estimatedStateProvider.GetState(out estimatedState, out observedState, out blindEstimatedState);
}
public void Update(SimulationStepResults step, JoystickOutput output)
{
// TODO Use substeps in physics simulation when sensors require higher frequency than the main-loop runs at
// Currently we simply use the state at the start of the timestep to feed into the sensors, so they read the state
// that was before any motion took place in that timestep.
// Later we may need to update sensors multiple times for each timestep.
TimestepStartingCondition start = step.StartingCondition;
TimestepResult end = step.Result;
var startPhysicalState = new PhysicalHeliState(start.Orientation, start.Position, start.Velocity,
start.Acceleration);
var endPhysicalState = new PhysicalHeliState(end.Orientation, end.Position, end.Velocity,
new Vector3(float.NaN));
// Update all sensors
foreach (ISensor sensor in _sensors)
{
sensor.Update(startPhysicalState, endPhysicalState, output, step.StartTime, step.EndTime);
}
}
public void Update(SimulationStepResults trueState, long elapsedMillis, long totalElapsedMillis,
JoystickOutput currentOutput)
{
_currentState = trueState;
}
public void Update(SimulationStepResults trueState, long elapsedMillis, long totalElapsedMillis,
JoystickOutput currentOutput)
{
#if XBOX
// TODO Xbox equivalent or fix the GPSINSFilter dependency on Iridium Math.Net
_isInitialized = true;
#else
// TODO If sensors are not ready we cannot produce an estimated state
// However, currently the helicopter starts mid-air so we have no time to wait for it to initialize.. so this is cheating.
// When the helicopter starts on the ground we can properly implement this process.
if (!_isInitialized)
{
if (!_sensors.Ready)
{
return;
}
_estimated.Position = _initialState.Position;
_estimated.Velocity = _initialState.Velocity;
_estimated.Acceleration = _initialState.Acceleration;
_estimated.Orientation = _initialState.Orientation;
_prevGPSPos = _sensors.GPS.Output.Position;
_gpsins = new GPSINSFilter(
TimeSpan.FromMilliseconds(totalElapsedMillis - elapsedMillis),
_initialState.Position,
_initialState.Velocity,
_initialState.Orientation,
_sensors.Specifications);
_isInitialized = true;
}
TimeSpan totalTime = TimeSpan.FromMilliseconds(totalElapsedMillis);
// Setup observations
var observations = new GPSINSObservation();
observations.Time = totalTime;
observations.RangeFinderHeightOverGround = _sensors.GroundRangeFinder.FlatGroundHeight; // TODO Update frequency? Noise?
if (_sensors.GPS.IsUpdated)
{
observations.GotGPSUpdate = true;
observations.GPSPosition = _sensors.GPS.Output.Position;
observations.GPSHVelocity = _sensors.GPS.Output.Velocity;
}
// Setup input to filter's internal model
var input = new GPSINSInput
{
AccelerationWorld = _sensors.IMU.AccelerationWorld,
Orientation = _sensors.IMU.AccumulatedOrientation,
// PitchDelta = _sensors.IMU.AngularDeltaBody.Radians.Pitch,
// RollDelta = _sensors.IMU.AngularDeltaBody.Radians.Roll,
// YawDelta = _sensors.IMU.AngularDeltaBody.Radians.Yaw,
// PitchRate = _sensors.IMU.Output.AngularRate.Radians.Pitch,
// RollRate = _sensors.IMU.Output.AngularRate.Radians.Roll,
// YawRate = _sensors.IMU.Output.AngularRate.Radians.Yaw,
};
// Estimate
StepOutput <GPSINSOutput> gpsinsEstimate = _gpsins.Filter(observations, input);
// Vector3 deltaPosition = gpsinsEstimate.PostX.Position - _currentEstimate.Position;
// if (deltaPosition.Length() > 40)
// deltaPosition = deltaPosition;
// var trueState = CheatingTrueState.Result;
GPSINSOutput observed = gpsinsEstimate.Z;
GPSINSOutput estimated = gpsinsEstimate.PostX;
GPSINSOutput blindEstimated = gpsinsEstimate.X;
_currentEstimate = new PhysicalHeliState(estimated.Orientation, estimated.Position, estimated.Velocity,
input.AccelerationWorld);
_currentBlindEstimate = new PhysicalHeliState(blindEstimated.Orientation, blindEstimated.Position, blindEstimated.Velocity,
input.AccelerationWorld);
_currentObservation = new PhysicalHeliState(observed.Orientation, observed.Position, observed.Velocity,
Vector3.Zero);
#endif
}
