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); }