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