RotationVectorSensor.cpp revision 984826cc158193e61e3a00359ef4f6699c7d748a 
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()
36    : mSensorDevice(SensorDevice::getInstance()),
37      mSensorFusion(SensorFusion::getInstance())
38{
39}
40
41bool RotationVectorSensor::process(sensors_event_t* outEvent,
42        const sensors_event_t& event)
43{
44    if (event.type == SENSOR_TYPE_ACCELEROMETER) {
45        if (mSensorFusion.hasEstimate()) {
46            const mat33_t R(mSensorFusion.getRotationMatrix());
47
48            // matrix to rotation vector (normalized quaternion)
49            const float Hx = R[0].x;
50            const float My = R[1].y;
51            const float Az = R[2].z;
52
53            float qw = sqrtf( clamp( Hx + My + Az + 1) * 0.25f );
54            float qx = sqrtf( clamp( Hx - My - Az + 1) * 0.25f );
55            float qy = sqrtf( clamp(-Hx + My - Az + 1) * 0.25f );
56            float qz = sqrtf( clamp(-Hx - My + Az + 1) * 0.25f );
57            qx = copysignf(qx, R[2].y - R[1].z);
58            qy = copysignf(qy, R[0].z - R[2].x);
59            qz = copysignf(qz, R[1].x - R[0].y);
60
61            // this quaternion is guaranteed to be normalized, by construction
62            // of the rotation matrix.
63
64            *outEvent = event;
65            outEvent->data[0] = qx;
66            outEvent->data[1] = qy;
67            outEvent->data[2] = qz;
68            outEvent->data[3] = qw;
69            outEvent->sensor = '_rov';
70            outEvent->type = SENSOR_TYPE_ROTATION_VECTOR;
71            return true;
72        }
73    }
74    return false;
75}
76
77status_t RotationVectorSensor::activate(void* ident, bool enabled) {
78    return mSensorFusion.activate(this, enabled);
79}
80
81status_t RotationVectorSensor::setDelay(void* ident, int handle, int64_t ns) {
82    return mSensorFusion.setDelay(this, ns);
83}
84
85Sensor RotationVectorSensor::getSensor() const {
86    sensor_t hwSensor;
87    hwSensor.name       = "Rotation Vector Sensor";
88    hwSensor.vendor     = "Google Inc.";
89    hwSensor.version    = mSensorFusion.hasGyro() ? 3 : 2;
90    hwSensor.handle     = '_rov';
91    hwSensor.type       = SENSOR_TYPE_ROTATION_VECTOR;
92    hwSensor.maxRange   = 1;
93    hwSensor.resolution = 1.0f / (1<<24);
94    hwSensor.power      = mSensorFusion.getPowerUsage();
95    hwSensor.minDelay   = mSensorFusion.getMinDelay();
96    Sensor sensor(&hwSensor);
97    return sensor;
98}
99
100// ---------------------------------------------------------------------------
101}; // namespace android
102
103