Blackberry: Use the rotation matrix as the compass source

This fixes the direction flipping over when holding the device
upside-down.

Change-Id: I57bac63208d475d4091b47593b9768613eea30b0
Reviewed-by: Sean Harmer <sean.harmer@kdab.com>
Reviewed-by: Adam Parco <aparco@rim.com>
Reviewed-by: Lorn Potter <lorn.potter@nokia.com>

6 files changed, 107 insertions, 39 deletions

diff --git a/src/plugins/sensors/blackberry/bbcompass.cpp b/src/plugins/sensors/blackberry/bbcompass.cpp
index a77c99ad..9ada2cd5 100644
--- a/src/plugins/sensors/blackberry/bbcompass.cpp
+++ b/src/plugins/sensors/blackberry/bbcompass.cpp
@@ -39,16 +39,26 @@
 **
 ****************************************************************************/
 #include "bbcompass.h"
+#include "bbutil.h"
+
+using namespace BbUtil;
 
 BbCompass::BbCompass(QSensor *sensor)
-    : BbSensorBackend<QCompassReading>(devicePath(), SENSOR_TYPE_AZIMUTH_PITCH_ROLL, sensor)
+    : BbSensorBackend<QCompassReading>(devicePath(), SENSOR_TYPE_ROTATION_MATRIX, sensor)
 {
     setDescription(QLatin1String("Azimuth in degrees from magnetic north"));
 }
 
 bool BbCompass::updateReadingFromEvent(const sensor_event_t &event, QCompassReading *reading)
 {
-    reading->setAzimuth(event.apr.azimuth);
+    float xRad, yRad, zRad;
+    matrixToEulerZXY(event.rotation_matrix, xRad, yRad, zRad);
+    float azimuth = radiansToDegrees(zRad);
+    if (azimuth < 0)
+        azimuth = -azimuth;
+    else
+        azimuth = 360.0f - azimuth;
+    reading->setAzimuth(azimuth);
 
     switch (event.accuracy) {
     case SENSOR_ACCURACY_UNRELIABLE:
@@ -75,5 +85,5 @@ bool BbCompass::updateReadingFromEvent(const sensor_event_t &event, QCompassRead
 
 QString BbCompass::devicePath()
 {
-    return QLatin1String("/dev/sensor/apr");
+    return QLatin1String("/dev/sensor/rotMatrix");
 }
diff --git a/src/plugins/sensors/blackberry/bbgyroscope.cpp b/src/plugins/sensors/blackberry/bbgyroscope.cpp
index 5372b55d..5280c5d6 100644
--- a/src/plugins/sensors/blackberry/bbgyroscope.cpp
+++ b/src/plugins/sensors/blackberry/bbgyroscope.cpp
@@ -43,6 +43,8 @@
 #include "bbutil.h"
 #include <QtCore/qmath.h>
 
+using namespace BbUtil;
+
 BbGyroscope::BbGyroscope(QSensor *sensor)
     : BbSensorBackend<QGyroscopeReading>(devicePath(), SENSOR_TYPE_GYROSCOPE, sensor)
 {
diff --git a/src/plugins/sensors/blackberry/bbrotationsensor.cpp b/src/plugins/sensors/blackberry/bbrotationsensor.cpp
index e6d038d2..0ed1495b 100644
--- a/src/plugins/sensors/blackberry/bbrotationsensor.cpp
+++ b/src/plugins/sensors/blackberry/bbrotationsensor.cpp
@@ -46,6 +46,8 @@
 #include <QScreen>
 #include <qpa/qplatformscreen.h>
 
+using namespace BbUtil;
+
 BbRotationSensor::BbRotationSensor(QSensor *sensor)
     : BbSensorBackend<QRotationReading>(devicePath(), SENSOR_TYPE_ROTATION_MATRIX, sensor),
       m_orientation(Qt::PrimaryOrientation),
@@ -72,35 +74,6 @@ bool BbRotationSensor::addDefaultRange()
     return false;
 }
 
-static float getMatrixElement(const float matrix[3*3], int index0, int index1)
-{
-    return matrix[index0 * 3 + index1];
-}
-
-static void matrixToEulerZXY(const float matrix[3*3],
-                             float &thetaX, float &thetaY, float& thetaZ)
-{
-    thetaX = asin( getMatrixElement(matrix, 2, 1));
-    if ( thetaX < M_PI_2 ) {
-        if ( thetaX > -M_PI_2 ) {
-            thetaZ = atan2( -getMatrixElement(matrix, 0, 1),
-                             getMatrixElement(matrix, 1, 1) );
-            thetaY = atan2( -getMatrixElement(matrix, 2, 0),
-                             getMatrixElement(matrix, 2, 2) );
-        } else {
-            // Not a unique solution
-            thetaZ = -atan2( getMatrixElement(matrix, 0, 2),
-                             getMatrixElement(matrix, 0, 0) );
-            thetaY = 0.0;
-        }
-    } else {
-        // Not a unique solution
-        thetaZ = atan2( getMatrixElement(matrix, 0, 2),
-                        getMatrixElement(matrix, 0, 0) );
-        thetaY = 0.0;
-    }
-}
-
 static void remapMatrix(const float inputMatrix[3*3],
                         const float mappingMatrix[4],
                         float outputMatrix[3*3])
diff --git a/src/plugins/sensors/blackberry/bbutil.cpp b/src/plugins/sensors/blackberry/bbutil.cpp
new file mode 100644
index 00000000..c01c6303
--- /dev/null
+++ b/src/plugins/sensors/blackberry/bbutil.cpp
@@ -0,0 +1,82 @@
+/****************************************************************************
+**
+** Copyright (C) 2012 Research In Motion
+** Contact: http://www.qt-project.org/
+**
+** This file is part of the QtSensors module of the Qt Toolkit.
+**
+** $QT_BEGIN_LICENSE:LGPL$
+** GNU Lesser General Public License Usage
+** This file may be used under the terms of the GNU Lesser General Public
+** License version 2.1 as published by the Free Software Foundation and
+** appearing in the file LICENSE.LGPL included in the packaging of this
+** file. Please review the following information to ensure the GNU Lesser
+** General Public License version 2.1 requirements will be met:
+** http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html.
+**
+** In addition, as a special exception, Nokia gives you certain additional
+** rights. These rights are described in the Nokia Qt LGPL Exception
+** version 1.1, included in the file LGPL_EXCEPTION.txt in this package.
+**
+** GNU General Public License Usage
+** Alternatively, this file may be used under the terms of the GNU General
+** Public License version 3.0 as published by the Free Software Foundation
+** and appearing in the file LICENSE.GPL included in the packaging of this
+** file. Please review the following information to ensure the GNU General
+** Public License version 3.0 requirements will be met:
+** http://www.gnu.org/copyleft/gpl.html.
+**
+** Other Usage
+** Alternatively, this file may be used in accordance with the terms and
+** conditions contained in a signed written agreement between you and Nokia.
+**
+**
+**
+**
+**
+**
+** $QT_END_LICENSE$
+**
+****************************************************************************/
+#include "bbutil.h"
+
+#include <QtCore/qmath.h>
+
+namespace BbUtil {
+
+static float getMatrixElement(const float matrix[3*3], int index0, int index1)
+{
+    return matrix[index0 * 3 + index1];
+}
+
+void matrixToEulerZXY(const float matrix[3*3],
+                      float &thetaX, float &thetaY, float& thetaZ)
+{
+    thetaX = asin( getMatrixElement(matrix, 2, 1));
+    if ( thetaX < M_PI_2 ) {
+        if ( thetaX > -M_PI_2 ) {
+            thetaZ = atan2( -getMatrixElement(matrix, 0, 1),
+                             getMatrixElement(matrix, 1, 1) );
+            thetaY = atan2( -getMatrixElement(matrix, 2, 0),
+                             getMatrixElement(matrix, 2, 2) );
+        } else {
+            // Not a unique solution
+            thetaZ = -atan2( getMatrixElement(matrix, 0, 2),
+                             getMatrixElement(matrix, 0, 0) );
+            thetaY = 0.0;
+        }
+    } else {
+        // Not a unique solution
+        thetaZ = atan2( getMatrixElement(matrix, 0, 2),
+                        getMatrixElement(matrix, 0, 0) );
+        thetaY = 0.0;
+    }
+}
+
+qreal radiansToDegrees(qreal radians)
+{
+    static const qreal radToDeg = 180.0f / M_PI;
+    return radians * radToDeg;
+}
+
+}
diff --git a/src/plugins/sensors/blackberry/bbutil.h b/src/plugins/sensors/blackberry/bbutil.h
index f360776f..86882400 100644
--- a/src/plugins/sensors/blackberry/bbutil.h
+++ b/src/plugins/sensors/blackberry/bbutil.h
@@ -41,14 +41,14 @@
 #ifndef BBUTIL_H
 #define BBUTIL_H
 
-#include <QtCore/qmath.h>
+#include <QtCore/qglobal.h>
+
+namespace BbUtil {
+
+void matrixToEulerZXY(const float matrix[3*3], float &thetaX, float &thetaY, float& thetaZ);
+
+qreal radiansToDegrees(qreal radians);
 
-namespace {
-qreal radiansToDegrees(qreal radians)
-{
-    static const qreal radToDeg = 180.0f / M_PI;
-    return radians * radToDeg;
-}
 }
 
 #endif
diff --git a/src/plugins/sensors/blackberry/blackberry.pro b/src/plugins/sensors/blackberry/blackberry.pro
index fecd8276..8f7c2f3b 100644
--- a/src/plugins/sensors/blackberry/blackberry.pro
+++ b/src/plugins/sensors/blackberry/blackberry.pro
@@ -40,6 +40,7 @@ SOURCES += bbsensorbackend.cpp \
     bbproximitysensor.cpp \
     bbrotationsensor.cpp \
     bbtemperaturesensor.cpp \
+    bbutil.cpp \
     main.cpp
 
 OTHER_FILES = plugin.json