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 }