Blackberry: Implement axes remapping for all axes-based sensors

The remapping code was moved to BbSensorBackend.

Change-Id: I6ede875594a2aaa3745b7deb4aa0ed64c2d9b855
Reviewed-by: Sean Harmer <sean.harmer@kdab.com>

8 files changed, 113 insertions, 76 deletions

diff --git a/src/plugins/sensors/blackberry/bbaccelerometer.cpp b/src/plugins/sensors/blackberry/bbaccelerometer.cpp
index ed4f51f6..a277c1dd 100644
--- a/src/plugins/sensors/blackberry/bbaccelerometer.cpp
+++ b/src/plugins/sensors/blackberry/bbaccelerometer.cpp
@@ -48,9 +48,13 @@ BbAccelerometer::BbAccelerometer(QSensor *sensor)
 
 bool BbAccelerometer::updateReadingFromEvent(const sensor_event_t &event, QAccelerometerReading *reading)
 {
-    reading->setX(event.motion.dsp.x);
-    reading->setY(event.motion.dsp.y);
-    reading->setZ(event.motion.dsp.z);
+    float x = event.motion.dsp.x;
+    float y = event.motion.dsp.y;
+    float z = event.motion.dsp.z;
+    remapAxes(&x, &y, &z);
+    reading->setX(x);
+    reading->setY(y);
+    reading->setZ(z);
     return true;
 }
 
diff --git a/src/plugins/sensors/blackberry/bbguihelper.h b/src/plugins/sensors/blackberry/bbguihelper.h
index 08c1de42..f0ee3dc0 100644
--- a/src/plugins/sensors/blackberry/bbguihelper.h
+++ b/src/plugins/sensors/blackberry/bbguihelper.h
@@ -44,8 +44,6 @@
 #include <QtCore/QAbstractNativeEventFilter>
 #include <QtCore/QObject>
 
-struct bps_event_t;
-
 // We can't depend on QtGui in this plugin, only on BPS.
 // This class provides replacements for some QtGui functions, implemented using BPS.
 class BbGuiHelper : public QObject, public QAbstractNativeEventFilter
diff --git a/src/plugins/sensors/blackberry/bbgyroscope.cpp b/src/plugins/sensors/blackberry/bbgyroscope.cpp
index 5280c5d6..829cd716 100644
--- a/src/plugins/sensors/blackberry/bbgyroscope.cpp
+++ b/src/plugins/sensors/blackberry/bbgyroscope.cpp
@@ -53,9 +53,13 @@ BbGyroscope::BbGyroscope(QSensor *sensor)
 
 bool BbGyroscope::updateReadingFromEvent(const sensor_event_t &event, QGyroscopeReading *reading)
 {
-    reading->setX(radiansToDegrees(event.motion.dsp.x));
-    reading->setY(radiansToDegrees(event.motion.dsp.y));
-    reading->setZ(radiansToDegrees(event.motion.dsp.z));
+    float x = radiansToDegrees(event.motion.dsp.x);
+    float y = radiansToDegrees(event.motion.dsp.y);
+    float z = radiansToDegrees(event.motion.dsp.z);
+    remapAxes(&x, &y, &z);
+    reading->setX(x);
+    reading->setY(y);
+    reading->setZ(z);
 
     return true;
 }
diff --git a/src/plugins/sensors/blackberry/bbmagnetometer.cpp b/src/plugins/sensors/blackberry/bbmagnetometer.cpp
index eed1497c..07426c8a 100644
--- a/src/plugins/sensors/blackberry/bbmagnetometer.cpp
+++ b/src/plugins/sensors/blackberry/bbmagnetometer.cpp
@@ -55,9 +55,13 @@ bool BbMagnetometer::updateReadingFromEvent(const sensor_event_t &event, QMagnet
 {
     // TODO: In the future, support returnGeoValues here. Right now, /dev/sensors/mag has no
     //       geomagnatic mode, but will gain it in the future.
-    reading->setX(convertValue(event.motion.dsp.x));
-    reading->setY(convertValue(event.motion.dsp.y));
-    reading->setZ(convertValue(event.motion.dsp.z));
+    float x = convertValue(event.motion.dsp.x);
+    float y = convertValue(event.motion.dsp.y);
+    float z = convertValue(event.motion.dsp.z);
+    remapAxes(&x, &y, &z);
+    reading->setX(x);
+    reading->setY(y);
+    reading->setZ(z);
 
     const bool returnGeoValues = sensor()->property("returnGeoValues").toBool();
     if (returnGeoValues) {
diff --git a/src/plugins/sensors/blackberry/bbrotationsensor.cpp b/src/plugins/sensors/blackberry/bbrotationsensor.cpp
index f64844ae..5701c279 100644
--- a/src/plugins/sensors/blackberry/bbrotationsensor.cpp
+++ b/src/plugins/sensors/blackberry/bbrotationsensor.cpp
@@ -42,7 +42,6 @@
 
 #include "bbguihelper.h"
 #include "bbutil.h"
-#include <QtCore/qmath.h>
 
 using namespace BbUtil;
 
@@ -50,8 +49,6 @@ BbRotationSensor::BbRotationSensor(QSensor *sensor)
     : BbSensorBackend<QRotationReading>(devicePath(), SENSOR_TYPE_ROTATION_MATRIX, sensor)
 {
     setDescription(QLatin1String("Device rotation in degrees"));
-    m_mappingMatrix[0] = m_mappingMatrix[2] = 1;
-    m_mappingMatrix[1] = m_mappingMatrix[3] = 0;
 }
 
 QString BbRotationSensor::devicePath()
@@ -59,13 +56,6 @@ QString BbRotationSensor::devicePath()
     return QLatin1String("/dev/sensor/rotMatrix");
 }
 
-void BbRotationSensor::setGuiHelper(BbGuiHelper *guiHelper)
-{
-    BbSensorBackend<QRotationReading>::setGuiHelper(guiHelper);
-    connect(guiHelper, SIGNAL(orientationChanged()), this, SLOT(updateOrientation()));
-    updateOrientation();
-}
-
 void BbRotationSensor::additionalDeviceInit()
 {
     addOutputRange(-180, 180, 0 /* ? */);
@@ -78,40 +68,15 @@ bool BbRotationSensor::addDefaultRange()
     return false;
 }
 
-static void remapMatrix(const float inputMatrix[3*3],
-                        const float mappingMatrix[4],
-                        float outputMatrix[3*3])
-{
-    int i,j,k;
-
-    for (i = 0; i < 3; i++) {
-        for (j = 0; j < 2; j++) { //only goto 2 because last column stays unchanged
-
-            outputMatrix[i*3+j] = 0;
-
-            for (k = 0; k < 2; k++) { //only goto 2 because we know rotation matrix is zero in bottom row
-                outputMatrix[i*3+j] += inputMatrix[i*3+k] * mappingMatrix[k*2+j];
-            }
-        }
-
-        outputMatrix[i*3+2] = inputMatrix[i*3+2];
-    }
-}
-
 bool BbRotationSensor::updateReadingFromEvent(const sensor_event_t &event, QRotationReading *reading)
 {
     // sensor_event_t has euler angles for a Z-Y'-X'' system, but the QtMobility API
     // uses Z-X'-Y''.
     // So extract the euler angles using the Z-X'-Y'' system from the matrix.
     float xRad, yRad, zRad;
-
-    if (isAutoAxisRemappingEnabled() && guiHelper()->currentOrientation() != 0) {
-        float mappedRotationMatrix[3*3];
-        remapMatrix(event.rotation_matrix, m_mappingMatrix, mappedRotationMatrix);
-        matrixToEulerZXY(mappedRotationMatrix, xRad, yRad, zRad);
-    } else {
-        matrixToEulerZXY(event.rotation_matrix, xRad, yRad, zRad);
-    }
+    float mappedRotationMatrix[3*3];
+    remapMatrix(event.rotation_matrix, mappedRotationMatrix);
+    matrixToEulerZXY(mappedRotationMatrix, xRad, yRad, zRad);
 
     reading->setFromEuler(radiansToDegrees(xRad),
                           radiansToDegrees(yRad),
@@ -124,20 +89,3 @@ qreal BbRotationSensor::convertValue(float bbValueInRad)
 {
     return radiansToDegrees(bbValueInRad);
 }
-
-bool BbRotationSensor::isAutoAxisRemappingEnabled() const
-{
-    return sensor()->property("automaticAxisRemapping").toBool();
-}
-
-void BbRotationSensor::updateOrientation()
-{
-    // ### I can't really test this, the rotation matrix has too many glitches and drifts over time,
-    // making any measurement quite hard
-    const int rotationAngle = guiHelper()->currentOrientation();
-
-    m_mappingMatrix[0] = cos(rotationAngle*M_PI/180);
-    m_mappingMatrix[1] = sin(rotationAngle*M_PI/180);
-    m_mappingMatrix[2] = -sin(rotationAngle*M_PI/180);
-    m_mappingMatrix[3] = cos(rotationAngle*M_PI/180);
-}
diff --git a/src/plugins/sensors/blackberry/bbrotationsensor.h b/src/plugins/sensors/blackberry/bbrotationsensor.h
index 6fb7f7a0..361bce80 100644
--- a/src/plugins/sensors/blackberry/bbrotationsensor.h
+++ b/src/plugins/sensors/blackberry/bbrotationsensor.h
@@ -53,20 +53,11 @@ public:
 
     static QString devicePath();
 
-    void setGuiHelper(BbGuiHelper *guiHelper) Q_DECL_OVERRIDE;
-
 protected:
     void additionalDeviceInit() Q_DECL_OVERRIDE;
     bool addDefaultRange() Q_DECL_OVERRIDE;
     qreal convertValue(float bbValue) Q_DECL_OVERRIDE;
     bool updateReadingFromEvent(const sensor_event_t &event, QRotationReading *reading) Q_DECL_OVERRIDE;
-    bool isAutoAxisRemappingEnabled() const;
-
-private slots:
-    void updateOrientation();
-
-private:
-    float m_mappingMatrix[4];
 };
 
 #endif
diff --git a/src/plugins/sensors/blackberry/bbsensorbackend.cpp b/src/plugins/sensors/blackberry/bbsensorbackend.cpp
index 8c53aa26..a64dcdfe 100644
--- a/src/plugins/sensors/blackberry/bbsensorbackend.cpp
+++ b/src/plugins/sensors/blackberry/bbsensorbackend.cpp
@@ -42,6 +42,7 @@
 
 #include "bbguihelper.h"
 #include <QtCore/QDebug>
+#include <QtCore/qmath.h>
 #include <fcntl.h>
 
 static const int microSecondsPerSecond = 1000 * 1000;
@@ -57,10 +58,32 @@ static uint hertzToMicroSeconds(int hertz)
     return microSecondsPerSecond / hertz;
 }
 
+static void remapMatrix(const float inputMatrix[3*3],
+                        const float mappingMatrix[4],
+                        float outputMatrix[3*3])
+{
+    int i,j,k;
+
+    for (i = 0; i < 3; i++) {
+        for (j = 0; j < 2; j++) { //only goto 2 because last column stays unchanged
+
+            outputMatrix[i*3+j] = 0;
+
+            for (k = 0; k < 2; k++) { //only goto 2 because we know rotation matrix is zero in bottom row
+                outputMatrix[i*3+j] += inputMatrix[i*3+k] * mappingMatrix[k*2+j];
+            }
+        }
+
+        outputMatrix[i*3+2] = inputMatrix[i*3+2];
+    }
+}
+
 BbSensorBackendBase::BbSensorBackendBase(const QString &devicePath, sensor_type_e sensorType,
                                          QSensor *sensor)
     : QSensorBackend(sensor), m_deviceFile(devicePath), m_sensorType(sensorType), m_guiHelper(0)
 {
+    m_mappingMatrix[0] = m_mappingMatrix[3] = 1;
+    m_mappingMatrix[1] = m_mappingMatrix[2] = 0;
     connect(sensor, SIGNAL(alwaysOnChanged()), this, SLOT(applyAlwaysOnProperty()));
 
     // Set some sensible default values
@@ -116,6 +139,8 @@ void BbSensorBackendBase::setGuiHelper(BbGuiHelper *guiHelper)
     Q_ASSERT(!m_guiHelper);
     m_guiHelper = guiHelper;
     connect(m_guiHelper, SIGNAL(applicationActiveChanged()), this, SLOT(updatePauseState()));
+    connect(guiHelper, SIGNAL(orientationChanged()), this, SLOT(updateOrientation()));
+    updateOrientation();
 }
 
 void BbSensorBackendBase::additionalDeviceInit()
@@ -132,6 +157,48 @@ qreal BbSensorBackendBase::convertValue(float bbValue)
     return bbValue;
 }
 
+bool BbSensorBackendBase::isAutoAxisRemappingEnabled() const
+{
+    return sensor()->property("automaticAxisRemapping").toBool();
+}
+
+void BbSensorBackendBase::remapMatrix(const float inputMatrix[], float outputMatrix[])
+{
+    if (!isAutoAxisRemappingEnabled() || m_guiHelper->currentOrientation() == 0) {
+        memcpy(outputMatrix, inputMatrix, sizeof(float) * 9);
+        return;
+    }
+
+    ::remapMatrix(inputMatrix, m_mappingMatrix, outputMatrix);
+}
+
+void BbSensorBackendBase::remapAxes(float *x, float *y, float *z)
+{
+    Q_ASSERT(x && y && z);
+    if (!isAutoAxisRemappingEnabled() || m_guiHelper->currentOrientation() == 0)
+        return;
+
+    const int angle = m_guiHelper->currentOrientation();
+
+    const float oldX = *x;
+    const float oldY = *y;
+
+    switch (angle) {
+    case 90:
+        *x = -oldY;
+        *y = oldX;
+    break;
+    case 180:
+        *x = -oldX;
+        *y = -oldY;
+    break;
+    case 270:
+        *x = oldY;
+        *y = -oldX;
+    break;
+    }
+}
+
 void BbSensorBackendBase::start()
 {
     Q_ASSERT(m_guiHelper);
@@ -267,3 +334,15 @@ void BbSensorBackendBase::updatePauseState()
 {
     setPaused(!sensor()->isAlwaysOn() && !m_guiHelper->applicationActive());
 }
+
+void BbSensorBackendBase::updateOrientation()
+{
+    // ### I can't really test this, the rotation matrix has too many glitches and drifts over time,
+    // making any measurement quite hard
+    const int rotationAngle = guiHelper()->currentOrientation();
+
+    m_mappingMatrix[0] = cos(rotationAngle*M_PI/180);
+    m_mappingMatrix[1] = sin(rotationAngle*M_PI/180);
+    m_mappingMatrix[2] = -sin(rotationAngle*M_PI/180);
+    m_mappingMatrix[3] = cos(rotationAngle*M_PI/180);
+}
diff --git a/src/plugins/sensors/blackberry/bbsensorbackend.h b/src/plugins/sensors/blackberry/bbsensorbackend.h
index 3a5a99dc..030f28ba 100644
--- a/src/plugins/sensors/blackberry/bbsensorbackend.h
+++ b/src/plugins/sensors/blackberry/bbsensorbackend.h
@@ -66,7 +66,7 @@ public:
     BbSensorBackendBase(const QString &devicePath, sensor_type_e sensorType, QSensor *sensor);
 
     void initSensorInfo();
-    virtual void setGuiHelper(BbGuiHelper *guiHelper);
+    void setGuiHelper(BbGuiHelper *guiHelper);
 
     void start() Q_DECL_OVERRIDE;
     void stop() Q_DECL_OVERRIDE;
@@ -92,6 +92,13 @@ protected:
     // QtSensors expects tesla. This function would therefore convert from microtesla to tesla.
     virtual qreal convertValue(float bbValue);
 
+    bool isAutoAxisRemappingEnabled() const;
+
+    // These functions will automatically remap the matrix or the axes if auto axes remapping is
+    // enabled
+    void remapMatrix(const float inputMatrix[3*3], float outputMatrix[3*3]);
+    void remapAxes(float *x, float *y, float *z);
+
     virtual void processEvent(const sensor_event_t &sensorEvent) = 0;
 
 private slots:
@@ -99,12 +106,14 @@ private slots:
     void applyAlwaysOnProperty();
     void setPaused(bool paused);
     void updatePauseState();
+    void updateOrientation();
 
 private:
     QFile m_deviceFile;
     QScopedPointer<QSocketNotifier> m_socketNotifier;
     sensor_type_e m_sensorType;
     BbGuiHelper *m_guiHelper;
+    float m_mappingMatrix[4];
 };
 
 template<class SensorReading>