From 1dfa6732e026c69472c821d83346cc66bcf3e476 Mon Sep 17 00:00:00 2001 From: Thomas McGuire Date: Thu, 19 Jul 2012 13:22:16 +0200 Subject: 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 Reviewed-by: Adam Parco Reviewed-by: Lorn Potter --- src/plugins/sensors/blackberry/bbcompass.cpp | 16 ++++- src/plugins/sensors/blackberry/bbgyroscope.cpp | 2 + .../sensors/blackberry/bbrotationsensor.cpp | 31 +------- src/plugins/sensors/blackberry/bbutil.cpp | 82 ++++++++++++++++++++++ src/plugins/sensors/blackberry/bbutil.h | 14 ++-- src/plugins/sensors/blackberry/blackberry.pro | 1 + 6 files changed, 107 insertions(+), 39 deletions(-) create mode 100644 src/plugins/sensors/blackberry/bbutil.cpp (limited to 'src/plugins') 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(devicePath(), SENSOR_TYPE_AZIMUTH_PITCH_ROLL, sensor) + : BbSensorBackend(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 +using namespace BbUtil; + BbGyroscope::BbGyroscope(QSensor *sensor) : BbSensorBackend(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 #include +using namespace BbUtil; + BbRotationSensor::BbRotationSensor(QSensor *sensor) : BbSensorBackend(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 + +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 +#include + +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 -- cgit v1.2.3