Add orientation (fusion pose)

Change-Id: I1eee8aa153d45b4dd651db616bf97b30cf4a5a68
Reviewed-by: Andy Nichols <andy.nichols@theqtcompany.com>

3 files changed, 23 insertions, 1 deletions

diff --git a/examples/sensehat/sensors/main.cpp b/examples/sensehat/sensors/main.cpp
index f2c6399..87968cf 100644
--- a/examples/sensehat/sensors/main.cpp
+++ b/examples/sensehat/sensors/main.cpp
@@ -70,6 +70,9 @@ int main(int argc, char **argv)
     QObject::connect(&sensors, &QSenseHatSensors::compassChanged, [](const QVector3D &v) {
         qDebug() << "Compass:" << v;
     });
+    QObject::connect(&sensors, &QSenseHatSensors::orientationChanged, [](const QVector3D &v) {
+        qDebug() << "Orientation:" << v;
+    });
 
     QTimer::singleShot(10000, &app, &QCoreApplication::quit);
 
diff --git a/src/sensehat/qsensehatsensors.cpp b/src/sensehat/qsensehatsensors.cpp
index 2ef6c9d..ff44437 100644
--- a/src/sensehat/qsensehatsensors.cpp
+++ b/src/sensehat/qsensehatsensors.cpp
@@ -77,6 +77,7 @@ public:
     QVector3D gyro;
     QVector3D acceleration;
     QVector3D compass;
+    QVector3D orientation;
 };
 
 QSenseHatSensorsPrivate::~QSenseHatSensorsPrivate()
@@ -150,7 +151,8 @@ void QSenseHatSensorsPrivate::update(QSenseHatSensors::UpdateFlags what)
             qWarning("Failed to read pressure data");
     }
 
-    const int imuFlags = QSenseHatSensors::UpdateGyro | QSenseHatSensors::UpdateAcceleration | QSenseHatSensors::UpdateCompass;
+    const int imuFlags = QSenseHatSensors::UpdateGyro | QSenseHatSensors::UpdateAcceleration
+            | QSenseHatSensors::UpdateCompass | QSenseHatSensors::UpdateOrientation;
     if (what & imuFlags) {
         if (!imuInited) {
             imuInited = true;
@@ -213,6 +215,13 @@ void QSenseHatSensorsPrivate::report(const RTIMU_DATA &data, QSenseHatSensors::U
             emit q->compassChanged(compass);
         }
     }
+
+    if (what.testFlag(QSenseHatSensors::UpdateOrientation)) {
+        if (data.fusionPoseValid) {
+            orientation = QVector3D(data.fusionPose.x(), data.fusionPose.y(), data.fusionPose.z());
+            emit q->orientationChanged(orientation);
+        }
+    }
 }
 
 QSenseHatSensors::QSenseHatSensors(InitFlags flags)
@@ -281,4 +290,10 @@ QVector3D QSenseHatSensors::compass() const
     return d->compass;
 }
 
+QVector3D QSenseHatSensors::orientation() const
+{
+    Q_D(const QSenseHatSensors);
+    return d->orientation;
+}
+
 QT_END_NAMESPACE
diff --git a/src/sensehat/qsensehatsensors.h b/src/sensehat/qsensehatsensors.h
index fb1cd23..299e1fa 100644
--- a/src/sensehat/qsensehatsensors.h
+++ b/src/sensehat/qsensehatsensors.h
@@ -55,6 +55,7 @@ class QSENSEHAT_EXPORT QSenseHatSensors : public QObject
     Q_PROPERTY(QVector3D gyro READ gyro NOTIFY gyroChanged)
     Q_PROPERTY(QVector3D acceleration READ acceleration NOTIFY accelerationChanged)
     Q_PROPERTY(QVector3D compass READ compass NOTIFY compassChanged)
+    Q_PROPERTY(QVector3D orientation READ orientation NOTIFY orientationChanged)
 
 public:
     enum InitFlag {
@@ -69,6 +70,7 @@ public:
         UpdateGyro = 0x08,
         UpdateAcceleration = 0x10,
         UpdateCompass = 0x20,
+        UpdateOrientation = 0x40,
         UpdateAll = 0xFF
     };
     Q_DECLARE_FLAGS(UpdateFlags, UpdateFlag)
@@ -85,6 +87,7 @@ public:
     QVector3D gyro() const;
     QVector3D acceleration() const;
     QVector3D compass() const;
+    QVector3D orientation() const;
 
 signals:
     void humidityChanged(qreal value);
@@ -93,6 +96,7 @@ signals:
     void gyroChanged(const QVector3D &value);
     void accelerationChanged(const QVector3D &value);
     void compassChanged(const QVector3D &value);
+    void orientationChanged(const QVector3D &value);
 
 private:
     Q_DISABLE_COPY(QSenseHatSensors)