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     .fifoReservedEventCount = 0,
  49     .fifoMaxEventCount      = 0,
  50     .stringType = SENSOR_STRING_TYPE_MAGNETIC_FIELD,
  51     .requiredPermission     = "",
  52     .maxDelay   = 0,
  53     .flags      = SENSOR_FLAG_CONTINUOUS_MODE,
  54     .reserved   = {},
  55 };
  56
  57 SynthCompassSensor::SynthCompassSensor()
  58 {
  59     mPendingEvent.version = sizeof(sensors_event_t);
  60     mPendingEvent.sensor = ID_SC;
  61     mPendingEvent.type = SENSOR_TYPE_MAGNETIC_FIELD;
  62     memset(mPendingEvent.data, 0, sizeof(mPendingEvent.data));
  63 }
  64
  65 // Converts a quaternion (need not be normalized) into a 3x3 rotation
  66 // matrix.  Form and convetions are basically verbatim from Ken
  67 // Shoemake's quatut.pdf.
  68 static void quat2mat(const float q[4], float m[9])
  69 {
  70     float x=q[0], y=q[1], z=q[2], w=q[3];
  71     float norm = 1/sqrt(x*x+y*y+z*z+w*w);
  72     x *= norm; y *= norm; z *= norm; w *= norm;
  73     float result[] = { 1-2*(y*y+z*z),   2*(x*y-w*z),   2*(x*z+w*y),
  74                          2*(x*y+w*z), 1-2*(x*x+z*z),   2*(y*z-w*x),
  75                          2*(x*z-w*y),   2*(y*z+w*x), 1-2*(x*x+y*y) };
  76     memcpy(m, result, 9*sizeof(float));
  77 }
  78
  79 // Multiplies a 3-vector by a 3x3 matrix in row-major order
  80 static void matmul(const float m[9], const float v[3], float out[3])
  81 {
  82     float x=v[0], y=v[1], z=v[2];
  83     out[0] = x*m[0] + y*m[1] + z*m[2];
  84     out[1] = x*m[3] + y*m[4] + z*m[5];
  85     out[2] = x*m[6] + y*m[7] + z*m[8];
  86 }
  87
  88 int SynthCompassSensor::enable(int en)
  89 {
  90     if (!mRotVec)
  91         return -1;
  92     mRotVec->startStop(en);
  93     mEnabled = en;
  94     return 0;
  95 }
  96
  97 int SynthCompassSensor::readEvents(sensors_event_t *data, int count)
  98 {
  99     if (!mHasPendingEvent)
 100         return 0;
 101
 102     mHasPendingEvent = false;
 103     mPendingEvent.timestamp = getTimestamp();
 104     *data = mPendingEvent;
 105     return 1;
 106 }
 107
 108 void SynthCompassSensor::setQuaternion(float qin[4])
 109 {
 110     if (!mEnabled)
 111         return;
 112
 113     // The quat defines a rotation from device coordinates to world
 114     // coordinates, so invert it and generate a matrix that takes
 115     // world coords to device space.
 116     float q[4];
 117     memcpy(q, qin, sizeof(float)*4);
 118     q[0] *= -1;  q[1] *= -1; q[2] *= -1;
 119
 120     float mat[9];
 121     quat2mat(q, mat);
 122
 123     // "North" is alone the Y axis in world coordiantes, convert to
 124     // device space
 125     static const float v[] = { 0, MAG_FIELD_UT, 0 };
 126     matmul(mat, v, mPendingEvent.data);
 127     mPendingEvent.timestamp = getTimestamp();
 128
 129     mHasPendingEvent = true;
 130 }