/**
* Calculates a rotation vector from the gyroscope angular speed values.
*
* @param gyroValues
* @param deltaRotationVector
* @param timeFactor
* @see http://developer.android .com/reference/android/hardware/SensorEvent.html#values
*/
private void getRotationVectorFromGyro(float timeFactor) {
// Calculate the angular speed of the sample
float magnitude =
(float)
Math.sqrt(
Math.pow(gyroscope[0], 2) + Math.pow(gyroscope[1], 2) + Math.pow(gyroscope[2], 2));
// Normalize the rotation vector if it's big enough to get the axis
if (magnitude > EPSILON) {
gyroscope[0] /= magnitude;
gyroscope[1] /= magnitude;
gyroscope[2] /= magnitude;
}
// Integrate around this axis with the angular speed by the timestep
// in order to get a delta rotation from this sample over the timestep
// We will convert this axis-angle representation of the delta rotation
// into a quaternion before turning it into the rotation matrix.
float thetaOverTwo = magnitude * timeFactor / 2.0f;
float sinThetaOverTwo = (float) Math.sin(thetaOverTwo);
float cosThetaOverTwo = (float) Math.cos(thetaOverTwo);
deltaVectorGyro[0] = sinThetaOverTwo * gyroscope[0];
deltaVectorGyro[1] = sinThetaOverTwo * gyroscope[1];
deltaVectorGyro[2] = sinThetaOverTwo * gyroscope[2];
deltaVectorGyro[3] = cosThetaOverTwo;
// Create a new quaternion object base on the latest rotation
// measurements...
quatGyroDelta = new Quaternion(deltaVectorGyro[3], Arrays.copyOfRange(deltaVectorGyro, 0, 3));
// Since it is a unit quaternion, we can just multiply the old rotation
// by the new rotation delta to integrate the rotation.
quatGyro = quatGyro.multiply(quatGyroDelta);
}
/** Calculates orientation angles from accelerometer and magnetometer output. */
private void calculateOrientation() {
// To get the orientation vector from the acceleration and magnetic
// sensors, we let Android do the heavy lifting. This call will
// automatically compensate for the tilt of the compass and fail if the
// magnitude of the acceleration is not close to 9.82m/sec^2. You could
// perform these steps yourself, but in my opinion, this is the best way
// to do it.
if (SensorManager.getRotationMatrix(rotationMatrix, null, acceleration, magnetic)) {
SensorManager.getOrientation(rotationMatrix, baseOrientation);
getRotationVectorFromAccelMag(baseOrientation);
if (!hasOrientation) {
quatGyro = new Quaternion(quatAccelMag.getScalarPart(), quatAccelMag.getVectorPart());
}
hasOrientation = true;
}
}
/** Calculate the fused orientation. */
private void calculateFusedOrientation() {
float oneMinusCoeff = (1.0f - filterCoefficient);
// Apply the complementary filter. // We multiply each rotation by their
// coefficients (scalar matrices)...
// Scale our quaternion for the gyroscope
quatGyro = quatGyro.multiply(filterCoefficient);
// Scale our quaternion for the accel/mag
quatAccelMag = quatAccelMag.multiply(oneMinusCoeff);
// ...and then add the two quaternions together.
// output[0] = alpha * output[0] + (1 - alpha) * input[0];
quatGyro = quatGyro.add(quatAccelMag);
// Now we get a structure we can pass to get a rotation matrix, and then
// an orientation vector from Android.
fusedVector[0] = (float) quatGyro.getVectorPart()[0];
fusedVector[1] = (float) quatGyro.getVectorPart()[1];
fusedVector[2] = (float) quatGyro.getVectorPart()[2];
fusedVector[3] = (float) quatGyro.getScalarPart();
// We need a rotation matrix so we can get the orientation vector...
// Getting Euler
// angles from a quaternion is not trivial, so this is the easiest way,
// but perhaps
// not the fastest way of doing this.
SensorManager.getRotationMatrixFromVector(fusedMatrix, fusedVector);
// Get the fused orienatation
SensorManager.getOrientation(fusedMatrix, fusedOrientation);
}
