public void HandleIMUData(double yawDown, double pitchLeft, double rollLeft, double accX, double accY, double accZ)
{
if (EnsureMahonyReady())
{
mahonyAHRS.Update((float)(rollLeft * (Math.PI / 180)), (float)(pitchLeft * (Math.PI / 180)), (float)(yawDown * (Math.PI / 180)), (float)accX, (float)accY, (float)accZ);
}
}
private void OnNextSensor(MoovEvent e)
{
_ahrs.Update(
Deg2Rad(e.Gyroscope.X), Deg2Rad(e.Gyroscope.Y), Deg2Rad(e.Gyroscope.Z),
e.Accelerometer.X, e.Accelerometer.Y, e.Accelerometer.Z
);
var quat = new Quaternion(_ahrs.Quaternion[0], _ahrs.Quaternion[1], _ahrs.Quaternion[2], _ahrs.Quaternion[3]);
var acc = e.Accelerometer;
var alignedAcc = Vector3.Transform(acc, Quaternion.Conjugate(quat));
var velocity = _prevVelocity + (alignedAcc * SamplePeriod);
_prevVelocity = velocity;
var position = _prevPosition + (velocity * SamplePeriod);
_prevPosition = position;
_motionEvents.OnNext(new MotionEvent
{
Quaternion = quat,
Acceleration = alignedAcc,
Velocity = velocity,
Position = position,
RawAccelerometer = e.Accelerometer,
RawGyroscope = e.Gyroscope
});
}
public void Update(byte[] bytes)
{
var dataInPackage = (Flags)bytes[1];
bool raw;
bool orientation;
SetFlags(dataInPackage, out raw, out orientation);
if (!freeqSampled && raw)
{
if (samples == 0)
{
started = DateTime.Now;
}
samples++;
var delta = (DateTime.Now - started).TotalSeconds;
if (delta > 1)
{
var freq = samples / (float)delta;
freeqSampled = true;
ahrs = new MahonyAHRS(1f / freq, 0.1f);
}
else
{
return;
}
}
var index = 2;
if (raw)
{
var ax = Raw.ax = GetFloat(bytes, index, 0);
var ay = Raw.ay = GetFloat(bytes, index, 4);
var az = Raw.az = GetFloat(bytes, index, 8);
var gx = Raw.gx = GetFloat(bytes, index, 12);
var gy = Raw.gy = GetFloat(bytes, index, 16);
var gz = Raw.gz = GetFloat(bytes, index, 20);
var mx = Raw.mx = GetFloat(bytes, index, 24);
var my = Raw.my = GetFloat(bytes, index, 28);
var mz = Raw.mz = GetFloat(bytes, index, 32);
ahrs.Update((float)gx, (float)gy, (float)gz, (float)ax, (float)ay, (float)az, (float)mx, (float)my, (float)mz);
quaternion.Update(ahrs.Quaternion[0], ahrs.Quaternion[1], ahrs.Quaternion[2], ahrs.Quaternion[3]);
index += 36;
}
if (orientation)
{
GoogleYaw = GetFloat(bytes, index, 0);
GooglePitch = GetFloat(bytes, index, 4);
GoogleRoll = GetFloat(bytes, index, 8);
index += 12;
}
NewData = true;
}