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