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