1 /*
2 * Copyright (C) 2010 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
17 #include <stdint.h>
18 #include <math.h>
19 #include <sys/types.h>
20
21 #include <utils/Errors.h>
22
23 #include <hardware/sensors.h>
24
25 #include "RotationVectorSensor.h"
26
27 namespace android {
28 // ---------------------------------------------------------------------------
29
30 template <typename T>
clamp(T v)31 static inline T clamp(T v) {
32 return v < 0 ? 0 : v;
33 }
34
RotationVectorSensor(sensor_t const * list,size_t count)35 RotationVectorSensor::RotationVectorSensor(sensor_t const* list, size_t count)
36 : mSensorDevice(SensorDevice::getInstance()),
37 mALowPass(M_SQRT1_2, 1.5f),
38 mAX(mALowPass), mAY(mALowPass), mAZ(mALowPass),
39 mMLowPass(M_SQRT1_2, 1.5f),
40 mMX(mMLowPass), mMY(mMLowPass), mMZ(mMLowPass)
41 {
42 for (size_t i=0 ; i<count ; i++) {
43 if (list[i].type == SENSOR_TYPE_ACCELEROMETER) {
44 mAcc = Sensor(list + i);
45 }
46 if (list[i].type == SENSOR_TYPE_MAGNETIC_FIELD) {
47 mMag = Sensor(list + i);
48 }
49 }
50 memset(mMagData, 0, sizeof(mMagData));
51 }
52
process(sensors_event_t * outEvent,const sensors_event_t & event)53 bool RotationVectorSensor::process(sensors_event_t* outEvent,
54 const sensors_event_t& event)
55 {
56 const static double NS2S = 1.0 / 1000000000.0;
57 if (event.type == SENSOR_TYPE_MAGNETIC_FIELD) {
58 const double now = event.timestamp * NS2S;
59 if (mMagTime == 0) {
60 mMagData[0] = mMX.init(event.magnetic.x);
61 mMagData[1] = mMY.init(event.magnetic.y);
62 mMagData[2] = mMZ.init(event.magnetic.z);
63 } else {
64 double dT = now - mMagTime;
65 mMLowPass.setSamplingPeriod(dT);
66 mMagData[0] = mMX(event.magnetic.x);
67 mMagData[1] = mMY(event.magnetic.y);
68 mMagData[2] = mMZ(event.magnetic.z);
69 }
70 mMagTime = now;
71 }
72 if (event.type == SENSOR_TYPE_ACCELEROMETER) {
73 const double now = event.timestamp * NS2S;
74 float Ax, Ay, Az;
75 if (mAccTime == 0) {
76 Ax = mAX.init(event.acceleration.x);
77 Ay = mAY.init(event.acceleration.y);
78 Az = mAZ.init(event.acceleration.z);
79 } else {
80 double dT = now - mAccTime;
81 mALowPass.setSamplingPeriod(dT);
82 Ax = mAX(event.acceleration.x);
83 Ay = mAY(event.acceleration.y);
84 Az = mAZ(event.acceleration.z);
85 }
86 mAccTime = now;
87 const float Ex = mMagData[0];
88 const float Ey = mMagData[1];
89 const float Ez = mMagData[2];
90 float Hx = Ey*Az - Ez*Ay;
91 float Hy = Ez*Ax - Ex*Az;
92 float Hz = Ex*Ay - Ey*Ax;
93 const float normH = sqrtf(Hx*Hx + Hy*Hy + Hz*Hz);
94 if (normH < 0.1f) {
95 // device is close to free fall (or in space?), or close to
96 // magnetic north pole. Typical values are > 100.
97 return false;
98 }
99 const float invH = 1.0f / normH;
100 const float invA = 1.0f / sqrtf(Ax*Ax + Ay*Ay + Az*Az);
101 Hx *= invH;
102 Hy *= invH;
103 Hz *= invH;
104 Ax *= invA;
105 Ay *= invA;
106 Az *= invA;
107 const float Mx = Ay*Hz - Az*Hy;
108 const float My = Az*Hx - Ax*Hz;
109 const float Mz = Ax*Hy - Ay*Hx;
110
111 // matrix to rotation vector (normalized quaternion)
112 float qw = sqrtf( clamp( Hx + My + Az + 1) * 0.25f );
113 float qx = sqrtf( clamp( Hx - My - Az + 1) * 0.25f );
114 float qy = sqrtf( clamp(-Hx + My - Az + 1) * 0.25f );
115 float qz = sqrtf( clamp(-Hx - My + Az + 1) * 0.25f );
116 qx = copysignf(qx, Ay - Mz);
117 qy = copysignf(qy, Hz - Ax);
118 qz = copysignf(qz, Mx - Hy);
119
120 // this quaternion is guaranteed to be normalized, by construction
121 // of the rotation matrix.
122
123 *outEvent = event;
124 outEvent->data[0] = qx;
125 outEvent->data[1] = qy;
126 outEvent->data[2] = qz;
127 outEvent->data[3] = qw;
128 outEvent->sensor = '_rov';
129 outEvent->type = SENSOR_TYPE_ROTATION_VECTOR;
130 return true;
131 }
132 return false;
133 }
134
activate(void * ident,bool enabled)135 status_t RotationVectorSensor::activate(void* ident, bool enabled) {
136 mSensorDevice.activate(this, mAcc.getHandle(), enabled);
137 mSensorDevice.activate(this, mMag.getHandle(), enabled);
138 if (enabled) {
139 mMagTime = 0;
140 mAccTime = 0;
141 }
142 return NO_ERROR;
143 }
144
setDelay(void * ident,int handle,int64_t ns)145 status_t RotationVectorSensor::setDelay(void* ident, int handle, int64_t ns)
146 {
147 mSensorDevice.setDelay(this, mAcc.getHandle(), ns);
148 mSensorDevice.setDelay(this, mMag.getHandle(), ns);
149 return NO_ERROR;
150 }
151
getSensor() const152 Sensor RotationVectorSensor::getSensor() const {
153 sensor_t hwSensor;
154 hwSensor.name = "Rotation Vector Sensor";
155 hwSensor.vendor = "Google Inc.";
156 hwSensor.version = 1;
157 hwSensor.handle = '_rov';
158 hwSensor.type = SENSOR_TYPE_ROTATION_VECTOR;
159 hwSensor.maxRange = 1;
160 hwSensor.resolution = 1.0f / (1<<24);
161 hwSensor.power = mAcc.getPowerUsage() + mMag.getPowerUsage();
162 hwSensor.minDelay = mAcc.getMinDelay();
163 Sensor sensor(&hwSensor);
164 return sensor;
165 }
166
167 // ---------------------------------------------------------------------------
168 }; // namespace android
169
170