internal override void ProcessNewImuReadings(ref SensorReadings imuReadings)
{
SampleNumber++;
Gyro = EnableGyro && imuReadings.GyroValid
? (Vector3?)imuReadings.Gyro
: null;
Acceleration = EnableAcceleration && imuReadings.AccelerationValid
? (Vector3?)imuReadings.Acceleration
: null;
MagneticField = EnableMagneticField && imuReadings.MagneticFieldValid
? (Vector3?)imuReadings.MagneticField
: null;
LastFusionTime = imuReadings.Timestamp;
CalculatePose(Acceleration, MagneticField, CompassAdjDeclination);
// initialize the poses
StateQ.FromEuler(MeasuredPose);
FusionQPose = StateQ;
FusionPose = MeasuredPose;
imuReadings.FusionPoseValid = true;
imuReadings.FusionQPoseValid = true;
imuReadings.FusionPose = FusionPose;
imuReadings.FusionQPose = FusionQPose;
}
internal override void ProcessNewImuReadings(ref ImuSensorReadings imuReadings)
{
SampleNumber++;
Gyro = EnableGyro && imuReadings.GyroValid
? (Vector3?)imuReadings.Gyro
: null;
Acceleration = EnableAcceleration && imuReadings.AccelerationValid
? (Vector3?)imuReadings.Acceleration
: null;
MagneticField = EnableMagneticField && imuReadings.MagneticFieldValid
? (Vector3?)imuReadings.MagneticField
: null;
if (FirstTime)
{
LastFusionTime = imuReadings.Timestamp;
CalculatePose(Acceleration, MagneticField, CompassAdjDeclination);
// initialize the poses
StateQ.FromEuler(MeasuredPose);
FusionQPose = StateQ;
FusionPose = MeasuredPose;
FirstTime = false;
}
else
{
TimeDelta = imuReadings.Timestamp - LastFusionTime;
if (TimeDelta > TimeSpan.Zero)
{
CalculatePose(Acceleration, MagneticField, CompassAdjDeclination);
Predict();
Update();
StateQ.ToEuler(out FusionPose);
FusionQPose = StateQ;
}
LastFusionTime = imuReadings.Timestamp;
}
imuReadings.FusionPoseValid = true;
imuReadings.FusionQPoseValid = true;
imuReadings.FusionPose = FusionPose;
imuReadings.FusionQPose = FusionQPose;
}
