private static void RotateByAirFriction(Quaternion currentOrientation, Vector3 airVelocity, float dt, out Quaternion newOrientation) { float headingAngle = VectorHelper.GetHeadingAngle(currentOrientation); float airflowHeadingAngle = VectorHelper.GetHeadingAngle(-airVelocity); float relativeAirflowAngle = airflowHeadingAngle - headingAngle; // Note: Simplificiation, we assume helicopter is nearly level. // This way any vertical wind speeds won't cause the helicopter to rotate. float horizontalWindMagnitudeFactor = VectorHelper.ToHorizontal3D(airVelocity).Length(); // Wind angle causes max rotation when wind is perfectly to the Right or Left of helicopter // since the arm of the tail rotor is the longest then, as seen from the wind. // As the wind approaches the frontside or tailside, the effect decreases. var windAngleFactor = (float)Math.Abs(Math.Sin(relativeAirflowAngle)); // float windFactor = horizontalWindMagnitudeFactor*windAngleFactor; // The yaw rate [rad/s] per unit of wind velocity [m/s] const float yawRatePerVelocity = 0.1f; float yawRate = windAngleFactor * horizontalWindMagnitudeFactor * yawRatePerVelocity; // TODO temp //0.1745f; // ~10 degrees per second float deltaYaw = yawRate * dt; if (relativeAirflowAngle < 0) { deltaYaw = -deltaYaw; } newOrientation = VectorHelper.AddPitchRollYaw(currentOrientation, 0, 0, deltaYaw); }
/// <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); }
public void Test() { JoystickOutput output; output.Pitch = 0.312312432f; output.Roll = 0.512312432f; output.Yaw = 0.912312432f; const float dt = 0.017001f; float pitchRate = output.Pitch * PhysicsConstants.MaxPitchRate; float rollRate = output.Roll * PhysicsConstants.MaxRollRate; float yawRate = output.Yaw * PhysicsConstants.MaxYawRate; Quaternion orient1 = Quaternion.Identity; Quaternion orient2 = Quaternion.Identity; for (int i = 0; i < 10000; i++) { float deltaPitch = (output.Pitch * PhysicsConstants.MaxPitchRate) * dt; float deltaRoll = (output.Roll * PhysicsConstants.MaxRollRate) * dt; float deltaYaw = (output.Yaw * PhysicsConstants.MaxYawRate) * dt; // Add deltas of pitch, roll and yaw to the rotation matrix orient1 = VectorHelper.AddPitchRollYaw(orient1, deltaPitch, deltaRoll, deltaYaw); deltaPitch = pitchRate * dt; deltaRoll = rollRate * dt; deltaYaw = yawRate * dt; orient2 = VectorHelper.AddPitchRollYaw(orient2, deltaPitch, deltaRoll, deltaYaw); } Assert.AreEqual(orient1.X, orient2.X, "X"); Assert.AreEqual(orient1.Y, orient2.Y, "Y"); Assert.AreEqual(orient1.Z, orient2.Z, "Z"); Assert.AreEqual(orient1.W, orient2.W, "W"); }