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