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); } }
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 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); } }
/// <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)); }
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); }
/// <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); }
void OnControlBegin(Collider other) { joystickOutput = other.transform.root.GetComponent <JoystickOutput>(); Lever = other.transform.parent.gameObject; joystickOutput = other.GetComponentInParent <JoystickOutput>(); ControlLeverTop = Lever.transform.parent.parent.transform.Find("Relative Center Point").gameObject; ControlleverAnimator = Lever.transform.parent.GetComponent <Animator>(); ControlLeverLastX = ControlleverAnimator.GetFloat("Blend X"); ControlLeverLastZ = ControlleverAnimator.GetFloat("Blend Z"); ControlLeverTop.transform.position = transform.position; RotateWhenPicked = transform.localEulerAngles.y; ConrolStickLever = true; }
public JoystickOutput MoveRelatively(HeliState s, float wantedHVelocityForward, float wantedHVelocityRight, float wantedYawAngleDeg, float wantedHeightAboveGround, long totalTicks, out ControlGoal control) { // Max degrees to tilt the cyclic when accelerating/decelerating const float maxCyclicAngleDeg = 10.0f; Vector2 hVelocityVector = VectorHelper.ToHorizontal(s.Velocity); Vector2 hForwardNorm = Vector2.Normalize(VectorHelper.ToHorizontal(s.Forward)); Vector2 hRightNorm = Vector2.Normalize(VectorHelper.ToHorizontal(s.Right)); float hVelocityForward = VectorHelper.Project(hVelocityVector, hForwardNorm); float hVelocityRight = VectorHelper.Project(hVelocityVector, hRightNorm); // Errors in velocity (derivative of position) float hVelocityForwardError = hVelocityForward - wantedHVelocityForward; float hVelocityRightError = hVelocityRight - wantedHVelocityRight; // Too much velocity should trigger deceleration and vice versa. // Forwards acceleration means pitching nose down, and rightwards acceleration means rolling right. // -1 == max deceleration, +1 == max acceleration float wantedForwardsAcceleration = PIDSetup.ForwardsAccel.ComputeExplicit(0, 0, hVelocityForwardError); float wantedRightwardsAcceleration = PIDSetup.RightwardsAccel.ComputeExplicit(0, 0, hVelocityRightError); // Determine the pitching/rolling angles to achieve wanted acceleration/deceleration in either direction // Invert pitch; negative angle (nose down) to accelerate. float wantedPitchAngleDeg = -wantedForwardsAcceleration * maxCyclicAngleDeg; float wantedRollAngleDeg = wantedRightwardsAcceleration * maxCyclicAngleDeg; // Determine the current errors in pitch and roll // Then adjust the inputs accordingly var moveToTargetInput = new JoystickOutput(); moveToTargetInput.Pitch = Pitch(s.Degrees.PitchAngle, wantedPitchAngleDeg, totalTicks); moveToTargetInput.Roll = Roll(s.Degrees.RollAngle, wantedRollAngleDeg, totalTicks); moveToTargetInput.Yaw = Yaw(s.Degrees.HeadingAngle, wantedYawAngleDeg, totalTicks); moveToTargetInput.Throttle = (wantedHeightAboveGround > 0) ? Throttle(s.HeightAboveGround, wantedHeightAboveGround, totalTicks) : Throttle(s.Position.Y, s.Waypoint.Position.Y, totalTicks); // This is used for debugging and visual feedback control = new ControlGoal { HVelocity = 0,//wantedVelocity, PitchAngle = MathHelper.ToRadians(wantedPitchAngleDeg), RollAngle = MathHelper.ToRadians(wantedRollAngleDeg), HeadingAngle = 0, }; return(moveToTargetInput); }
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="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)); }
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)); }
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); } }
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); } }
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); }
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 Update(SimulationStepResults trueState, long elapsedMillis, long totalElapsedMillis, JoystickOutput currentOutput) { _currentState = trueState; }
public abstract void Update(PhysicalHeliState startState, PhysicalHeliState endState, JoystickOutput output, TimeSpan totalSimulationTime, TimeSpan endTime);
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); } }
public Ship() { js = new JoystickOutput(); js.DirectionChanged += OnDirectionChanged; }
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 }
/// <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); }
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 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)); } }
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 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 static bool Setup() { if (requiresSetup) { requiresSetup = false; JoystickInput deskCtrl, g25Wheel, ps4Ctrl; // Joysticks var dskObj = JoystickInputDevice.Search("Hotas").FirstOrDefault(); deskCtrl = dskObj == null ? default(JoystickInput) : new JoystickInput(dskObj); var ps4Obj = JoystickInputDevice.Search("Wireless Controller").FirstOrDefault(); ps4Ctrl = ps4Obj == null ? default(JoystickInput) : new JoystickInput(ps4Obj); var g25Obj = JoystickInputDevice.Search("G25").FirstOrDefault(); g25Wheel = g25Obj == null ? default(JoystickInput) : new JoystickInput(g25Obj); var vJoy = new JoystickOutput(); // add main controller: if (dskCtlActive) { RawJoysticksIn.Add(deskCtrl); } else if (ps4CtlActive) { RawJoysticksIn.Add(ps4Ctrl); } else { RawJoysticksIn.Add(default(JoystickInput)); } RawJoysticksIn.Add(g25Wheel); RawJoysticksOut.Add(vJoy); // Data source Data = new DataArbiter(); Data.CarChanged += (s, e) => { if (Data.Active.Application == "eurotrucks2") { Drivetrain = new Ets2Drivetrain(); } else { Drivetrain = new GenericDrivetrain(); } // reset all modules Antistall.ResetParameters(); CruiseControl.ResetParameters(); Drivetrain.ResetParameters(); Transmission.ResetParameters(); TractionControl.ResetParameters(); Speedlimiter.ResetParameters(); CarProfile = new Profiles(Data.Active.Application, Data.Telemetry.Car); LoadNextProfile(10000); }; // TODO: Temporary.. Data.AppActive += (s, e) => { CameraHorizon.CameraHackEnabled = Data.Active.Application == "TestDrive2"; }; if (deskCtrl == null && g25Wheel == null && ps4Ctrl == null) { //MessageBox.Show("No controllers found"); return(false); } // Modules Antistall = new Antistall(); ACC = new ACC(); CruiseControl = new CruiseControl(); Drivetrain = new GenericDrivetrain(); Transmission = new Transmission(); TractionControl = new TractionControl(); ProfileSwitcher = new ProfileSwitcher(); Speedlimiter = new Speedlimiter(); LaunchControl = new LaunchControl(); DrivetrainCalibrator = new DrivetrainCalibrator(); TransmissionCalibrator = new TransmissionCalibrator(); LaneAssistance = new LaneAssistance(); VariableSpeedControl = new VariableSpeedTransmission(); CameraHorizon = new CameraHorizon(); // Controls Controls = new ControlChain(); Data.Run(); return(true); } return(false); }
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); }
/// <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); }
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 }
public override void Update(PhysicalHeliState startState, PhysicalHeliState endState, JoystickOutput output, TimeSpan startTime, TimeSpan endTime) { if (!_isInitialized) { _prevStartTime = startTime; _isInitialized = true; } PhysicalHeliState stateToMeasure = startState; // Only update according to max frequency var timeSinceLastUpdate = startTime - _prevStartTime; if (_frequency > 0 && timeSinceLastUpdate < _timeBetweenUpdates) { return; } _prevStartTime = startTime; // Project world acceleration onto the helicopter's orientation and its XYZ sensors // See how we use the original acceleration vector (the cause) directly instead of // deriving it from changes in position over time (the effect) which has // unrealistic delay and would amplify errors AccelerationWorld = stateToMeasure.Acceleration; // TODO Verify these are correct Vector3 accelerationBody = VectorHelper.MapFromWorld(AccelerationWorld, stateToMeasure.Orientation); Right = accelerationBody.X; Up = accelerationBody.Y; Forward = -accelerationBody.Z; if (!IsPerfect) { var noiseRight = _gaussRand.NextGaussian(0, _rmsNoiseXY); var noiseUp = _gaussRand.NextGaussian(0, _rmsNoiseXY); var noiseForward = _gaussRand.NextGaussian(0, _rmsNoiseZ); // Add noise to local axes representing the accelerometer readings Forward += noiseForward; Right += noiseRight; Up += noiseUp; // Add noise to world acceleration vector, by transforming noise in local axes to world axes (with some non-important precision loss) Vector3 accelerationNoiseWorld = VectorHelper.MapToWorld(new Vector3(noiseRight, noiseUp, noiseForward), stateToMeasure.Orientation); AccelerationWorld += accelerationNoiseWorld; } }