Android rotation sensor value and datarate fix

There was a breaking change in Android rotation sensor handling in
Qt 5.12.4 when migrating to NDK usage:
-The unit conversion logic was not transferred as part of
the migration. This task reintroduces the unit conversion part.
-The datarate interval was misinterpreted as usec, whereas it is Hz

Fixes: QTBUG-77423
Change-Id: Ibe51b3dea393e1cbe1502b5611c07f32024daf57
Reviewed-by: Paul Olav Tvete <paul.tvete@qt.io>

2 files changed, 37 insertions, 7 deletions

diff --git a/src/plugins/sensors/android/androidrotation.cpp b/src/plugins/sensors/android/androidrotation.cpp
index c1d141a0..068d305f 100644
--- a/src/plugins/sensors/android/androidrotation.cpp
+++ b/src/plugins/sensors/android/androidrotation.cpp
@@ -48,12 +48,41 @@ AndroidRotation::AndroidRotation(int type, QSensor *sensor, QObject *parent)
 
 void AndroidRotation::dataReceived(const ASensorEvent &event)
 {
-    // ### Check me, at first look it seems wrong,
-    // here https://developer.android.com/reference/android/hardware/SensorEvent.html#sensor.type_rotation_vector:
-    // are the Android values
-    qreal rz = -qRadiansToDegrees(qreal(event.data[0])); // event.data[0] corresponds to x
-    qreal rx = -qRadiansToDegrees(qreal(event.data[1])); // event.data[1] corresponds to y
-    qreal ry =  qRadiansToDegrees(qreal(event.data[2])); // event.data[2] corresponds to z
+    // From android documentation, the rotation sensor values are:
+    // values[0]: x*sin(θ/2)
+    // values[1]: y*sin(θ/2)
+    // values[2]: z*sin(θ/2)
+    // values[3]: cos(θ/2)
+
+    // The mathematics below is adapted from
+    // https://android.googlesource.com/platform/frameworks/base/+/master/core/java/android/hardware/SensorManager.java#1644
+    // and
+    // https://android.googlesource.com/platform/frameworks/base/+/master/core/java/android/hardware/SensorManager.java#1469
+    // (getRotationMatrixFromVector() followed by getOrientation())
+    qreal angles[3];
+    qreal q1 = qreal(event.data[0]);
+    qreal q2 = qreal(event.data[1]);
+    qreal q3 = qreal(event.data[2]);
+    qreal q0 = qreal(event.data[3]);
+
+    qreal sq_q1 = 2 * q1 * q1;
+    qreal sq_q2 = 2 * q2 * q2;
+    qreal sq_q3 = 2 * q3 * q3;
+    qreal q1_q2 = 2 * q1 * q2;
+    qreal q3_q0 = 2 * q3 * q0;
+    qreal q1_q3 = 2 * q1 * q3;
+    qreal q2_q0 = 2 * q2 * q0;
+    qreal q2_q3 = 2 * q2 * q3;
+    qreal q1_q0 = 2 * q1 * q0;
+
+    angles[0] = std::atan2((q1_q2 - q3_q0), (1 - sq_q1 - sq_q3));
+    angles[1] = std::asin(-(q2_q3 + q1_q0));
+    angles[2] = std::atan2(-(q1_q3 - q2_q0), (1 - sq_q1 - sq_q2));
+
+    qreal rz = -qRadiansToDegrees(angles[0]);
+    qreal rx = -qRadiansToDegrees(angles[1]);
+    qreal ry =  qRadiansToDegrees(angles[2]);
+
     if (sensor()->skipDuplicates() && qFuzzyCompare(m_reader.x(), rx) &&
             qFuzzyCompare(m_reader.y(), ry) &&
             qFuzzyCompare(m_reader.z(), rz)) {
diff --git a/src/plugins/sensors/android/sensoreventqueue.h b/src/plugins/sensors/android/sensoreventqueue.h
index 77bf05c6..e6087059 100644
--- a/src/plugins/sensors/android/sensoreventqueue.h
+++ b/src/plugins/sensors/android/sensoreventqueue.h
@@ -125,8 +125,9 @@ public:
     void start() override
     {
         ASensorEventQueue_enableSensor(m_sensorEventQueue, m_sensor);
+        // The QSensor datarate is in Hz (frequency), whereas the setEventRate expects microseconds
         if (sensor()->dataRate() > 0)
-            ASensorEventQueue_setEventRate(m_sensorEventQueue, m_sensor, std::max(ASensor_getMinDelay(m_sensor), sensor()->dataRate()));
+            ASensorEventQueue_setEventRate(m_sensorEventQueue, m_sensor, std::max(ASensor_getMinDelay(m_sensor), 1000000/sensor()->dataRate()));
         m_started = true;
     }
     void stop() override