public SensorModel(SensorSpecifications sensorSpecifications, NavigationMap map, Vector3 startPosition, Quaternion startOrientation)
{
_sensorSpecifications = sensorSpecifications;
Console.WriteLine("Using sensors:\n" + sensorSpecifications);
// Instantiate sensors and populate properties and sensor list
const bool isPerfect = false;
_sensors = new List<ISensor>
{
(GPS = new GPS(sensorSpecifications, isPerfect, true)),
(IMU = new IMU(sensorSpecifications, startOrientation, isPerfect)),
(Magnetometer = new Magnetometer3Axis(sensorSpecifications, isPerfect)),
(GroundRangeFinder = new SonicRangeFinder(sensorSpecifications, isPerfect, map)),
(PressureGauge = new StaticPressureGauge(sensorSpecifications, isPerfect)),
};
}
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 SonicRangeFinder(SensorSpecifications sensorSpecifications, bool isPerfect, NavigationMap map)
: base(isPerfect)
{
_map = map;
_relativeRangeDirection = Vector3.Down;
}
private void InitHelicopters(SunlightParameters skyParameters, NavigationMap heightmap)
{
// TODO 1 or more helicopters?
if (_scenario.HelicopterScenarios == null || _scenario.HelicopterScenarios.Count == 0)
throw new Exception("Could not find helicopter scenario.");
HelicopterScenario heliScenario = _scenario.HelicopterScenarios[0];
var startVelocity = new Vector3(); //new Vector3(-0.5f, 0, -1.0f)
var startState = new PhysicalHeliState(
Quaternion.Identity,
heliScenario.StartPosition,
startVelocity,
Vector3.Zero);
_helicopter = new HelicopterBase(this, _testConfiguration, _terrainCollision, this, _basicEffect, skyParameters, heliScenario,
heliScenario.EngineSound, heliScenario.PlayerControlled, DrawText);
// If testing then we will continue to the next waypoint as soon as the true position is within
// the radius of the waypoint. If not testing (non-simulated scenarios), we typically want to
// progress only if the estimated state is belived to be within the radius.
_helicopter.Autopilot.IsTestMode = IsTestMode;
_helicopter.Autopilot.IsTruePositionWithinRadius = false;
_helicopter.Autopilot.MaxHVelocity = _testMaxHVelocityIter.Current;
// Optionally a PID setup may have been provided to use with the helicopters.
// If not a default setup is used.
if (_initialPIDSetup.HasValue)
SetPIDSetup(_initialPIDSetup.Value);
// InitFormationHelicopters(startPos, skyParameters);
_helicopter.Autopilot.Map = heightmap;
}
private NavigationMap UpdateTerrain(Scenario scenario)
{
if (scenario.SceneElements.Contains("Terrain"))
{
// Build terrain and derive a heightmap from it
TerrainInfo ti = scenario.TerrainInfo;
_terrain.BuildTerrain(ti.Width, ti.MinHeight, ti.MaxHeight);
_heightmap = new NavigationMap(HeightmapToValues(_terrain.Mesh.Heightmap));
}
else
{
// Assume flat ground if not using terrain
_heightmap = new NavigationMap(new [,]
{
{0f, 0f,},
{0f, 0f,},
});
}
return _heightmap;
}
