public HelicopterLogSnapshot(PhysicalHeliState trueState, PhysicalHeliState observedState, PhysicalHeliState estimatedState, PhysicalHeliState blindEstimatedState, ForwardRightUp accelerometer, float groundAltitude, TimeSpan time) { True = trueState; Estimated = estimatedState; Observed = observedState; Time = time; Accelerometer = accelerometer; BlindEstimated = blindEstimatedState; GroundAltitude = groundAltitude; }
private static void WriteFRU(XmlWriter w, string nodeName, ForwardRightUp value) { w.WriteStartElement(nodeName); w.WriteAttributeString("Forward", value.Forward.ToString(Localization)); w.WriteAttributeString("Right", value.Right.ToString(Localization)); w.WriteAttributeString("Up", value.Up.ToString(Localization)); w.WriteEndElement(); }
public override void Update(PhysicalHeliState startState, PhysicalHeliState endState, JoystickOutput output, TimeSpan startTime, TimeSpan endTime) { _accelerometer.Update(startState, endState, output, startTime, endTime); _gyroscope.Update(startState, endState, output, startTime, endTime); // Make sure we use orientation at start of timestep to transform to world, since IMU should be able to calculate accumulated position // by this world acceleration, and according to my physics simulation the position is calculated before rotation is. AccelerationWorld = _accelerometer.AccelerationWorld; AccelerationLocal = new ForwardRightUp(_accelerometer.Forward, _accelerometer.Right, _accelerometer.Up); AngularRateBody = AngularValues.FromRadians(_gyroscope.Rate); AngularDeltaBody = AngularValues.FromRadians(_gyroscope.Delta); PitchRollYaw delta = AngularDeltaBody.Radians; AccumulatedOrientation = VectorHelper.AddPitchRollYaw(AccumulatedOrientation, delta.Pitch, delta.Roll, delta.Yaw); }