/// <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 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 static Waypoint ParseWaypoint(XmlNode waypointNode, float waypointRadius) { // TODO Implement heading angle (also in XML file!) Vector3 position = XMLHelper.ParseVector3(waypointNode.SelectSingleNode("Position")); float heading = 0f; var headingNode = waypointNode.SelectSingleNode("HeadingAngle"); if (headingNode != null) heading = float.Parse(headingNode.InnerText); var type = (WaypointType) Enum.Parse(typeof (WaypointType), waypointNode.SelectSingleNode("Type").InnerText, true); var result = new Waypoint(position, heading, type, waypointRadius); var secondsToWaitNode = waypointNode.SelectSingleNode("SecondsToWait"); if (secondsToWaitNode != null) result.SecondsToWait = float.Parse(secondsToWaitNode.InnerText); return result; }
private static Waypoint ParseWaypoint(XmlNode waypointNode) { var result = new Waypoint( XMLHelper.ParseVector3(waypointNode.SelectSingleNode("Position")), 0, (WaypointType)Enum.Parse(typeof(WaypointType), waypointNode.SelectSingleNode("WaypointType").InnerText, true), float.Parse(waypointNode.SelectSingleNode("Radius").InnerText)); return result; }