public void AddMeasurement(IBandGyroscopeReading reading)
{
var calibratedXReading = reading.AngularVelocityX - 0.0232528209480614;
var calibratedYReading = reading.AngularVelocityY + 0.648518794348248;
var calibratedZReading = reading.AngularVelocityZ - 0.162816270729729;
if (_lastGyroStamp == default(DateTimeOffset))
{
_lastVelocity = new Vector(calibratedXReading, calibratedYReading, calibratedZReading);
_lastGyroStamp = reading.Timestamp;
return;
}
var msSinceLast = (reading.Timestamp - _lastGyroStamp).TotalMilliseconds;
_lastGyroStamp = reading.Timestamp;
if (msSinceLast < 0.01)
return;
var orientationX = (calibratedXReading + (calibratedXReading - _lastVelocity.X) / 2.0) * msSinceLast / 1000.0;
var orientationY = (calibratedYReading + (calibratedYReading - _lastVelocity.Y) / 2.0) * msSinceLast / 1000.0;
var orientationZ = (calibratedZReading + (calibratedZReading - _lastVelocity.Z) / 2.0) * msSinceLast / 1000.0;
_orientation = _orientation.Add(orientationX, orientationY, orientationZ);
_lastVelocity = new Vector(calibratedXReading, calibratedYReading, calibratedZReading);
}
private void CalibratePointer()
{
if (!_gotFirstCalib)
{
_maxValues = _bandInputOutput.GyroReading;
_minValues = _bandInputOutput.GyroReading;
_gotFirstCalib = true;
return;
}
_maxValues = _maxValues.Max(_bandInputOutput.GyroReading);
_minValues = _minValues.Min(_bandInputOutput.GyroReading);
_zOffset = -_minValues.Z;
_zGain = 1.0 / (_maxValues.Z - _minValues.Z);
_yOffset = -_minValues.Y;
_yGain = 1.0 / (_maxValues.Y - _minValues.Y);
}
public void AddMeasurement(IBandAccelerometerReading reading)
{
_numMeasPoints++;
var acc = new Vector(reading.AccelerationX, reading.AccelerationY, reading.AccelerationZ);
if (_numMeasPoints < 10)
{
_calibrationValues.Add(acc);
return;
}
if (_numMeasPoints == 10)
{
_calibrationValue = 1.0 / _calibrationValues.AverageValue;
_calibrationValues = null;
Debug.WriteLine($"Calibrated to {_calibrationValue}");
}
_lastAccValue = acc.Multiply(9.81 * _calibrationValue);
//Debug.WriteLine($"Acc: {acc}, value: {acc.Value}");
}
public void Add(Vector pt)
{
_movingAverage.Add(pt);
if (_movingAverage.Count > 10000)
_movingAverage.RemoveAt(0);
}
public Vector Min(Vector gyroReading)
{
return new Vector(Math.Min(X, gyroReading.X), Math.Min(Y, gyroReading.Y), Math.Min(Z, gyroReading.Z));
}
public Vector Add(Vector other)
{
return new Vector(X + other.X, Y + other.Y, Z + other.Z);
}