Use of hybris rotation vectors for compass

adaptors/adaptors.pro

@@ -10,6 +10,8 @@ contains(CONFIG,hybris) {
     SUBDIRS += hybrispressureadaptor
     SUBDIRS += hybrisproximityadaptor
     SUBDIRS += hybrisorientationadaptor
+    SUBDIRS += hybrisrotationadaptor
+    SUBDIRS += hybrisgeorotationadaptor
     SUBDIRS += hybrisstepcounteradaptor
 
     } else {
@@ -55,6 +57,8 @@ config_hybris {
     SUBDIRS += hybrispressureadaptor
     SUBDIRS += hybrisproximityadaptor
     SUBDIRS += hybrisorientationadaptor
+    SUBDIRS += hybrisrotationadaptor
+    SUBDIRS += hybrisgeorotationadaptor
     SUBDIRS += hybrisstepcounteradaptor
  }
 }

adaptors/hybrisgeorotationadaptor/hybrisgeorotationadaptor.cpp

@@ -0,0 +1,119 @@
+/****************************************************************************
+**
+** Copyright (C) 2013 Jolla Ltd
+** Contact: lorn.potter@jollamobile.com
+**
+** Copyright (C) 2020 Rinigus
+**
+**
+** $QT_BEGIN_LICENSE:LGPL$
+**
+** GNU Lesser General Public License Usage
+** Alternatively, 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.
+**
+** $QT_END_LICENSE$
+**
+****************************************************************************/
+
+#include "hybrisgeorotationadaptor.h"
+#include "logging.h"
+#include "datatypes/utils.h"
+#include "config.h"
+
+#include <QtCore/qmath.h>
+#include <QtGlobal>
+
+#define RADIANS_TO_DEGREES 57.2957795
+
+/*
+ * azimuth: angle between the magnetic north direction and the Y axis, around
+ * the Z axis (0<=azimuth<360).
+ * 0=North, 90=East, 180=South, 270=West
+ *
+ **/
+
+HybrisGeoRotationAdaptor::HybrisGeoRotationAdaptor(const QString& id) :
+    HybrisAdaptor(id,SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR)
+{
+    m_buffer = new DeviceAdaptorRingBuffer<CompassData>(1);
+    setAdaptedSensor("hybrisgeorotation", "Internal rotation coordinates using geo rotation vector", m_buffer);
+
+    setDescription("Hybris georotation");
+    m_powerStatePath = SensorFrameworkConfig::configuration()->value("georotation/powerstate_path").toByteArray();
+    if (!m_powerStatePath.isEmpty() && !QFile::exists(m_powerStatePath)) {
+        sensordLogW() << "Path does not exists: " << m_powerStatePath;
+        m_powerStatePath.clear();
+    }
+}
+
+HybrisGeoRotationAdaptor::~HybrisGeoRotationAdaptor()
+{
+    delete m_buffer;
+}
+
+bool HybrisGeoRotationAdaptor::startSensor()
+{
+    if (!(HybrisAdaptor::startSensor()))
+        return false;
+    if (isRunning() && !m_powerStatePath.isEmpty())
+        writeToFile(m_powerStatePath, "1");
+    sensordLogD() << "Hybris GeoRotationAdaptor start";
+    return true;
+}
+
+void HybrisGeoRotationAdaptor::stopSensor()
+{
+    HybrisAdaptor::stopSensor();
+    if (!isRunning() && !m_powerStatePath.isEmpty())
+        writeToFile(m_powerStatePath, "0");
+    sensordLogD() << "Hybris GeoRotationAdaptor stop";
+}
+
+void HybrisGeoRotationAdaptor::processSample(const sensors_event_t& data)
+{
+    CompassData *d = m_buffer->nextSlot();
+    d->timestamp_ = quint64(data.timestamp * .001);
+#ifdef USE_BINDER
+    const float *rotationVector = data.u.data;
+#else
+    const float *rotationVector = data.data;
+#endif
+    // Calculations are based on Android methods
+    // getRotationMatrixFromVector and getOrientation
+    // of SensorManager.java
+    float q0 = rotationVector[3];
+    float q1 = rotationVector[0];
+    float q2 = rotationVector[1];
+    float q3 = rotationVector[2];
+    float accuracy = rotationVector[4];
+
+    float sq_q1 = 2 * q1 * q1;
+    float sq_q3 = 2 * q3 * q3;
+    float q1_q2 = 2 * q1 * q2;
+    float q3_q0 = 2 * q3 * q0;
+
+    float R1 = q1_q2 - q3_q0;
+    float R4 = 1 - sq_q1 - sq_q3;
+    float azimuth = qAtan2(R1, R4) * RADIANS_TO_DEGREES;
+    d->degrees_ = (int)(azimuth + 360) % 360;
+    d->rawDegrees_ = d->degrees_;
+
+    // level_ is set to 3 (pass csd) when accuracy is higher than 10 degrees
+    const int maxLevel = 3;
+    if (accuracy < 0) d->level_ = 0;
+    else d->level_ = (accuracy > 1e-5) ?
+           (int)(qMin(qFloor(maxLevel * 0.174533/accuracy), maxLevel)) :
+           maxLevel;
+
+    m_buffer->commit();
+    m_buffer->wakeUpReaders();
+}
+
+void HybrisGeoRotationAdaptor::init()
+{
+}

adaptors/hybrisgeorotationadaptor/hybrisgeorotationadaptor.h

@@ -0,0 +1,60 @@
+/****************************************************************************
+**
+** Copyright (C) 2013 Jolla Ltd
+** Contact: lorn.potter@jollamobile.com
+**
+**
+** $QT_BEGIN_LICENSE:LGPL$
+**
+** GNU Lesser General Public License Usage
+** Alternatively, 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.
+**
+** $QT_END_LICENSE$
+**
+****************************************************************************/
+
+#ifndef HYBRISGEOROTATIONADAPTOR_H
+#define HYBRISGEOROTATIONADAPTOR_H
+#include "hybrisadaptor.h"
+
+#include <QString>
+#include <QStringList>
+#include "deviceadaptorringbuffer.h"
+#include "datatypes/orientationdata.h"
+
+/**
+ * @brief Adaptor for hybris compass.
+ *
+ * Adaptor for internal compass
+ * Uses hybris sensor daemon driver interface in interval
+ * polling mode, i.e. values are read with given constant interval.
+ */
+class HybrisGeoRotationAdaptor : public HybrisAdaptor
+{
+    Q_OBJECT
+
+public:
+    static DeviceAdaptor* factoryMethod(const QString& id) {
+        return new HybrisGeoRotationAdaptor(id);
+    }
+    HybrisGeoRotationAdaptor(const QString& id);
+    ~HybrisGeoRotationAdaptor();
+
+    bool startSensor();
+    void stopSensor();
+
+protected:
+    void processSample(const sensors_event_t& data);
+    void init();
+
+private:
+    DeviceAdaptorRingBuffer<CompassData>* m_buffer;
+    QByteArray m_powerStatePath;
+
+};
+#endif

adaptors/hybrisgeorotationadaptor/hybrisgeorotationadaptor.pro

@@ -0,0 +1,14 @@
+TARGET       = hybrisgeorotationadaptor
+
+HEADERS += hybrisgeorotationadaptor.h \
+           hybrisgeorotationadaptorplugin.h
+
+SOURCES += hybrisgeorotationadaptor.cpp \
+           hybrisgeorotationadaptorplugin.cpp
+
+LIBS+= -L../../core -lhybrissensorfw-qt5
+
+include( ../adaptor-config.pri )
+config_hybris {
+    PKGCONFIG += android-headers
+}

adaptors/hybrisgeorotationadaptor/hybrisgeorotationadaptorplugin.cpp

@@ -0,0 +1,31 @@
+/****************************************************************************
+**
+** Copyright (C) 2013 Jolla Ltd
+** Contact: lorn.potter@jollamobile.com
+**
+**
+** $QT_BEGIN_LICENSE:LGPL$
+**
+** GNU Lesser General Public License Usage
+** Alternatively, 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.
+**
+** $QT_END_LICENSE$
+**
+****************************************************************************/
+
+#include "hybrisgeorotationadaptorplugin.h"
+#include "hybrisgeorotationadaptor.h"
+#include "sensormanager.h"
+#include "logging.h"
+
+void HybrisGeoRotationAdaptorPlugin::Register(class Loader&)
+{
+    sensordLogD() << "registering hybrisgeorotationadaptor";
+    SensorManager& sm = SensorManager::instance();
+    sm.registerDeviceAdaptor<HybrisGeoRotationAdaptor>("orientationadaptor");
+}

adaptors/hybrisgeorotationadaptor/hybrisgeorotationadaptorplugin.h

@@ -0,0 +1,35 @@
+/****************************************************************************
+**
+** Copyright (C) 2013 Jolla Ltd
+** Contact: lorn.potter@jollamobile.com
+**
+**
+** $QT_BEGIN_LICENSE:LGPL$
+**
+** GNU Lesser General Public License Usage
+** Alternatively, 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.
+**
+** $QT_END_LICENSE$
+**
+****************************************************************************/
+
+#ifndef HYBRISGEOROTATIONADAPTORPLUGIN_H
+#define HYBRISGEOROTATIONADAPTORPLUGIN_H
+
+#include "plugin.h"
+
+class HybrisGeoRotationAdaptorPlugin : public Plugin
+{
+    Q_OBJECT
+    Q_PLUGIN_METADATA(IID "com.nokia.SensorService.Plugin/1.0")
+
+private:
+    void Register(class Loader& l);
+};
+
+#endif

adaptors/hybrisrotationadaptor/hybrisrotationadaptor.cpp

@@ -0,0 +1,117 @@
+/****************************************************************************
+**
+** Copyright (C) 2013 Jolla Ltd
+** Contact: lorn.potter@jollamobile.com
+**
+** Copyright (C) 2020 Rinigus
+**
+**
+** $QT_BEGIN_LICENSE:LGPL$
+**
+** GNU Lesser General Public License Usage
+** Alternatively, 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.
+**
+** $QT_END_LICENSE$
+**
+****************************************************************************/
+
+#include "hybrisrotationadaptor.h"
+#include "logging.h"
+#include "datatypes/utils.h"
+#include "config.h"
+
+#include <QtCore/qmath.h>
+#include <QtGlobal>
+
+#define RADIANS_TO_DEGREES 57.2957795
+
+/*
+ * azimuth: angle between the magnetic north direction and the Y axis, around
+ * the Z axis (0<=azimuth<360).
+ * 0=North, 90=East, 180=South, 270=West
+ *
+ **/
+
+HybrisRotationAdaptor::HybrisRotationAdaptor(const QString& id) :
+    HybrisAdaptor(id,SENSOR_TYPE_ROTATION_VECTOR)
+{
+    m_buffer = new DeviceAdaptorRingBuffer<CompassData>(1);
+    setAdaptedSensor("hybrisrotation", "Internal rotation coordinates", m_buffer);
+
+    setDescription("Hybris rotation");
+    m_powerStatePath = SensorFrameworkConfig::configuration()->value("rotation/powerstate_path").toByteArray();
+    if (!m_powerStatePath.isEmpty() && !QFile::exists(m_powerStatePath)) {
+        sensordLogW() << "Path does not exists: " << m_powerStatePath;
+        m_powerStatePath.clear();
+    }
+}
+
+HybrisRotationAdaptor::~HybrisRotationAdaptor()
+{
+    delete m_buffer;
+}
+
+bool HybrisRotationAdaptor::startSensor()
+{
+    if (!(HybrisAdaptor::startSensor()))
+        return false;
+    if (isRunning() && !m_powerStatePath.isEmpty())
+        writeToFile(m_powerStatePath, "1");
+    sensordLogD() << "Hybris RotationAdaptor start";
+    return true;
+}
+
+void HybrisRotationAdaptor::stopSensor()
+{
+    HybrisAdaptor::stopSensor();
+    if (!isRunning() && !m_powerStatePath.isEmpty())
+        writeToFile(m_powerStatePath, "0");
+    sensordLogD() << "Hybris RotationAdaptor stop";
+}
+
+void HybrisRotationAdaptor::processSample(const sensors_event_t& data)
+{
+    CompassData *d = m_buffer->nextSlot();
+    d->timestamp_ = quint64(data.timestamp * .001);
+#ifdef USE_BINDER
+    const float *rotationVector = data.u.data;
+#else
+    const float *rotationVector = data.data;
+#endif
+    // Calculations are based on Android methods
+    // getRotationMatrixFromVector and getOrientation
+    // of SensorManager.java
+    float q0 = rotationVector[3];
+    float q1 = rotationVector[0];
+    float q2 = rotationVector[1];
+    float q3 = rotationVector[2];
+    float accuracy = rotationVector[4];
+
+    float sq_q1 = 2 * q1 * q1;
+    float sq_q3 = 2 * q3 * q3;
+    float q1_q2 = 2 * q1 * q2;
+    float q3_q0 = 2 * q3 * q0;
+
+    float R1 = q1_q2 - q3_q0;
+    float R4 = 1 - sq_q1 - sq_q3;
+    float azimuth = qAtan2(R1, R4) * RADIANS_TO_DEGREES;
+    d->degrees_ = (int)(azimuth + 360) % 360;
+    d->rawDegrees_ = d->degrees_;
+
+    // level_ is set to 3 (pass csd) when accuracy is higher than 10 degrees
+    const int maxLevel = 3;
+    if (accuracy < 0) d->level_ = 0;
+    else d->level_ = (accuracy > 1e-5) ? (int)(qMin(qFloor(maxLevel * 0.174533/accuracy), maxLevel)) : maxLevel;
+
+    m_buffer->commit();
+    m_buffer->wakeUpReaders();
+}
+
+void HybrisRotationAdaptor::init()
+{
+}