public void PacketReceived(INEMO2_FrameData data)
{
if (alg == 1) //Complementary filter
{
ComplementaryFilter.complementaryFiltering(data.Gyroscope.X, data.Gyroscope.Y, data.Gyroscope.Z,
data.Accelometer.X, data.Accelometer.Y, data.Accelometer.Z,
data.Magnetometer.X, data.Magnetometer.Y, data.Magnetometer.Z);
}
if (alg == 2) //AHRS
{
AHRS.update(-data.Gyroscope.X, -data.Gyroscope.Y, -data.Gyroscope.Z,
-data.Accelometer.X, data.Accelometer.Y, -data.Accelometer.Z,
data.Magnetometer.X, data.Magnetometer.Y, -data.Magnetometer.Z); // update filter with sensor data
}
if (alg == 3) //Kalman filter
{
KalmanFilter.filter_step(data.Gyroscope.X, data.Gyroscope.Y, data.Gyroscope.Z,
data.Accelometer.X, data.Accelometer.Y, data.Accelometer.Z,
data.Magnetometer.X, data.Magnetometer.Y, data.Magnetometer.Z);
}
}
public void PacketReceived(INEMO2_FrameData data)
{
filter.filterStep(data.Gyroscope.X, data.Gyroscope.Y, data.Gyroscope.Z,
data.Accelometer.X, data.Accelometer.Y, data.Accelometer.Z,
data.Magnetometer.X, data.Magnetometer.Y, data.Magnetometer.Z);
}
