Magnetometer/Compass integrated with orientation

There is a little 3D scene used for acceleration. This scene is now
being used to set Magnetometer and Compass values. As far as I can
tell, this is accurate though I have never actually used these
sensors on a real device.

The magnetometer code assumes a constant 30 micro-tesla field pointing
towards the North direction.

The compass code is awful but seems to give an accurate reading based
on the top of the device's heading.

8 files changed, 102 insertions, 14 deletions

diff --git a/src/mobility/mobilitydata.cpp b/src/mobility/mobilitydata.cpp
index 00eb592..287b4e7 100644
--- a/src/mobility/mobilitydata.cpp
+++ b/src/mobility/mobilitydata.cpp
@@ -84,13 +84,13 @@ void MobilityData::setInitialSensorsData()
     sensors.accelerometerY = 9.8;
     sensors.accelerometerZ = 0;
 
-    sensors.magnetometerX = 0.5;
-    sensors.magnetometerY = 0.5;
-    sensors.magnetometerZ = 0.5;
+    sensors.magnetometerX = 0;
+    sensors.magnetometerY = 0;
+    sensors.magnetometerZ = 30;
     sensors.magnetometerCalibrationLevel = 0.85;
 
     sensors.compassCalibrationLevel = 0.85;
-    sensors.compassAzimuth = 132.65;
+    sensors.compassAzimuth = 0;
 
     sensors.proximitySensorClose = true;
 
diff --git a/src/mobility/mobilitymanager.cpp b/src/mobility/mobilitymanager.cpp
index 52ad9a0..888ea64 100644
--- a/src/mobility/mobilitymanager.cpp
+++ b/src/mobility/mobilitymanager.cpp
@@ -355,9 +355,9 @@ void MobilityClient::sendSensorsData(const SensorsUi::SensorsData &data)
                                                    "setAccelerometerData", accelermometerData);
 
     QMagnetometerReadingData magnetometerData;
-    magnetometerData.x = data.magnetometerX;
-    magnetometerData.y = data.magnetometerY;
-    magnetometerData.z = data.magnetometerZ;
+    magnetometerData.x = data.magnetometerX * 1e-6; // scale to Teslas
+    magnetometerData.y = data.magnetometerY * 1e-6;
+    magnetometerData.z = data.magnetometerZ * 1e-6;
     magnetometerData.calibrationLevel = data.magnetometerCalibrationLevel;
     magnetometerData.timestamp = timestampToSend;
     QtSimulatorPrivate::RemoteMetacall<void>::call(mSendSocket, QtSimulatorPrivate::NoSync,
diff --git a/src/ui/accelerometercontrol.cpp b/src/ui/accelerometercontrol.cpp
index 1ab7434..b9ebb97 100644
--- a/src/ui/accelerometercontrol.cpp
+++ b/src/ui/accelerometercontrol.cpp
@@ -46,11 +46,15 @@ static QVector3D accelerometerZ(0, 0, -1);
 static QVector3D unrotatedGravity(0, 1, 0);
 
 static const double kEarthGravity = 9.8;
+static const double kEarthMagneticFieldStrength = 30;
 static const double kZero = 1e-4;
 
 AccelerometerControl::AccelerometerControl(QWidget *parent)
-    : QGLWidget(parent), mGravity(kEarthGravity)
+    : QGLWidget(parent), mGravity(kEarthGravity), mMagneticFieldStrength(kEarthMagneticFieldStrength)
 {
+    QMatrix4x4 qtMatrix;
+    qtMatrix.rotate(90, QVector3D(1, 0, 0)); // angle down to point at the horizon
+    mNorthVector = qtMatrix.map(unrotatedGravity); // give us a vector that represents where North is
 }
 
 void AccelerometerControl::setValue(const QVector3D &newValue)
@@ -75,6 +79,7 @@ void AccelerometerControl::setValue(const QVector3D &newValue)
         mRotation = QQuaternion::fromAxisAndAngle(cross, qAcos(dot) * 180 / M_PI);
     }
 
+    updateAzimuth();
     updateGL();
 }
 
@@ -105,6 +110,8 @@ void AccelerometerControl::initializeGL()
     mRightLandscapeTexture = bindTexture(rightImg);
     mTopLandscapeTexture = bindTexture(topImg);
     mBottomLandscapeTexture = bindTexture(topImg);
+    QImage northImg(":ui/icons/north.png");
+    mNorthTexture = bindTexture(northImg);
 }
 
 void AccelerometerControl::resizeGL(int w, int h)
@@ -224,14 +231,19 @@ void AccelerometerControl::drawMobile()
     glEnd();
 }
 
-static void drawGround()
+void AccelerometerControl::drawGround()
 {
+    glBindTexture(GL_TEXTURE_2D, mNorthTexture);
     glBegin(GL_QUADS);
     glColor3d(0.3, 0.3, 0.3);
     qreal extends = 1.5;
+    glTexCoord2f(0, 0);
     glVertex3f(-extends, -1, -extends);
+    glTexCoord2f(0, 1);
     glVertex3f(-extends, -1, extends);
+    glTexCoord2f(1, 1);
     glVertex3f(extends, -1, extends);
+    glTexCoord2f(1, 0);
     glVertex3f(extends, -1, -extends);
     glEnd();
 }
@@ -275,6 +287,7 @@ void AccelerometerControl::mouseMoveEvent(QMouseEvent *event)
     mOldMousePosition = event->pos();
 
     emit valueChanged(value());
+    updateAzimuth();
     updateGL();
 }
 
@@ -315,6 +328,55 @@ QVector3D AccelerometerControl::value() const
     if (qAbs(newValue.y()) < kZero) newValue.setY(0);
     if (qAbs(newValue.z()) < kZero) newValue.setZ(0);
     newValue *= mGravity;
+    return newValue;
+}
+
+void AccelerometerControl::updateAzimuth()
+{
+    emit magneticFieldChanged(magneticField());
+    emit azimuthChanged(azimuth());
+}
+
+qreal AccelerometerControl::azimuth() const
+{
+    // Generate an azimuth value based on the position
+    QVector3D grav = value();
+    QVector3D mag = magneticField();
+    QVector3D dev = mRotation.vector().normalized();
+    qreal heading;
+    if (grav.z() > 0) {
+        // Face up
+        heading = (1 - qAbs(dev.x())) * 180;
+        // correct for 360 degrees of rotation
+        if (mag.x() < 0) heading = 360 - heading;
+        // rotate in the correct direction!
+        heading = 360 - heading;
+    } else {
+        // Face down
+        heading = qAbs(dev.x()) * 180;
+        // correct for 360 degrees of rotation
+        if (mag.x() < 0) heading = 360 - heading;
+    }
+    // May be too big
+    while (heading > 360) heading -= 360;
+    // report 360 as 0
+    if (heading == 360) heading = 0;
+    // report tiny numbers as 0
+    if (heading < kZero) heading = 0;
+    return heading;
+}
 
+QVector3D AccelerometerControl::magneticField() const
+{
+    QVector3D newValue;
+    newValue.setX(QVector3D::dotProduct(mRotation.conjugate().rotatedVector(accelerometerX), mNorthVector));
+    newValue.setY(QVector3D::dotProduct(mRotation.conjugate().rotatedVector(accelerometerY), mNorthVector));
+    newValue.setZ(QVector3D::dotProduct(mRotation.conjugate().rotatedVector(accelerometerZ), mNorthVector));
+    // remove rounding errors
+    if (qAbs(newValue.x()) < kZero) newValue.setX(0);
+    if (qAbs(newValue.y()) < kZero) newValue.setY(0);
+    if (qAbs(newValue.z()) < kZero) newValue.setZ(0);
+    newValue *= mMagneticFieldStrength;
     return newValue;
 }
+
diff --git a/src/ui/accelerometercontrol.h b/src/ui/accelerometercontrol.h
index 81993fb..591cbd5 100644
--- a/src/ui/accelerometercontrol.h
+++ b/src/ui/accelerometercontrol.h
@@ -47,8 +47,13 @@ public:
     void setDeviceOrientation(bool portrait);
 
     QVector3D value() const;
+    QVector3D magneticField() const;
+    double azimuth() const;
+
 signals:
     void valueChanged(const QVector3D &value);
+    void magneticFieldChanged(const QVector3D &value);
+    void azimuthChanged(double azimuth);
 
 public slots:
     void setValue(const QVector3D &value);
@@ -63,12 +68,17 @@ protected:
 
 private:
     void drawMobile();
+    void drawGround();
+    void updateAzimuth();
 
 private:
     QPoint mOldMousePosition;
     QQuaternion mRotation;
     qreal mGravity;
     bool mDefaultPortrait;
+    QVector3D mNorthVector;
+    double mMagneticFieldStrength;
+    double mAzimuth;
 
     GLuint mFrontTexture;
     GLuint mBackTexture;
@@ -76,6 +86,7 @@ private:
     GLuint mRightTexture;
     GLuint mTopTexture;
     GLuint mBottomTexture;
+    GLuint mNorthTexture;
 
     GLuint mFrontPortraitTexture;
     GLuint mBackPortraitTexture;
diff --git a/src/ui/icons/north.png b/src/ui/icons/north.png
new file mode 100644
index 0000000..65c69e4
--- /dev/null
+++ b/src/ui/icons/north.png
diff --git a/src/ui/sensorsui.cpp b/src/ui/sensorsui.cpp
index 2bd4fbf..7ea6228 100644
--- a/src/ui/sensorsui.cpp
+++ b/src/ui/sensorsui.cpp
@@ -202,7 +202,7 @@ SensorsUi::SensorsUi(QWidget *parent)
 
         control->setLayout(vlayout);
 
-        item = new OptionsItem(tr("Accelerometer"), control);
+        item = new OptionsItem(tr("Orientation"), control);
         item->setTags(tags);
         optionsList << item;
     }
@@ -234,8 +234,10 @@ SensorsUi::SensorsUi(QWidget *parent)
     item->setAdvanced(true);
     optionsList << item;
 
+    connect(mAccelerometerControl, SIGNAL(magneticFieldChanged(QVector3D)), SLOT(setMagnetometerValue(QVector3D)));
+
     mMagnetometerXEdit = new SensorDoubleEdit();
-    mMagnetometerXEdit->setRange(0, 1);
+    mMagnetometerXEdit->setRange(-30, 30);
     mMagnetometerXEdit->setDecimalPlaces(2);
     item = new OptionsItem(tr("Magnetometer x"), mMagnetometerXEdit);
     connect(mMagnetometerXEdit, SIGNAL(valueChanged(double)), SLOT(emitSensorsDataChange()));
@@ -244,7 +246,7 @@ SensorsUi::SensorsUi(QWidget *parent)
     optionsList << item;
 
     mMagnetometerYEdit = new SensorDoubleEdit();
-    mMagnetometerYEdit->setRange(0, 1);
+    mMagnetometerYEdit->setRange(-30, 30);
     mMagnetometerYEdit->setDecimalPlaces(2);
     item = new OptionsItem(tr("Magnetometer y"), mMagnetometerYEdit);
     connect(mMagnetometerYEdit, SIGNAL(valueChanged(double)), SLOT(emitSensorsDataChange()));
@@ -253,7 +255,7 @@ SensorsUi::SensorsUi(QWidget *parent)
     optionsList << item;
 
     mMagnetometerZEdit = new SensorDoubleEdit();
-    mMagnetometerZEdit->setRange(0, 1);
+    mMagnetometerZEdit->setRange(-30, 30);
     mMagnetometerZEdit->setDecimalPlaces(2);
     item = new OptionsItem(tr("Magnetometer z"), mMagnetometerZEdit);
     connect(mMagnetometerZEdit, SIGNAL(valueChanged(double)), SLOT(emitSensorsDataChange()));
@@ -273,8 +275,9 @@ SensorsUi::SensorsUi(QWidget *parent)
     mCompassAzimuthEdit = new SensorDoubleEdit();
     mCompassAzimuthEdit->setRange(0, 360);
     mCompassAzimuthEdit->setDecimalPlaces(2);
-    item = new OptionsItem(tr("Compass azimuth"), mCompassAzimuthEdit);
+    item = new OptionsItem(tr("Compass Azimuth"), mCompassAzimuthEdit);
     connect(mCompassAzimuthEdit, SIGNAL(valueChanged(double)), SLOT(emitSensorsDataChange()));
+    connect(mAccelerometerControl, SIGNAL(azimuthChanged(double)), mCompassAzimuthEdit, SLOT(setValue(double)));
     item->setTags(tags);
     optionsList << item;
 
@@ -391,6 +394,14 @@ void SensorsUi::setAccelerometerValue(const QVector3D &value)
     emit sensorsDataChanged(sensorsData());
 }
 
+void SensorsUi::setMagnetometerValue(const QVector3D &value)
+{
+    mMagnetometerXEdit->setValue(value.x(), true);
+    mMagnetometerYEdit->setValue(value.y(), true);
+    mMagnetometerZEdit->setValue(value.z(), true);
+    emit sensorsDataChanged(sensorsData());
+}
+
 void SensorsUi::initializeAmbientLightOptions()
 {
     mAmbientLightBox->clear();
diff --git a/src/ui/sensorsui.h b/src/ui/sensorsui.h
index 7b1efd1..9b8a391 100644
--- a/src/ui/sensorsui.h
+++ b/src/ui/sensorsui.h
@@ -58,7 +58,9 @@ public:
     SensorDoubleEdit(QWidget *parent = 0);
 
     double value() const;
+public slots:
     void setValue(double newValue, bool skipSignal = false);
+public:
 
     void setRange(double min, double max);
     QPair<double, double> range() const;
@@ -210,6 +212,7 @@ signals:
 private slots:
     void emitSensorsDataChange() const;
     void setAccelerometerValue(const QVector3D &value);
+    void setMagnetometerValue(const QVector3D &value);
     void updateTimeEditDisabled();
     void updateProximityButtonText();
 
diff --git a/src/ui/ui.qrc b/src/ui/ui.qrc
index 708a217..d1b54d8 100644
--- a/src/ui/ui.qrc
+++ b/src/ui/ui.qrc
@@ -22,5 +22,6 @@
         <file>textures/N900_front.png</file>
         <file>textures/N900_left.png</file>
         <file>textures/N900_top.png</file>
+        <file>icons/north.png</file>
     </qresource>
 </RCC>