2 * Copyright (C) 2010 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.
19 #include <sys/types.h>
21 #include <utils/Errors.h>
23 #include <hardware/sensors.h>
25 #include "RotationVectorSensor.h"
28 // ---------------------------------------------------------------------------
31 static inline T clamp(T v) {
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)
42 for (size_t i=0 ; i<count ; i++) {
43 if (list[i].type == SENSOR_TYPE_ACCELEROMETER) {
44 mAcc = Sensor(list + i);
46 if (list[i].type == SENSOR_TYPE_MAGNETIC_FIELD) {
47 mMag = Sensor(list + i);
50 memset(mMagData, 0, sizeof(mMagData));
53 bool RotationVectorSensor::process(sensors_event_t* outEvent,
54 const sensors_event_t& event)
56 const static double NS2S = 1.0 / 1000000000.0;
57 if (event.type == SENSOR_TYPE_MAGNETIC_FIELD) {
58 const double now = event.timestamp * NS2S;
60 mMagData[0] = mMX.init(event.magnetic.x);
61 mMagData[1] = mMY.init(event.magnetic.y);
62 mMagData[2] = mMZ.init(event.magnetic.z);
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);
72 if (event.type == SENSOR_TYPE_ACCELEROMETER) {
73 const double now = event.timestamp * NS2S;
76 Ax = mAX.init(event.acceleration.x);
77 Ay = mAY.init(event.acceleration.y);
78 Az = mAZ.init(event.acceleration.z);
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);
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);
95 // device is close to free fall (or in space?), or close to
96 // magnetic north pole. Typical values are > 100.
99 const float invH = 1.0f / normH;
100 const float invA = 1.0f / sqrtf(Ax*Ax + Ay*Ay + Az*Az);
107 const float Mx = Ay*Hz - Az*Hy;
108 const float My = Az*Hx - Ax*Hz;
109 const float Mz = Ax*Hy - Ay*Hx;
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);
120 // this quaternion is guaranteed to be normalized, by construction
121 // of the rotation matrix.
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;
135 status_t RotationVectorSensor::activate(void* ident, bool enabled) {
136 mSensorDevice.activate(this, mAcc.getHandle(), enabled);
137 mSensorDevice.activate(this, mMag.getHandle(), enabled);
145 status_t RotationVectorSensor::setDelay(void* ident, int handle, int64_t ns)
147 mSensorDevice.setDelay(this, mAcc.getHandle(), ns);
148 mSensorDevice.setDelay(this, mMag.getHandle(), ns);
152 Sensor RotationVectorSensor::getSensor() const {
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);
167 // ---------------------------------------------------------------------------
168 }; // namespace android