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 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 IList<HelicopterScenario> ParseHelicopterScenario(XmlNode scenarioNode)
{
var result = new List<HelicopterScenario>();
XmlNodeList helicopterNodes = scenarioNode.SelectNodes("Helicopter");
foreach (XmlNode heliNode in helicopterNodes)
{
var heliScenario = new HelicopterScenario();
heliScenario.Task = ParseTask(heliNode);
XmlNode playerControlledNode = heliNode.SelectSingleNode("PlayerControlled");
heliScenario.PlayerControlled = (playerControlledNode != null) ? bool.Parse(playerControlledNode.InnerText) : false;
XmlNode playEngineSoundNode = heliNode.SelectSingleNode("EngineSound");
heliScenario.EngineSound = (playEngineSoundNode != null) ? bool.Parse(playEngineSoundNode.InnerText) : false;
XmlNode startPosNode = heliNode.SelectSingleNode("StartPosition");
heliScenario.StartPosition = XMLHelper.ParseVector3(startPosNode);
var assistedAutopilotNode = heliNode.SelectSingleNode("AssistedAutopilot");
heliScenario.AssistedAutopilot = (assistedAutopilotNode != null)
? bool.Parse(assistedAutopilotNode.InnerText)
: false;
result.Add(heliScenario);
}
return result;
}