From 1f204c48c5f9e3237fd158199e58e3d156a5a70d Mon Sep 17 00:00:00 2001 From: Laszlo Agocs Date: Sat, 21 Nov 2015 17:16:05 +0100 Subject: Report orientation in a Python-compatible manner Change-Id: Ifc0390439e93751ceb88977b14344576f919afa7 Reviewed-by: Laszlo Agocs --- README.md | 3 +++ src/sensehat/qsensehatsensors.cpp | 12 +++++++++++- 2 files changed, 14 insertions(+), 1 deletion(-) 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); } } -- cgit v1.2.3