Report orientation in a Python-compatible manner

Change-Id: Ifc0390439e93751ceb88977b14344576f919afa7
Reviewed-by: Laszlo Agocs <laszlo.agocs@theqtcompany.com>

2 files changed, 14 insertions, 1 deletions

diff --git a/README.md b/README.md
index 8801636..eafd59a 100644
--- a/README.md
+++ b/README.md
@@ -67,3 +67,6 @@ Raspbian's default calibration from /etc is picked up automatically, similarly t
 Python lib. Note however that this is a text file and numbers may not be parsable with
 locale settings that use a decimal separator other than the dot. If the orientation is
 reported as all-NaNs, check this first.
+
+Orientation maintains compatibility with Python's get_orientation(): the QVector3D's x, y,
+z components correspond to pitch, roll, yaw in degrees in range 0..360.
diff --git a/src/sensehat/qsensehatsensors.cpp b/src/sensehat/qsensehatsensors.cpp
index ce0e098..6f72cae 100644
--- a/src/sensehat/qsensehatsensors.cpp
+++ b/src/sensehat/qsensehatsensors.cpp
@@ -178,6 +178,12 @@ void QSenseHatSensorsPrivate::update(QSenseHatSensors::UpdateFlags what)
     }
 }
 
+static inline qreal toDeg360(qreal rad)
+{
+    const qreal deg = qRadiansToDegrees(rad);
+    return deg < 0 ? deg + 360 : deg;
+}
+
 void QSenseHatSensorsPrivate::report(const RTIMU_DATA &data, QSenseHatSensors::UpdateFlags what)
 {
     if (what.testFlag(QSenseHatSensors::UpdateHumidity)) {
@@ -224,7 +230,11 @@ void QSenseHatSensorsPrivate::report(const RTIMU_DATA &data, QSenseHatSensors::U
 
     if (what.testFlag(QSenseHatSensors::UpdateOrientation)) {
         if (data.fusionPoseValid) {
-            orientation = QVector3D(data.fusionPose.x(), data.fusionPose.y(), data.fusionPose.z());
+            // To be compatible with the Python lib's get_orientation(), report
+            // degrees in range 0..360 and in the order pitch, roll, yaw.
+            orientation = QVector3D(toDeg360(data.fusionPose.y()),
+                                    toDeg360(data.fusionPose.x()),
+                                    toDeg360(data.fusionPose.z()));
             emit q->orientationChanged(orientation);
         }
     }