Android SensorManager-Gyroskop zu absoluter Lage

1// Create a constant to convert nanoseconds to seconds.
2private static final float NS2S = 1.0f / 1000000000.0f;
3private final float[] deltaRotationVector = new float[4]();
4private float timestamp;
5
6public void onSensorChanged(SensorEvent event) {
7 // This timestep's delta rotation to be multiplied by the current rotation
8 // after computing it from the gyro sample data.
9 if (timestamp != 0) {
10 final float dT = (event.timestamp - timestamp) * NS2S;
11 // Axis of the rotation sample, not normalized yet.
12 float axisX = event.values[0];
13 float axisY = event.values[1];
14 float axisZ = event.values[2];
15
16 // Calculate the angular speed of the sample
17 float omegaMagnitude = sqrt(axisX*axisX + axisY*axisY + axisZ*axisZ);
18
19 // Normalize the rotation vector if it's big enough to get the axis
20 // (that is, EPSILON should represent your maximum allowable margin of error)
21 if (omegaMagnitude > EPSILON) {
22 axisX /= omegaMagnitude;
23 axisY /= omegaMagnitude;
24 axisZ /= omegaMagnitude;
25 }
26
27 // Integrate around this axis with the angular speed by the timestep
28 // in order to get a delta rotation from this sample over the timestep
29 // We will convert this axis-angle representation of the delta rotation
30 // into a quaternion before turning it into the rotation matrix.
31 float thetaOverTwo = omegaMagnitude * dT / 2.0f;
32 float sinThetaOverTwo = sin(thetaOverTwo);
33 float cosThetaOverTwo = cos(thetaOverTwo);
34 deltaRotationVector[0] = sinThetaOverTwo * axisX;
35 deltaRotationVector[1] = sinThetaOverTwo * axisY;
36 deltaRotationVector[2] = sinThetaOverTwo * axisZ;
37 deltaRotationVector[3] = cosThetaOverTwo;
38 }
39 timestamp = event.timestamp;
40 float[] deltaRotationMatrix = new float[9];
41 SensorManager.getRotationMatrixFromVector(deltaRotationMatrix, deltaRotationVector);
42 // User code should concatenate the delta rotation we computed with the current rotation
43 // in order to get the updated rotation.
44 // rotationCurrent = rotationCurrent * deltaRotationMatrix;
45 }
46}