public override void Update(PhysicalHeliState startState, PhysicalHeliState endState, JoystickOutput output, TimeSpan startTime, TimeSpan endTime)
        {
            // Note: The sonic range finder is concerned with measuring at the new position in the timestep,
            // so always use endState here.
            PhysicalHeliState stateToMeasure = endState;
            Vector3 worldDirection = Vector3.Transform(_relativeRangeDirection, stateToMeasure.Orientation);
            worldDirection.Normalize();

            // Line equation, how long must line be to reach ground at y=y0 at given angle?
            // y = ax + b
            // y0 = worldDirection.Y * u + state.Position.Y =>
            // u = (y0-state.Position.Y)/worldDirection.Y
            Vector2 mapPosition = VectorHelper.ToHorizontal(stateToMeasure.Position);
            float y0 = _map.GetAltitude(mapPosition);

            if (stateToMeasure.Position.Y <= y0)
                FlatGroundHeight = stateToMeasure.Position.Y - y0;   // This will only be the case if we can fly through the terrain.. usually this should never happen
            else
            {
                const float maxRange = 10f;
                const float minDelta = 0.001f;    // Search resolution, returns distance when searching further does not improve the distance accuracy more than this
                float rayDistanceToGround = BinarySearchTerrainRayIntersection(0, maxRange, minDelta, stateToMeasure.Position, worldDirection);
                if (float.IsNaN(rayDistanceToGround))
                    FlatGroundHeight = float.NaN;
                else
                {
                    // A vector from the current position to the ground
                    Vector3 rangeVectorToGround = worldDirection*rayDistanceToGround;

                    // Invert vector and take its Y component to give flat ground height
                    FlatGroundHeight = Vector3.Negate(rangeVectorToGround).Y;
                }
            }
        }
        public void Test()
        {
            var simSettings = new SimulationSettings();
            simSettings.RenderMode = RenderModes.Normal;

            var game = new SimulatorGame(simSettings, IntPtr.Zero);
            _collision = new TerrainCollision(game, game);
            _collision.Initialize();
            _collision.TmpBuildScene();

            // Find new position and velocity from a constant acceleration over timestep
            const float dt = 0.017f;

            var a = new Vector3(0, 9.81f, 0);
            var startCondition1 = new TimestepStartingCondition(Vector3.Zero, Vector3.Zero, a,
                                                                Quaternion.Identity, TimeSpan.Zero);
            TimestepStartingCondition startCondition2 = startCondition1;

            var joystickOutput = new JoystickOutput(0.1f, 0.1f, 0, 0.5f);

            for (int i = 0; i < 100; i++)
            {
                TimestepResult jitterResult = ByJitter(startCondition1, joystickOutput,
                                                       startCondition1.StartTime + TimeSpan.FromSeconds(dt), dt);

                TimestepResult physicsResult = ByPhysics(startCondition2, joystickOutput,
                                                         startCondition2.StartTime + TimeSpan.FromSeconds(dt), dt);

                Vector3 dPos = jitterResult.Position - physicsResult.Position;
                Vector3 dVel = jitterResult.Velocity - physicsResult.Velocity;

                if (jitterResult.Orientation != physicsResult.Orientation)
                {
                    float dPitchDeg =
                        MathHelper.ToDegrees(VectorHelper.GetPitchAngle(jitterResult.Orientation) -
                                             VectorHelper.GetPitchAngle(physicsResult.Orientation));

                    float dRollDeg =
                        MathHelper.ToDegrees(VectorHelper.GetRollAngle(jitterResult.Orientation) -
                                             VectorHelper.GetRollAngle(physicsResult.Orientation));

                    float dYawDeg =
                        MathHelper.ToDegrees(VectorHelper.GetHeadingAngle(jitterResult.Orientation) -
                                             VectorHelper.GetHeadingAngle(physicsResult.Orientation));

                    Console.WriteLine("YPR delta " + dPitchDeg + " " + dRollDeg + " " + dYawDeg);
                }

                TimeSpan nextStartTime = physicsResult.EndTime;
                startCondition1 = new TimestepStartingCondition(jitterResult.Position, jitterResult.Velocity,
                                                                a, jitterResult.Orientation, nextStartTime);

                startCondition2 = new TimestepStartingCondition(physicsResult.Position, physicsResult.Velocity,
                                                                a, physicsResult.Orientation, nextStartTime);
            }
        }
        public override void Update(PhysicalHeliState startState, PhysicalHeliState endState, JoystickOutput output, TimeSpan startTime, TimeSpan endTime)
        {
            PhysicalHeliState stateToMeasure = endState;

            // TODO Fill XYZ magnetic readings
            // TODO Simulate sensor lag, inaccuarcy, noise..
            _sampleHistory.Add(stateToMeasure.Axes);

            CurrentMeasuredAxes = IsPerfect ? stateToMeasure.Axes : ComputeMeanAxes();
        }
        public override void Update(PhysicalHeliState startState, PhysicalHeliState endState, JoystickOutput output, TimeSpan totalSimulationTime, TimeSpan endTime)
        {
            PhysicalHeliState stateToMeasure = endState;
            float trueAltitude = stateToMeasure.Position.Y;

            if (IsPerfect)
                Altitude = trueAltitude;
            else
            {
                float simulatedPressure = _pressureProvider.GetSimulatedStaticPressure(trueAltitude);
                Altitude = StaticPressureHelper.GetAltitude(simulatedPressure, SeaLevelStaticPressure);
            }
        }
        /// <summary>Calculates the input and external forces.</summary>
        /// <returns>The new orientation and the total acceleration for this orientation in this timestep.</returns>
        public SimulationStepResults PerformTimestep(PhysicalHeliState prev, JoystickOutput output, TimeSpan stepDuration,
                                                     TimeSpan stepEndTime)
        {
            if (!_isInitialized)
            {
            //                startCondition.Acceleration = CalculateAcceleration(startCondition.Orientation, startCondition.Velocity, input);
                _prevTimestepResult = new TimestepResult(prev.Position, prev.Velocity, prev.Orientation,
                                                         stepEndTime - stepDuration);

                _isInitialized = true;
            }

            // If the number of substeps is 0 then only the state at the end of the timestep will be calculated.
            // If the number is greater than 1, then the timestep will 1 then a substep will be calculated in the middle of the timestep.
            const int substeps = 0;
            if (substeps < 0)
                throw new Exception("The number of substeps is invalid.");

            TimeSpan substepDuration = stepDuration.Divide(1 + substeps);

            Vector3 initialAcceleration = CalculateAcceleration(prev.Orientation, prev.Velocity, output);
            var initialState = new TimestepStartingCondition(_prevTimestepResult, initialAcceleration);
                //_prevTimestepResult.Result;
            //            var substepResults = new List<SubstepResults> {initialState};

            // We always need to calculate at least the timestep itself, plus any optional substeps.
            // Substeps are used to provide sensors with a higher frequency of data than the simulator is capable of rendering real-time.
            //            const int stepsToCalculate = substeps + 1;
            //            SubstepResults prevSubstep = initialState;
            //            for (int i = 0; i < stepsToCalculate; i++)
            //            {
            //                prevSubstep.Acceleration = CalculateAcceleration(prevSubstep.Orientation, prevSubstep.Velocity, input);
            //                SubstepResults r = SimulateStep(prevSubstep, prevSubstep.Acceleration, input, substepDuration, stepEndTime);
            //
            //                substepResults.Add(r);
            //                prevSubstep = r;
            //            }

            TimestepResult result = SimulateStep(initialState, output, substepDuration, stepEndTime);
            //new SimulationStepResults(stepDuration, substepDuration, substepResults);
            _prevTimestepResult = result;

            //            DebugInformation.Time1 = stepEndTime;
            //            DebugInformation.Q1 = result.Orientation;
            //
            //            DebugInformation.Vectors["Pos"] = result.Position;
            //            DebugInformation.Vectors["Vel"] = result.Velocity;
            //            DebugInformation.Vectors["Acc"] = initialAcceleration;

            return new SimulationStepResults(initialState, result, stepEndTime - stepDuration, stepEndTime);
        }
        public override void Update(PhysicalHeliState startState, PhysicalHeliState endState, JoystickOutput output, TimeSpan startTime, TimeSpan endTime)
        {
            var dt = (float) ((endTime - startTime).TotalSeconds);
            _rate.Pitch = output.Pitch*PhysicsConstants.MaxPitchRate;
            _rate.Roll = output.Roll*PhysicsConstants.MaxRollRate;
            _rate.Yaw = output.Yaw*PhysicsConstants.MaxYawRate;

            // Note it is important to explicitly calculate the delta and not reuse the _rate values multiplied by dt
            // See http://stackoverflow.com/questions/2491161/why-differs-floating-point-precision-in-c-when-separated-by-parantheses-and-when
            // and QuaternionPrecisionTest.Test() for more information about precision.
            _delta.Pitch = output.Pitch*PhysicsConstants.MaxPitchRate*dt;
            _delta.Roll = output.Roll*PhysicsConstants.MaxRollRate*dt;
            _delta.Yaw = output.Yaw*PhysicsConstants.MaxYawRate*dt;

            // TODO Handle IsPerfect == false
        }
        /// <summary>
        /// TODO Temporary. We might later distinguish between physical state and navigation state.
        /// </summary>
        /// <param name="s"></param>
        /// <param name="heightAboveGround"></param>
        /// <param name="waypoint"></param>
        /// <param name="output"></param>
        /// <returns></returns>
        public static HeliState ToHeliState(PhysicalHeliState s, float heightAboveGround, Waypoint waypoint, JoystickOutput output)
        {
            // Update state
            var r = new HeliState
                        {
                            HeightAboveGround = heightAboveGround,
                            Orientation = s.Orientation,
                            Forward = s.Axes.Forward,
                            Up = s.Axes.Up,
                            Right = s.Axes.Right,
                            Output = output,
                            Position = s.Position,
                            Velocity = s.Velocity,
                            Acceleration = s.Acceleration,
                            Waypoint = waypoint,
                            HPositionToGoal = VectorHelper.ToHorizontal(waypoint.Position - s.Position),
                        };

            // Update angles from current state
            var radians = new Angles
                              {
                                  PitchAngle = VectorHelper.GetPitchAngle(s.Orientation),
                                  RollAngle = VectorHelper.GetRollAngle(s.Orientation),
                                  HeadingAngle = VectorHelper.GetHeadingAngle(s.Orientation),
                              };

            // Velocity can be zero, and thus have no direction (NaN angle)
            radians.BearingAngle = (s.Velocity.Length() < 0.001f)
                                       ? radians.HeadingAngle
                                       : VectorHelper.GetHeadingAngle(s.Velocity);
                //GetAngle(VectorHelper.ToHorizontal(s.Velocity));

            radians.GoalAngle = VectorHelper.GetAngle(r.HPositionToGoal);
            radians.BearingErrorAngle = VectorHelper.DiffAngle(radians.BearingAngle, radians.GoalAngle);

            // Store angles as both radians and degrees for ease-of-use later
            r.Radians = radians;
            r.Degrees = ToDegrees(radians);
            return r;
        }
        private TimestepResult SimulateStep(TimestepStartingCondition startCondition, JoystickOutput output,
                                                   TimeSpan stepDuration, TimeSpan stepEndTime)
        {
            // Find new position and velocity from a constant acceleration over timestep
            var dt = (float) stepDuration.TotalSeconds;

            //            TimestepResult result;
            if (_useTerrainCollision)
            {
                // Note we override gravity here because the gravity acceleration is already accounted for in startCondition.Acceleration vector
                _collision.SetGravity(Vector3.Zero);

                float heliMass = _collision.HelicopterBody.Mass;
                _collision.HelicopterBody.Position = Conversion.ToJitterVector(startCondition.Position);
                _collision.HelicopterBody.LinearVelocity = Conversion.ToJitterVector(startCondition.Velocity);
                _collision.HelicopterBody.AddForce(Conversion.ToJitterVector(heliMass * startCondition.Acceleration));

                var localAngVelocity = new Vector3(
                    output.Pitch * PhysicsConstants.MaxPitchRate,
                    output.Yaw * PhysicsConstants.MaxYawRate,
                    -output.Roll * PhysicsConstants.MaxRollRate);

                var worldAngVelocity = VectorHelper.MapFromWorld(localAngVelocity, startCondition.Orientation);

                _collision.HelicopterBody.AngularVelocity = Conversion.ToJitterVector(worldAngVelocity);

                // Simulate physics
            //                Vector3 preForward = Vector3.Transform(Vector3.Forward, startCondition.Orientation);
                _collision.World.Step(dt, false);

                // TODO Testing with Jitter Physics
                return new TimestepResult(
                    Conversion.ToXNAVector(_collision.HelicopterBody.Position),
                    Conversion.ToXNAVector(_collision.HelicopterBody.LinearVelocity),
                    Quaternion.CreateFromRotationMatrix(Conversion.ToXNAMatrix(_collision.HelicopterBody.Orientation)),
                    stepEndTime);

            //                Vector3 postForward = Vector3.Transform(Vector3.Forward, result.Orientation);
            }
            else
            {
                Vector3 newPosition;
                Vector3 newVelocity;
                ClassicalMechanics.ConstantAcceleration(
                    dt,
                    startCondition.Position,
                    startCondition.Velocity,
                    startCondition.Acceleration,
                    out newPosition, out newVelocity);

                // TODO Add wind
                // Rotate helicopter by joystick input after moving it
                //            Vector3 airVelocity = -startCondition.Velocity + Vector3.Zero;

                // TODO Re-apply rotation by air friction as soon as the Kalman Filter works fine without it
                Quaternion newOrientation, afterJoystick;
                RotateByJoystickInput(startCondition.Orientation, dt, output, out afterJoystick);
                //            RotateByAirFriction(afterJoystick, airVelocity, dt, out newOrientation);
                newOrientation = afterJoystick;
                return new TimestepResult(newPosition, newVelocity, newOrientation, stepEndTime);

            }
        }
        /// <summary>
        ///   Rotate helicopter entirely by joystick input (major simplification of physics).
        /// </summary>
        /// <param name="currentOrientation"></param>
        /// <param name="dt"></param>
        /// <param name="output"></param>
        /// <param name="newOrientation"></param>
        private static void RotateByJoystickInput(Quaternion currentOrientation, float dt, JoystickOutput output,
                                                  out Quaternion newOrientation)
        {
            // Move helicopter from previous state according to user/autopilot input
            // Change roll, pitch and yaw according to input
            // Invert yaw, so positive is clockwise
            float deltaRoll = output.Roll*PhysicsConstants.MaxRollRate*dt;
            float deltaPitch = output.Pitch*PhysicsConstants.MaxPitchRate*dt;
            float deltaYaw = output.Yaw*PhysicsConstants.MaxYawRate*dt;

            // Add deltas of pitch, roll and yaw to the rotation matrix
            newOrientation = VectorHelper.AddPitchRollYaw(currentOrientation, deltaPitch, deltaRoll, deltaYaw);

            // Get axis vectors for the new orientation
            //            newAxes = VectorHelper.GetAxes(newOrientation);
        }
        private TimestepResult ByJitter(TimestepStartingCondition startCondition, JoystickOutput output,
                                        TimeSpan stepEndTime, float dt)
        {
            // Note we override gravity here because the gravity acceleration is already accounted for in startCondition.Acceleration vector
            _collision.SetGravity(Vector3.Zero);

            float heliMass = _collision.HelicopterBody.Mass;
            _collision.HelicopterBody.Position = Conversion.ToJitterVector(startCondition.Position);
            _collision.HelicopterBody.LinearVelocity = Conversion.ToJitterVector(startCondition.Velocity);
            _collision.HelicopterBody.AddForce(Conversion.ToJitterVector(heliMass*startCondition.Acceleration));

            var localAngVelocity = new Vector3(
                output.Pitch*PhysicsConstants.MaxPitchRate,
                output.Yaw*PhysicsConstants.MaxYawRate,
                -output.Roll*PhysicsConstants.MaxRollRate);

            Vector3 worldAngVelocity = VectorHelper.MapFromWorld(localAngVelocity, startCondition.Orientation);

            _collision.HelicopterBody.AngularVelocity = Conversion.ToJitterVector(worldAngVelocity);

            // Simulate physics
            //                Vector3 preForward = Vector3.Transform(Vector3.Forward, startCondition.Orientation);
            _collision.World.Step(dt, false);

            // TODO Testing with Jitter Physics
            return new TimestepResult(
                Conversion.ToXNAVector(_collision.HelicopterBody.Position),
                Conversion.ToXNAVector(_collision.HelicopterBody.LinearVelocity),
                Quaternion.CreateFromRotationMatrix(Conversion.ToXNAMatrix(_collision.HelicopterBody.Orientation)),
                stepEndTime);

            //                Vector3 postForward = Vector3.Transform(Vector3.Forward, result.Orientation);
        }
        private TimestepResult ByPhysics(TimestepStartingCondition startCondition, JoystickOutput output,
                                         TimeSpan stepEndTime, float dt)
        {
            Vector3 newPosition;
            Vector3 newVelocity;
            ClassicalMechanics.ConstantAcceleration(
                dt,
                startCondition.Position,
                startCondition.Velocity,
                startCondition.Acceleration,
                out newPosition, out newVelocity);

            Quaternion newOrientation, afterJoystick;
            RotateByJoystickInput(startCondition.Orientation, dt, output, out afterJoystick);
            newOrientation = afterJoystick;
            return new TimestepResult(newPosition, newVelocity, newOrientation, stepEndTime);
        }
        private static Vector3 CalculateAcceleration(Quaternion orientation, Vector3 velocity, JoystickOutput output)
        {
            Axes prevAxes = VectorHelper.GetAxes(orientation);

            Vector3 liftDir = prevAxes.Up;
            float liftForce = output.Throttle*MaxThrust; // 100% throttle => max thrust, simple but works OK
            Vector3 rotorLift = liftForce*liftDir;
            var wind = new Vector3(0, 0, 0); // TODO Use real winds here..
            Vector3 drag = CalculateDrag(velocity, prevAxes);
            Vector3 totalForces = drag + wind + rotorLift;
            Vector3 constantTimestepAcceleration = PhysicsConstants.Gravity + totalForces/Mass;

            return constantTimestepAcceleration;
        }
        public void Update(SimulationStepResults trueState, long elapsedMillis, long totalElapsedMillis,
                           JoystickOutput currentOutput)
        {
            #if XBOX
            // TODO Xbox equivalent or fix the GPSINSFilter dependency on Iridium Math.Net
            _isInitialized = true;
            #else

            // TODO If sensors are not ready we cannot produce an estimated state
            // However, currently the helicopter starts mid-air so we have no time to wait for it to initialize.. so this is cheating.
            // When the helicopter starts on the ground we can properly implement this process.
            if (!_isInitialized)
            {
                if (!_sensors.Ready) return;

                _estimated.Position = _initialState.Position;
                _estimated.Velocity = _initialState.Velocity;
                _estimated.Acceleration = _initialState.Acceleration;
                _estimated.Orientation = _initialState.Orientation;
                _prevGPSPos = _sensors.GPS.Output.Position;

                _gpsins = new GPSINSFilter(
                    TimeSpan.FromMilliseconds(totalElapsedMillis - elapsedMillis),
                    _initialState.Position,
                    _initialState.Velocity,
                    _initialState.Orientation,
                    _sensors.Specifications);

                _isInitialized = true;
            }

            TimeSpan totalTime = TimeSpan.FromMilliseconds(totalElapsedMillis);

            // Setup observations
            var observations = new GPSINSObservation();
            observations.Time = totalTime;
            observations.RangeFinderHeightOverGround = _sensors.GroundRangeFinder.FlatGroundHeight; // TODO Update frequency? Noise?

            if (_sensors.GPS.IsUpdated)
            {
                observations.GotGPSUpdate = true;
                observations.GPSPosition = _sensors.GPS.Output.Position;
                observations.GPSHVelocity = _sensors.GPS.Output.Velocity;
            }

            // Setup input to filter's internal model
            var input = new GPSINSInput
                            {
                                AccelerationWorld = _sensors.IMU.AccelerationWorld,
                                Orientation = _sensors.IMU.AccumulatedOrientation,
            //                                PitchDelta = _sensors.IMU.AngularDeltaBody.Radians.Pitch,
            //                                RollDelta = _sensors.IMU.AngularDeltaBody.Radians.Roll,
            //                                YawDelta = _sensors.IMU.AngularDeltaBody.Radians.Yaw,
            //                                PitchRate = _sensors.IMU.Output.AngularRate.Radians.Pitch,
            //                                RollRate = _sensors.IMU.Output.AngularRate.Radians.Roll,
            //                                YawRate = _sensors.IMU.Output.AngularRate.Radians.Yaw,
                            };

            // Estimate
            StepOutput<GPSINSOutput> gpsinsEstimate = _gpsins.Filter(observations, input);

            //            Vector3 deltaPosition = gpsinsEstimate.PostX.Position - _currentEstimate.Position;
            //            if (deltaPosition.Length() > 40)
            //                deltaPosition = deltaPosition;

            //            var trueState = CheatingTrueState.Result;
            GPSINSOutput observed = gpsinsEstimate.Z;
            GPSINSOutput estimated = gpsinsEstimate.PostX;
            GPSINSOutput blindEstimated = gpsinsEstimate.X;

            _currentEstimate = new PhysicalHeliState(estimated.Orientation, estimated.Position, estimated.Velocity,
                                                     input.AccelerationWorld);

            _currentBlindEstimate = new PhysicalHeliState(blindEstimated.Orientation, blindEstimated.Position, blindEstimated.Velocity,
                                                     input.AccelerationWorld);

            _currentObservation = new PhysicalHeliState(observed.Orientation, observed.Position, observed.Velocity,
                                                        Vector3.Zero);
            #endif
        }
 private void UpdateSound(JoystickOutput output)
 {
     if (_playEngineSound)
     {
         // Volume from 50-100% depending on throttle
         _engineSoundInst.Pitch = MyMathHelper.Lerp(output.Throttle, 0, 1, -0.5f, 0.5f);
     }
 }
        private void UpdatePhysicalState(GameTime gameTime, JoystickOutput output, out PhysicalHeliState trueState)
        {
            _simulationState = _physics.PerformTimestep(_physicalState, output, gameTime.ElapsedGameTime,
                                                        gameTime.TotalGameTime);

            TimestepResult final = _simulationState.Result;

            // We need to use the second last simulation substep to obtain the current acceleration used
            // because the entire substep has constant acceleration and it makes no sense
            // to use the acceleration calculated for the state after the timestep because no animation will occur after it.
            // TODO Support substeps
            Vector3 currentAcceleration = _simulationState.StartingCondition.Acceleration;

            trueState = new PhysicalHeliState(final.Orientation, final.Position, final.Velocity, currentAcceleration);
        }
        /// <summary>
        /// 
        /// </summary>
        /// <param name="userInput"></param>
        /// <param name="s"></param>
        /// <param name="ticks">TimeSpan ticks (100ns).</param>
        /// <param name="control"></param>
        /// <returns></returns>
        public JoystickOutput GetAssistedOutput(JoystickOutput userInput, HeliState s, long ticks,
                                                out ControlGoal control)
        {
            Navigation = NavigationState.AssistedAutopilot;
            JoystickOutput result = GetOutput_AssistedAutopilot(userInput, s, ticks, out control);
            ProcessNavigation(s, ticks);

            return result;
        }
 public abstract void Update(PhysicalHeliState startState, PhysicalHeliState endState, JoystickOutput output, TimeSpan totalSimulationTime, TimeSpan endTime);
 public void Update(SimulationStepResults trueState, long elapsedMillis, long totalElapsedMillis,
                    JoystickOutput currentOutput)
 {
     _currentState = trueState;
 }
        /// <summary>
        /// 
        /// </summary>
        /// <param name="userInput"></param>
        /// <param name="s"></param>
        /// <param name="totalTicks">TimeSpan ticks (100ns).</param>
        /// <param name="control"></param>
        /// <returns></returns>
        private JoystickOutput GetOutput_AssistedAutopilot(JoystickOutput userInput, HeliState s, long totalTicks,
                                                           out ControlGoal control)
        {
            // Command the output controller based on the user input.
            // Joystick commands:
            // Forward/backward - Move forwards/backwards
            // Right/left - Move rightwards/leftwards
            // Throttle  - Set the height above the ground to hold
            float heightAboveGroundToHold = MyMathHelper.Lerp(userInput.Throttle, 0, 1, 1, 10);
            float forwardsVelocity = MyMathHelper.Lerp(-userInput.Pitch, -1, 1, -10, 10);
            float rightwardsVelocity = MyMathHelper.Lerp(userInput.Roll, -1, 1, -10, 10);
            float headingDeg = s.Degrees.HeadingAngle - 15*userInput.Yaw;

            return Output.MoveRelatively(s, forwardsVelocity, rightwardsVelocity, headingDeg, heightAboveGroundToHold,
                                         totalTicks, out control);
        }
Ejemplo n.º 20
0
        public void Update(PhysicalHeliState tartState, PhysicalHeliState endState, JoystickOutput output, TimeSpan startTime, TimeSpan endTime)
        {
            if (!_isInitialized)
            {
                _initStartTime = startTime;
                _isInitialized = true;
            }

            PhysicalHeliState stateToMeasure = endState;

            // Only update GPS readout as often as SecondsPerUpdate says
            if (_useUpdateFrequency && startTime < _prevUpdate + TimeSpan.FromSeconds(SecondsPerUpdate))
                return;

            // It takes some time for a GPS to connect and produce positional values
            if (!Ready && startTime >= _initStartTime + _setupDuration)
                Ready = true;

            // Update the GPS information if the device is ready
            if (Ready)
            {
                _prevUpdate = startTime;

                Vector3 position = stateToMeasure.Position;
                Vector3 velocity = stateToMeasure.Velocity;

                if (!_isPerfect)
                {
                    position += VectorHelper.GaussRandom3(_sensorSpecifications.GPSPositionStdDev);
                    velocity += VectorHelper.GaussRandom3(_sensorSpecifications.GPSVelocityStdDev);
                }

                Output = new GPSOutput(position, velocity);
            }
        }
        private void UpdateStateEstimators(SimulationStepResults simulationState, GameTime gameTime, JoystickOutput output,
                                           out PhysicalHeliState estimatedState,
                                           out PhysicalHeliState blindEstimatedState,
                                           out PhysicalHeliState observedState)
        {
            var dtMillis = (long)gameTime.ElapsedGameTime.TotalMilliseconds;
            var totalMillis = (long)gameTime.TotalGameTime.TotalMilliseconds;

            //            TimeSpan totalSimulationTime = gameTime.TotalGameTime;

            // Update sensors prior to using the state provider, 
            // because they are typically dependent on the state of the sensors.
            Sensors.Update(_simulationState, output);

            // Update states
            ((SensorEstimatedState)_estimatedStateProvider).CheatingTrueState = simulationState; // TODO Temp cheating
            _estimatedStateProvider.Update(simulationState, dtMillis, totalMillis, output);

            // TODO Separate physical and navigational states
            _estimatedStateProvider.GetState(out estimatedState, out observedState, out blindEstimatedState);
        }
Ejemplo n.º 22
0
        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);
        }
        private JoystickOutput GetUserInput()
        {
            float pitch = 0, roll = 0, yaw = 0, throttle = 0;
#if XBOX
            Vector2 leftTS = GamePad.GetState(PlayerIndex.One).ThumbSticks.Left;
            Vector2 rightTS = GamePad.GetState(PlayerIndex.One).ThumbSticks.Right;
            pitch = -rightTS.Y;
            roll = rightTS.X;
            yaw = leftTS.X;
            throttle = MyMathHelper.Lerp(leftTS.Y, -1, 1, 0, 1);
#else
            if (_joystickSystem == null)
                throw new Exception("No joystick connected? Either \n1) Connect a properly configured joystick and restart. \n2) Modify Scenarios.xml to run an autopilot scenario that does not require joystick input.");

            _joystickSystem.Update();

            pitch = _joystickSystem.GetAxisValue(JoystickAxisAction.Pitch);
            roll = _joystickSystem.GetAxisValue(JoystickAxisAction.Roll);
            yaw = _joystickSystem.GetAxisValue(JoystickAxisAction.Yaw);
            throttle = _joystickSystem.GetAxisValue(JoystickAxisAction.Throttle);

            throttle = 0.5f + throttle/2; // -1 to +1 => 0 to +1

            // Try square exponential response (i^2) and a linear reduction of max
            // TODO Define response in terms of max angular rotation per second
            const float cyclicResponse = 0.7f; // 0 means no response, 1 means full response
            const float rudderResponse = 0.7f;

            roll = Math.Sign(roll)*cyclicResponse*roll*roll; 
            pitch = Math.Sign(pitch)*cyclicResponse*pitch*pitch;
            yaw = Math.Sign(yaw)*rudderResponse*yaw*yaw;
#endif


            // Dead zone to avoid jittery behavior at idle stick
            if (Math.Abs(roll) < 0.05) roll = 0;
            if (Math.Abs(pitch) < 0.05) pitch = 0;
            if (Math.Abs(yaw) < 0.05) yaw = 0;

            var result = new JoystickOutput {Roll = roll, Pitch = pitch, Yaw = yaw, Throttle = throttle};
            return result;
        }
        public void Update(SimulationStepResults step, JoystickOutput output)
        {
            // TODO Use substeps in physics simulation when sensors require higher frequency than the main-loop runs at
            // Currently we simply use the state at the start of the timestep to feed into the sensors, so they read the state
            // that was before any motion took place in that timestep.
            // Later we may need to update sensors multiple times for each timestep.
            TimestepStartingCondition start = step.StartingCondition;
            TimestepResult end = step.Result;

            var startPhysicalState = new PhysicalHeliState(start.Orientation, start.Position, start.Velocity,
                                                      start.Acceleration);

            var endPhysicalState = new PhysicalHeliState(end.Orientation, end.Position, end.Velocity,
                                                      new Vector3(float.NaN));

            // Update all sensors
            foreach (ISensor sensor in _sensors)
                sensor.Update(startPhysicalState, endPhysicalState, output, step.StartTime, step.EndTime);
        }