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