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