2 * Copyright (C) 2013 The Android Open Source Project
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
8 * http://www.apache.org/licenses/LICENSE-2.0
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.
16 #include <cutils/log.h>
18 #include "SensorConfig.h"
19 #include "SynthCompassSensor.h"
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
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.
36 #define MAG_FIELD_UT 45 // Microtessla "field strength" reported
38 const struct sensor_t SynthCompassSensor::sSensorInfo_compass = {
39 .name = "HID_SENSOR Leveled Compass",
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
48 .fifoReservedEventCount = 0,
49 .fifoMaxEventCount = 0,
53 SynthCompassSensor::SynthCompassSensor()
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));
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])
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));
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])
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];
84 int SynthCompassSensor::enable(int en)
88 mRotVec->startStop(en);
93 int SynthCompassSensor::readEvents(sensors_event_t *data, int count)
95 if (!mHasPendingEvent)
98 mHasPendingEvent = false;
99 mPendingEvent.timestamp = getTimestamp();
100 *data = mPendingEvent;
104 void SynthCompassSensor::setQuaternion(float qin[4])
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.
113 memcpy(q, qin, sizeof(float)*4);
114 q[0] *= -1; q[1] *= -1; q[2] *= -1;
119 // "North" is alone the Y axis in world coordiantes, convert to
121 static const float v[] = { 0, MAG_FIELD_UT, 0 };
122 matmul(mat, v, mPendingEvent.data);
123 mPendingEvent.timestamp = getTimestamp();
125 mHasPendingEvent = true;