common/libsensors/SynthCompassSensor.cpp

   1 /*
   2  * Copyright (C) 2013 The Android Open Source Project
   3  *
   4  * Licensed under the Apache License, Version 2.0 (the "License");
   5  * you may not use this file except in compliance with the License.
   6  * You may obtain a copy of the License at
   7  *
   8  *      http://www.apache.org/licenses/LICENSE-2.0
   9  *
  10  * Unless required by applicable law or agreed to in writing, software
  11  * distributed under the License is distributed on an "AS IS" BASIS,
  12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  13  * See the License for the specific language governing permissions and
  14  * limitations under the License.
  15  */
  16 #include <cutils/log.h>
  17 #include "common.h"
  18 #include "SensorConfig.h"
  19 #include "SynthCompassSensor.h"
  20
  21 // This is a "2D" faked compass.  The Intel sensor hub reports
  22 // uncalibrated/raw magnetometer data because of Microsoft
  23 // requirements for Windows sensor data.  Android has no calibration
  24 // built into the frameworks and relies on the HAL to do it.  Which is
  25 // sad, because that calibration is already being done internal to the
  26 // sensor hub for use by the integrated orientation/quaternion
  27 // sensors.
  28 //
  29 // Instead of doing the calibration again, we're just using that
  30 // output quaternion by returning a faked "magnetometer" reading
  31 // pointing directly at magnetic north.  This isn't quite correct, as
  32 // real magnetic fields are 3D vectors with (typically) non-zero
  33 // elevations.  The output from this sensor is always constrained to
  34 // lie along the ground plane.
  35
  36 #define MAG_FIELD_UT 45 // Microtessla "field strength" reported
  37
  38 const struct sensor_t SynthCompassSensor::sSensorInfo_compass = {
  39     .name       = "HID_SENSOR Leveled Compass",
  40     .vendor     = "Intel",
  41     .version    = 1,
  42     .handle     = SENSORS_SYNCOMPASS_HANDLE,
  43     .type       = SENSOR_TYPE_MAGNETIC_FIELD,
  44     .maxRange   = MAG_FIELD_UT * 2,
  45     .resolution = MAG_FIELD_UT/256.0, // IIO reports 8 bit samples
  46     .power      = 0.1f,
  47     .minDelay   = 0,
  48     .reserved   = {},
  49 };
  50
  51 SynthCompassSensor::SynthCompassSensor()
  52 {
  53     mPendingEvent.version = sizeof(sensors_event_t);
  54     mPendingEvent.sensor = ID_SC;
  55     mPendingEvent.type = SENSOR_TYPE_MAGNETIC_FIELD;
  56     memset(mPendingEvent.data, 0, sizeof(mPendingEvent.data));
  57 }
  58
  59 // Converts a quaternion (need not be normalized) into a 3x3 rotation
  60 // matrix.  Form and convetions are basically verbatim from Ken
  61 // Shoemake's quatut.pdf.
  62 static void quat2mat(const float q[4], float m[9])
  63 {
  64     float x=q[0], y=q[1], z=q[2], w=q[3];
  65     float norm = 1/sqrt(x*x+y*y+z*z+w*w);
  66     x *= norm; y *= norm; z *= norm; w *= norm;
  67     float result[] = { 1-2*(y*y+z*z),   2*(x*y-w*z),   2*(x*z+w*y),
  68                          2*(x*y+w*z), 1-2*(x*x+z*z),   2*(y*z-w*x),
  69                          2*(x*z-w*y),   2*(y*z+w*x), 1-2*(x*x+y*y) };
  70     memcpy(m, result, 9*sizeof(float));
  71 }
  72
  73 // Multiplies a 3-vector by a 3x3 matrix in row-major order
  74 static void matmul(const float m[9], const float v[3], float out[3])
  75 {
  76     float x=v[0], y=v[1], z=v[2];
  77     out[0] = x*m[0] + y*m[1] + z*m[2];
  78     out[1] = x*m[3] + y*m[4] + z*m[5];
  79     out[2] = x*m[6] + y*m[7] + z*m[8];
  80 }
  81
  82 int SynthCompassSensor::enable(int en)
  83 {
  84     if (!mRotVec)
  85         return -1;
  86     mRotVec->startStop(en);
  87     mEnabled = en;
  88     return 0;
  89 }
  90
  91 int SynthCompassSensor::readEvents(sensors_event_t *data, int count)
  92 {
  93     if (!mHasPendingEvent)
  94         return 0;
  95
  96     mHasPendingEvent = false;
  97     mPendingEvent.timestamp = getTimestamp();
  98     *data = mPendingEvent;
  99     return 1;
 100 }
 101
 102 void SynthCompassSensor::setQuaternion(float qin[4])
 103 {
 104     if (!mEnabled)
 105         return;
 106
 107     // The quat defines a rotation from device coordinates to world
 108     // coordinates, so invert it and generate a matrix that takes
 109     // world coords to device space.
 110     float q[4];
 111     memcpy(q, qin, sizeof(float)*4);
 112     q[0] *= -1;  q[1] *= -1; q[2] *= -1;
 113
 114     float mat[9];
 115     quat2mat(q, mat);
 116
 117     // "North" is alone the Y axis in world coordiantes, convert to
 118     // device space
 119     static const float v[] = { 0, MAG_FIELD_UT, 0 };
 120     matmul(mat, v, mPendingEvent.data);
 121     mPendingEvent.timestamp = getTimestamp();
 122
 123     mHasPendingEvent = true;
 124 }