1// Copyright (C) 2016 The Qt Company Ltd.
2// SPDX-License-Identifier: LicenseRef-Qt-Commercial OR LGPL-3.0-only OR GPL-2.0-only OR GPL-3.0-only
3
4#include "generictiltsensor.h"
5#include <QDebug>
6#include <qmath.h>
7
8char const * const GenericTiltSensor::id("generic.tilt");
9
10GenericTiltSensor::GenericTiltSensor(QSensor *sensor)
11 : QSensorBackend(sensor)
12 , radAccuracy(qDegreesToRadians(degrees: qreal(1)))
13 , pitch(0)
14 , roll(0)
15 , calibratedPitch(0)
16 , calibratedRoll(0)
17 , xRotation(0)
18 , yRotation(0)
19{
20 accelerometer = new QAccelerometer(this);
21 accelerometer->addFilter(filter: this);
22 accelerometer->connectToBackend();
23
24 setReading<QTiltReading>(&m_reading);
25 setDataRates(accelerometer);
26}
27
28void GenericTiltSensor::start()
29{
30 accelerometer->setDataRate(sensor()->dataRate());
31 accelerometer->setAlwaysOn(sensor()->isAlwaysOn());
32 accelerometer->start();
33 if (!accelerometer->isActive())
34 sensorStopped();
35 if (accelerometer->isBusy())
36 sensorBusy();
37}
38
39void GenericTiltSensor::stop()
40{
41 accelerometer->stop();
42}
43
44/*
45 Angle between Ground and X
46*/
47static inline qreal calcPitch(double Ax, double Ay, double Az)
48{
49 return qAtan2(y: -Ax, x: qSqrt(v: Ay * Ay + Az * Az));
50}
51
52/*
53 Angle between Ground and Y
54*/
55static inline qreal calcRoll(double /*Ax*/, double Ay, double Az)
56{
57 return qAtan2(y: Ay, x: Az);
58}
59
60void GenericTiltSensor::calibrate()
61{
62 calibratedPitch = pitch;
63 calibratedRoll = roll;
64}
65
66bool GenericTiltSensor::filter(QAccelerometerReading *reading)
67{
68 /*
69 z y
70 | /
71 |/___ x
72 */
73
74 qreal ax = reading->x();
75 qreal ay = reading->y();
76 qreal az = reading->z();
77#ifdef LOGCALIBRATION
78 qDebug() << "------------ new value -----------";
79 qDebug() << "old _pitch: " << pitch;
80 qDebug() << "old _roll: " << roll;
81 qDebug() << "_calibratedPitch: " << calibratedPitch;
82 qDebug() << "_calibratedRoll: " << calibratedRoll;
83#endif
84 pitch = calcPitch(Ax: ax, Ay: ay, Az: az);
85 roll = calcRoll (ax, Ay: ay, Az: az);
86#ifdef LOGCALIBRATION
87 qDebug() << "_pitch: " << pitch;
88 qDebug() << "_roll: " << roll;
89#endif
90 qreal xrot = roll - calibratedRoll;
91 qreal yrot = pitch - calibratedPitch;
92 //get angle between 0 and 180 or 0 -180
93 xrot = qAtan2(y: qSin(v: xrot), x: qCos(v: xrot));
94 yrot = qAtan2(y: qSin(v: yrot), x: qCos(v: yrot));
95
96#ifdef LOGCALIBRATION
97 qDebug() << "new xrot: " << xrot;
98 qDebug() << "new yrot: " << yrot;
99 qDebug() << "----------------------------------";
100#endif
101 qreal dxrot = qRadiansToDegrees(radians: xrot) - xRotation;
102 qreal dyrot = qRadiansToDegrees(radians: yrot) - yRotation;
103 if (dxrot < 0) dxrot = -dxrot;
104 if (dyrot < 0) dyrot = -dyrot;
105
106 bool setNewReading = false;
107 if (dxrot >= qRadiansToDegrees(radians: radAccuracy) || !sensor()->skipDuplicates()) {
108 xRotation = qRadiansToDegrees(radians: xrot);
109 setNewReading = true;
110 }
111 if (dyrot >= qRadiansToDegrees(radians: radAccuracy) || !sensor()->skipDuplicates()) {
112 yRotation = qRadiansToDegrees(radians: yrot);
113 setNewReading = true;
114 }
115
116 if (setNewReading || m_reading.timestamp() == 0) {
117 m_reading.setTimestamp(reading->timestamp());
118 m_reading.setXRotation(xRotation);
119 m_reading.setYRotation(yRotation);
120 newReadingAvailable();
121 }
122
123 return false;
124}
125
126bool GenericTiltSensor::isFeatureSupported(QSensor::Feature feature) const
127{
128 return (feature == QSensor::Feature::SkipDuplicates);
129}
130

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