protected void AssignNewReadings(SensorReadings readings, bool processReadings = true)
{
if (processReadings)
{
ProcessReadings(ref readings);
}
Readings = readings;
OnReadingsChanged?.Invoke(this, EventArgs.Empty);
}
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;
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 static bool TryReadSensor(
Sensor sensor,
out SensorReadings readings,
out string errorMessage)
{
errorMessage = string.Empty;
if (sensor.Initiated)
{
try
{
if (sensor.Update())
{
readings = sensor.Readings;
return true;
}
}
catch (Exception exception)
{
errorMessage = exception.Message;
}
}
readings = new SensorReadings();
return false;
}
protected virtual void ProcessReadings(ref SensorReadings readings)
{
}
/// <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 SensorReadings
{
Gyro = new Vector3(gx, gy, gz),
Acceleration = new Vector3(ax, ay, az),
MagneticField = new Vector3(mx, my, mz),
Timestamp = timestamp
};
AssignNewReadings(readings, false);
}
private void SetCalibrationData(SensorReadings 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;
}
}
private void CalibrateAverageCompass(ref SensorReadings 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;
}
}
private void HandleGyroBias(ref SensorReadings 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;
}
}
}
private void PerformAxisRotation(ref SensorReadings 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);
}
}
protected override void ProcessReadings(ref SensorReadings readings)
{
PerformAxisRotation(ref readings);
HandleGyroBias(ref readings);
CalibrateAverageCompass(ref readings);
Fusion.ProcessNewImuReadings(ref readings);
}