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,
50 .stringType = SENSOR_STRING_TYPE_MAGNETIC_FIELD,
51 .requiredPermission = "",
53 .flags = SENSOR_FLAG_CONTINUOUS_MODE,
57 SynthCompassSensor::SynthCompassSensor()
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));
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])
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));
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])
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];
88 int SynthCompassSensor::enable(int en)
92 mRotVec->startStop(en);
97 int SynthCompassSensor::readEvents(sensors_event_t *data, int count)
99 if (!mHasPendingEvent)
102 mHasPendingEvent = false;
103 mPendingEvent.timestamp = getTimestamp();
104 *data = mPendingEvent;
108 void SynthCompassSensor::setQuaternion(float qin[4])
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.
117 memcpy(q, qin, sizeof(float)*4);
118 q[0] *= -1; q[1] *= -1; q[2] *= -1;
123 // "North" is alone the Y axis in world coordiantes, convert to
125 static const float v[] = { 0, MAG_FIELD_UT, 0 };
126 matmul(mat, v, mPendingEvent.data);
127 mPendingEvent.timestamp = getTimestamp();
129 mHasPendingEvent = true;