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