protected ImuSensor(SensorFusion fusion)
{
Fusion = fusion;
AxisRotation = new AxisRotation();
for (int i = 0; i < 3; i++)
{
_magMax[i] = -1000.0;
_magMin[i] = 1000.0;
}
}
private void PerformAxisRotation(ref ImuSensorReadings readings)
{
if (readings.GyroValid)
{
readings.Gyro = AxisRotation.Rotate(readings.Gyro);
}
if (readings.AccelerationValid)
{
readings.Acceleration = AxisRotation.Rotate(readings.Acceleration);
}
if (readings.MagneticFieldValid)
{
readings.MagneticField = AxisRotation.Rotate(readings.MagneticField);
}
}