protected virtual void ProcessReadings(ref ImuSensorReadings readings)
{
PerformAxisRotation(ref readings);
HandleGyroBias(ref readings);
CalibrateAverageCompass(ref readings);
Fusion.ProcessNewImuReadings(ref readings);
}
protected void AssignNewReadings(ImuSensorReadings readings, bool processReadings = true)
{
if (processReadings)
{
ProcessReadings(ref readings);
}
Readings = readings;
OnReadingsChanged?.Invoke(this, EventArgs.Empty);
}
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;
}
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);
}
}
/// <summary>
/// Tries to update the readings.
/// Returns true if new readings are available, otherwise false.
/// An exception is thrown if something goes wrong.
/// </summary>
public override bool Update()
{
byte status = I2CSupport.Read8Bits(_accelGyroI2CDevice, LSM9DS1Defines.STATUS, "Failed to read LSM9DS1 status");
if ((status & 0x03) != 0x03)
{
// Data not yet available
return(false);
}
byte[] gyroData = I2CSupport.ReadBytes(_accelGyroI2CDevice, 0x80 + LSM9DS1Defines.OUT_X_L_G, 6, "Failed to read LSM9DS1 gyro data");
byte[] accelData = I2CSupport.ReadBytes(_accelGyroI2CDevice, 0x80 + LSM9DS1Defines.OUT_X_L_XL, 6, "Failed to read LSM9DS1 accel data");
byte[] magData = I2CSupport.ReadBytes(_magI2CDevice, 0x80 + LSM9DS1Defines.MAG_OUT_X_L, 6, "Failed to read LSM9DS1 compass data");
var readings = new ImuSensorReadings
{
Timestamp = DateTime.Now,
Gyro = MathSupport.ConvertToVector(gyroData, _gyroScale),
GyroValid = true,
Acceleration = MathSupport.ConvertToVector(accelData, _accelerationScale),
AccelerationValid = true,
MagneticField = MathSupport.ConvertToVector(magData, _magneticFieldScale),
MagneticFieldValid = true
};
// Sort out gyro axes and correct for bias
readings.Gyro.Z = -readings.Gyro.Z;
// Sort out accel data;
readings.Acceleration.X = -readings.Acceleration.X;
readings.Acceleration.Y = -readings.Acceleration.Y;
// Sort out mag axes
readings.MagneticField.X = -readings.MagneticField.X;
readings.MagneticField.Z = -readings.MagneticField.Z;
AssignNewReadings(readings);
return(true);
}
private void HandleGyroBias(ref ImuSensorReadings readings)
{
if (readings.AccelerationValid)
{
Vector3 deltaAccel = _previousAccel;
deltaAccel -= readings.Acceleration; // compute difference
_previousAccel = readings.Acceleration;
if (readings.GyroValid)
{
if ((deltaAccel.Length() < FuzzyAccelZero) && (readings.Gyro.Length() < FuzzyGyroZero))
{
// what we are seeing on the gyros should be bias only so learn from this
if (_gyroSampleCount < (5 * SampleRate))
{
_gyroBias.X = (1.0 - _gyroLearningAlpha) * _gyroBias.X + _gyroLearningAlpha * readings.Gyro.X;
_gyroBias.Y = (1.0 - _gyroLearningAlpha) * _gyroBias.Y + _gyroLearningAlpha * readings.Gyro.Y;
_gyroBias.Z = (1.0 - _gyroLearningAlpha) * _gyroBias.Z + _gyroLearningAlpha * readings.Gyro.Z;
_gyroSampleCount++;
if (_gyroSampleCount == (5 * SampleRate))
{
// this could have been true already of course
GyroBiasValid = true;
}
}
else
{
_gyroBias.X = (1.0 - _gyroContinuousAlpha) * _gyroBias.X + _gyroContinuousAlpha * readings.Gyro.X;
_gyroBias.Y = (1.0 - _gyroContinuousAlpha) * _gyroBias.Y + _gyroContinuousAlpha * readings.Gyro.Y;
_gyroBias.Z = (1.0 - _gyroContinuousAlpha) * _gyroBias.Z + _gyroContinuousAlpha * readings.Gyro.Z;
}
}
readings.Gyro -= _gyroBias;
}
}
}
/// <summary>
/// Assign a new reading from an external source
/// </summary>
public void SetExtData(
double gx,
double gy,
double gz,
double ax,
double ay,
double az,
double mx,
double my,
double mz,
DateTime timestamp)
{
var readings = new ImuSensorReadings
{
Gyro = new Vector3(gx, gy, gz),
Acceleration = new Vector3(ax, ay, az),
MagneticField = new Vector3(mx, my, mz),
Timestamp = timestamp
};
AssignNewReadings(readings, false);
}
public ImuSensorWatcher(CoreDispatcher coreDispatcher, int period)
{
this.coreDispatcher = coreDispatcher;
imuSensor = new LSM9DS1ImuSensor(
LSM9DS1Defines.ADDRESS0,
LSM9DS1Defines.MAG_ADDRESS0,
new LSM9DS1Config(),
new SensorFusionRTQF());
imuSensor.InitAsync().ContinueWith(task =>
{
pollTimer = ThreadPoolTimer.CreatePeriodicTimer(timer =>
{
if (imuSensor.Initiated)
{
if (imuSensor.Update())
{
Readings = imuSensor.Readings;
}
}
}, TimeSpan.FromMilliseconds(period));
});
}
private void CalibrateAverageCompass(ref ImuSensorReadings readings)
{
// calibrate if required
SetCalibrationData(readings);
if (readings.MagneticFieldValid)
{
if (MagCalValid)
{
readings.MagneticField.X = (readings.MagneticField.X - _magCalOffset[0]) * _magCalScale[0];
readings.MagneticField.Y = (readings.MagneticField.Y - _magCalOffset[1]) * _magCalScale[1];
readings.MagneticField.Z = (readings.MagneticField.Z - _magCalOffset[2]) * _magCalScale[2];
}
// update running average
_magAverage.X = readings.MagneticField.X * CompassAlpha + _magAverage.X * (1.0 - CompassAlpha);
_magAverage.Y = readings.MagneticField.Y * CompassAlpha + _magAverage.Y * (1.0 - CompassAlpha);
_magAverage.Z = readings.MagneticField.Z * CompassAlpha + _magAverage.Z * (1.0 - CompassAlpha);
readings.MagneticField = _magAverage;
}
}
internal abstract void ProcessNewImuReadings(ref ImuSensorReadings imuReadings);
private void SetCalibrationData(ImuSensorReadings readings)
{
if (!readings.MagneticFieldValid)
{
return;
}
bool changed = false;
// see if there is a new max or min
if (_magMax[0] < readings.MagneticField.X)
{
_magMax[0] = readings.MagneticField.X;
changed = true;
}
if (_magMax[1] < readings.MagneticField.Y)
{
_magMax[1] = readings.MagneticField.Y;
changed = true;
}
if (_magMax[2] < readings.MagneticField.Z)
{
_magMax[2] = readings.MagneticField.Z;
changed = true;
}
if (_magMin[0] > readings.MagneticField.X)
{
_magMin[0] = readings.MagneticField.X;
changed = true;
}
if (_magMin[1] > readings.MagneticField.Y)
{
_magMin[1] = readings.MagneticField.Y;
changed = true;
}
if (_magMin[2] > readings.MagneticField.Z)
{
_magMin[2] = readings.MagneticField.Z;
changed = true;
}
if (!changed)
{
return;
}
double delta;
if (!MagCalValid)
{
MagCalValid = true;
for (int i = 0; i < 3; i++)
{
delta = _magMax[i] - _magMin[i];
if ((delta < 30) || (_magMin[i] > 0) || (_magMax[i] < 0))
{
MagCalValid = false;
break;
}
}
}
if (MagCalValid)
{
_magMaxDelta = -1;
for (int i = 0; i < 3; i++)
{
if ((_magMax[i] - _magMin[i]) > _magMaxDelta)
{
_magMaxDelta = _magMax[i] - _magMin[i];
}
}
// adjust for + and - range
_magMaxDelta /= 2.0;
}
for (int i = 0; i < 3; i++)
{
delta = (_magMax[i] - _magMin[i]) / 2.0;
_magCalScale[i] = _magMaxDelta / delta;
_magCalOffset[i] = (_magMax[i] + _magMin[i]) / 2.0;
}
}
