private static void WriteFRU(XmlWriter w, string nodeName, ForwardRightUp value)
{
w.WriteStartElement(nodeName);
w.WriteAttributeString("Forward", value.Forward.ToString(Localization));
w.WriteAttributeString("Right", value.Right.ToString(Localization));
w.WriteAttributeString("Up", value.Up.ToString(Localization));
w.WriteEndElement();
}
public override void Update(PhysicalHeliState startState, PhysicalHeliState endState, JoystickOutput output, TimeSpan startTime, TimeSpan endTime)
{
_accelerometer.Update(startState, endState, output, startTime, endTime);
_gyroscope.Update(startState, endState, output, startTime, endTime);
// Make sure we use orientation at start of timestep to transform to world, since IMU should be able to calculate accumulated position
// by this world acceleration, and according to my physics simulation the position is calculated before rotation is.
AccelerationWorld = _accelerometer.AccelerationWorld;
AccelerationLocal = new ForwardRightUp(_accelerometer.Forward, _accelerometer.Right, _accelerometer.Up);
AngularRateBody = AngularValues.FromRadians(_gyroscope.Rate);
AngularDeltaBody = AngularValues.FromRadians(_gyroscope.Delta);
PitchRollYaw delta = AngularDeltaBody.Radians;
AccumulatedOrientation =
VectorHelper.AddPitchRollYaw(AccumulatedOrientation, delta.Pitch, delta.Roll, delta.Yaw);
}
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);
}
static ForwardRightUp()
{
Zero = new ForwardRightUp();
}
static ForwardRightUp()
{
Zero = new ForwardRightUp();
}