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);
        }
Esempio n. 4
0
 static ForwardRightUp()
 {
     Zero = new ForwardRightUp();
 }
 static ForwardRightUp()
 {
     Zero = new ForwardRightUp();
 }