androidcompass.cpp

++ // Copyright (C) 2019 BogDan Vatra <bogdan@kde.org>

++ // Copyright (C) 2008 The Android Open Source Project

++ // SPDX-License-Identifier: Apache-2.0

++ // Qt-Security score:significant reason:default

++

++ #include "androidcompass.h"

++

++ #include <qmath.h>

++

++ #include "sensormanager.h"

++

++ AndroidCompass::AndroidCompass(QSensor *sensor, QObject *parent)

++     : ThreadSafeSensorBackend(sensor, parent)

++ {

++     setDescription("Compass");

++     setReading<QCompassReading>(&m_reading);

++     memset(&m_accelerometerEvent, 0, sizeof(ASensorVector));

++     memset(&m_magneticEvent, 0, sizeof(ASensorVector));

++     m_sensorEventQueue = ASensorManager_createEventQueue(m_sensorManager->manager(), m_sensorManager->looper(), -1, &looperCallback, this);

++     m_accelerometer = ASensorManager_getDefaultSensor(m_sensorManager->manager(), ASENSOR_TYPE_ACCELEROMETER);

++     m_magnetometer = ASensorManager_getDefaultSensor(m_sensorManager->manager(), ASENSOR_TYPE_MAGNETIC_FIELD);

++ }

++

++ AndroidCompass::~AndroidCompass()

++ {

++     stop();

++     ASensorManager_destroyEventQueue(m_sensorManager->manager(), m_sensorEventQueue);

++ }

++

++ void AndroidCompass::start()

++ {

++     ASensorEventQueue_enableSensor(m_sensorEventQueue, m_accelerometer);

++     if (sensor()->dataRate() > 0)

++         ASensorEventQueue_setEventRate(m_sensorEventQueue, m_accelerometer, std::max(ASensor_getMinDelay(m_accelerometer), sensor()->dataRate()));

++

++     ASensorEventQueue_enableSensor(m_sensorEventQueue, m_magnetometer);

++     if (sensor()->dataRate() > 0)

++         ASensorEventQueue_setEventRate(m_sensorEventQueue, m_magnetometer, std::max(ASensor_getMinDelay(m_magnetometer), sensor()->dataRate()));

++ }

++

++ void AndroidCompass::stop()

++ {

++     ASensorEventQueue_disableSensor(m_sensorEventQueue, m_accelerometer);

++     ASensorEventQueue_disableSensor(m_sensorEventQueue, m_magnetometer);

++ }

++

++ void AndroidCompass::readAllEvents()

++ {

++     {

++         ASensorEvent sensorEvent;

++         QMutexLocker lock(&m_sensorsMutex);

++         while (ASensorEventQueue_getEvents(m_sensorEventQueue, &sensorEvent, 1)) {

++             switch (sensorEvent.type) {

++             case ASENSOR_TYPE_ACCELEROMETER:

++                 m_accelerometerEvent = sensorEvent.acceleration;

++                 m_accelerometerEvent.status = m_accelerometerEvent.status == ASENSOR_STATUS_NO_CONTACT ? 0 : m_accelerometerEvent.status;

++                 break;

++             case ASENSOR_TYPE_MAGNETIC_FIELD:

++                 m_magneticEvent = sensorEvent.magnetic;

++                 m_magneticEvent.status = m_magneticEvent.status == ASENSOR_STATUS_NO_CONTACT ? 0 : m_magneticEvent.status;

++                 break;

++             }

++         }

++     }

++

++     QCoreApplication::postEvent(this, new FunctionEvent{[=]() {

++         // merged getRotationMatrix https://android.googlesource.com/platform/frameworks/base/+/master/core/java/android/hardware/SensorManager.java#1182

++         // and getOrientation https://android.googlesource.com/platform/frameworks/base/+/master/core/java/android/hardware/SensorManager.java#1477

++         QMutexLocker lock(&m_sensorsMutex);

++         auto Ax = qreal(m_accelerometerEvent.x);

++         auto Ay = qreal(m_accelerometerEvent.y);

++         auto Az = qreal(m_accelerometerEvent.z);

++

++         const qreal normsqA = (Ax * Ax + Ay * Ay + Az * Az);

++         const auto g = qreal(ASENSOR_STANDARD_GRAVITY);

++         const qreal freeFallGravitySquared = 0.01 * g * g;

++         if (normsqA < freeFallGravitySquared)

++             return;

++

++         auto Ex = qreal(m_magneticEvent.x);

++         auto Ey = qreal(m_magneticEvent.y);

++         auto Ez = qreal(m_magneticEvent.z);

++         qreal Hx = Ey * Az - Ez * Ay;

++         qreal Hy = Ez * Ax - Ex * Az;

++         qreal Hz = Ex * Ay - Ey * Ax;

++         const qreal normH = std::sqrt(Hx * Hx + Hy * Hy + Hz * Hz);

++

++         if (normH < 0.1)

++             return;

++         const qreal invH = 1.0 / normH;

++         Hx *= invH;

++         Hy *= invH;

++         Hz *= invH;

++         const qreal invA = 1.0 / std::sqrt(Ax * Ax + Ay * Ay + Az * Az);

++         Ax *= invA;

++         Ay *= invA;

++         Az *= invA;

++         const qreal My = Az * Hx - Ax * Hz;

++         qreal azimuth = std::atan2(Hy, My);

++         qreal accuracyValue = (m_accelerometerEvent.status + m_magneticEvent.status) / 6.0;

++         if (sensor()->skipDuplicates() && qFuzzyCompare(azimuth, m_reading.azimuth()) &&

++                 qFuzzyCompare(accuracyValue, m_reading.calibrationLevel())) {

++             return;

++         }

++         m_reading.setAzimuth(qRadiansToDegrees(azimuth));

++         m_reading.setCalibrationLevel(accuracyValue);

++         newReadingAvailable();

++     }});

++ }

++

++ int AndroidCompass::looperCallback(int, int, void *data)

++ {

++     auto self = reinterpret_cast<AndroidCompass*>(data);

++     self->readAllEvents();

++     return 1; // 1 means keep receiving events

++ }

Revision control