1/****************************************************************************
2**
3** Copyright (C) 2016 The Qt Company Ltd.
4** Contact: https://www.qt.io/licensing/
5**
6** This file is part of the QtSensors module of the Qt Toolkit.
7**
8** $QT_BEGIN_LICENSE:LGPL$
9** Commercial License Usage
10** Licensees holding valid commercial Qt licenses may use this file in
11** accordance with the commercial license agreement provided with the
12** Software or, alternatively, in accordance with the terms contained in
13** a written agreement between you and The Qt Company. For licensing terms
14** and conditions see https://www.qt.io/terms-conditions. For further
15** information use the contact form at https://www.qt.io/contact-us.
16**
17** GNU Lesser General Public License Usage
18** Alternatively, this file may be used under the terms of the GNU Lesser
19** General Public License version 3 as published by the Free Software
20** Foundation and appearing in the file LICENSE.LGPL3 included in the
21** packaging of this file. Please review the following information to
22** ensure the GNU Lesser General Public License version 3 requirements
23** will be met: https://www.gnu.org/licenses/lgpl-3.0.html.
24**
25** GNU General Public License Usage
26** Alternatively, this file may be used under the terms of the GNU
27** General Public License version 2.0 or (at your option) the GNU General
28** Public license version 3 or any later version approved by the KDE Free
29** Qt Foundation. The licenses are as published by the Free Software
30** Foundation and appearing in the file LICENSE.GPL2 and LICENSE.GPL3
31** included in the packaging of this file. Please review the following
32** information to ensure the GNU General Public License requirements will
33** be met: https://www.gnu.org/licenses/gpl-2.0.html and
34** https://www.gnu.org/licenses/gpl-3.0.html.
35**
36** $QT_END_LICENSE$
37**
38****************************************************************************/
39
40#include "generictiltsensor.h"
41#include <QDebug>
42#include <qmath.h>
43
44char const * const GenericTiltSensor::id("generic.tilt");
45
46GenericTiltSensor::GenericTiltSensor(QSensor *sensor)
47 : QSensorBackend(sensor)
48 , radAccuracy(qDegreesToRadians(degrees: qreal(1)))
49 , pitch(0)
50 , roll(0)
51 , calibratedPitch(0)
52 , calibratedRoll(0)
53 , xRotation(0)
54 , yRotation(0)
55{
56 accelerometer = new QAccelerometer(this);
57 accelerometer->addFilter(filter: this);
58 accelerometer->connectToBackend();
59
60 setReading<QTiltReading>(&m_reading);
61 setDataRates(accelerometer);
62}
63
64void GenericTiltSensor::start()
65{
66 accelerometer->setDataRate(sensor()->dataRate());
67 accelerometer->setAlwaysOn(sensor()->isAlwaysOn());
68 accelerometer->start();
69 if (!accelerometer->isActive())
70 sensorStopped();
71 if (accelerometer->isBusy())
72 sensorBusy();
73}
74
75void GenericTiltSensor::stop()
76{
77 accelerometer->stop();
78}
79
80/*
81 Angle between Ground and X
82*/
83static inline qreal calcPitch(double Ax, double Ay, double Az)
84{
85 return qAtan2(y: -Ax, x: qSqrt(v: Ay * Ay + Az * Az));
86}
87
88/*
89 Angle between Ground and Y
90*/
91static inline qreal calcRoll(double /*Ax*/, double Ay, double Az)
92{
93 return qAtan2(y: Ay, x: Az);
94}
95
96void GenericTiltSensor::calibrate()
97{
98 calibratedPitch = pitch;
99 calibratedRoll = roll;
100}
101
102bool GenericTiltSensor::filter(QAccelerometerReading *reading)
103{
104 /*
105 z y
106 | /
107 |/___ x
108 */
109
110 qreal ax = reading->x();
111 qreal ay = reading->y();
112 qreal az = reading->z();
113#ifdef LOGCALIBRATION
114 qDebug() << "------------ new value -----------";
115 qDebug() << "old _pitch: " << pitch;
116 qDebug() << "old _roll: " << roll;
117 qDebug() << "_calibratedPitch: " << calibratedPitch;
118 qDebug() << "_calibratedRoll: " << calibratedRoll;
119#endif
120 pitch = calcPitch(Ax: ax, Ay: ay, Az: az);
121 roll = calcRoll (ax, Ay: ay, Az: az);
122#ifdef LOGCALIBRATION
123 qDebug() << "_pitch: " << pitch;
124 qDebug() << "_roll: " << roll;
125#endif
126 qreal xrot = roll - calibratedRoll;
127 qreal yrot = pitch - calibratedPitch;
128 //get angle between 0 and 180 or 0 -180
129 xrot = qAtan2(y: qSin(v: xrot), x: qCos(v: xrot));
130 yrot = qAtan2(y: qSin(v: yrot), x: qCos(v: yrot));
131
132#ifdef LOGCALIBRATION
133 qDebug() << "new xrot: " << xrot;
134 qDebug() << "new yrot: " << yrot;
135 qDebug() << "----------------------------------";
136#endif
137 qreal dxrot = qRadiansToDegrees(radians: xrot) - xRotation;
138 qreal dyrot = qRadiansToDegrees(radians: yrot) - yRotation;
139 if (dxrot < 0) dxrot = -dxrot;
140 if (dyrot < 0) dyrot = -dyrot;
141
142 bool setNewReading = false;
143 if (dxrot >= qRadiansToDegrees(radians: radAccuracy) || !sensor()->skipDuplicates()) {
144 xRotation = qRadiansToDegrees(radians: xrot);
145 setNewReading = true;
146 }
147 if (dyrot >= qRadiansToDegrees(radians: radAccuracy) || !sensor()->skipDuplicates()) {
148 yRotation = qRadiansToDegrees(radians: yrot);
149 setNewReading = true;
150 }
151
152 if (setNewReading || m_reading.timestamp() == 0) {
153 m_reading.setTimestamp(reading->timestamp());
154 m_reading.setXRotation(xRotation);
155 m_reading.setYRotation(yRotation);
156 newReadingAvailable();
157 }
158
159 return false;
160}
161
162bool GenericTiltSensor::isFeatureSupported(QSensor::Feature feature) const
163{
164 return (feature == QSensor::SkipDuplicates);
165}
166

source code of qtsensors/src/plugins/sensors/generic/generictiltsensor.cpp