RotationVectorSensor.cpp revision f66684a6fb2a2991e84a085673629db2a0494fc6 
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
30RotationVectorSensor::RotationVectorSensor(int mode)
31    : mSensorDevice(SensorDevice::getInstance()),
32      mSensorFusion(SensorFusion::getInstance()),
33      mMode(mode)
34{
35}
36
37bool RotationVectorSensor::process(sensors_event_t* outEvent,
38        const sensors_event_t& event)
39{
40    if (event.type == SENSOR_TYPE_ACCELEROMETER) {
41        if (mSensorFusion.hasEstimate(mMode)) {
42            const vec4_t q(mSensorFusion.getAttitude(mMode));
43            *outEvent = event;
44            outEvent->data[0] = q.x;
45            outEvent->data[1] = q.y;
46            outEvent->data[2] = q.z;
47            outEvent->data[3] = q.w;
48            outEvent->sensor = getSensorToken();
49            outEvent->type = getSensorType();
50            return true;
51        }
52    }
53    return false;
54}
55
56status_t RotationVectorSensor::activate(void* ident, bool enabled) {
57    return mSensorFusion.activate(mMode, ident, enabled);
58}
59
60status_t RotationVectorSensor::setDelay(void* ident, int /*handle*/, int64_t ns) {
61    return mSensorFusion.setDelay(mMode, ident, ns);
62}
63
64Sensor RotationVectorSensor::getSensor() const {
65    sensor_t hwSensor;
66    hwSensor.name       = getSensorName();
67    hwSensor.vendor     = "AOSP";
68    hwSensor.version    = 3;
69    hwSensor.handle     = getSensorToken();
70    hwSensor.type       = getSensorType();
71    hwSensor.maxRange   = 1;
72    hwSensor.resolution = 1.0f / (1<<24);
73    hwSensor.power      = mSensorFusion.getPowerUsage();
74    hwSensor.minDelay   = mSensorFusion.getMinDelay();
75    Sensor sensor(&hwSensor);
76    return sensor;
77}
78
79int RotationVectorSensor::getSensorType() const {
80    switch(mMode) {
81        case FUSION_9AXIS:
82            return SENSOR_TYPE_ROTATION_VECTOR;
83        case FUSION_NOMAG:
84            return SENSOR_TYPE_GAME_ROTATION_VECTOR;
85        case FUSION_NOGYRO:
86            return SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR;
87        default:
88            assert(0);
89            return 0;
90    }
91}
92
93const char* RotationVectorSensor::getSensorName() const {
94    switch(mMode) {
95        case FUSION_9AXIS:
96            return "Rotation Vector Sensor";
97        case FUSION_NOMAG:
98            return "Game Rotation Vector Sensor";
99        case FUSION_NOGYRO:
100            return "GeoMag Rotation Vector Sensor";
101        default:
102            assert(0);
103            return NULL;
104    }
105}
106
107int RotationVectorSensor::getSensorToken() const {
108    switch(mMode) {
109        case FUSION_9AXIS:
110            return '_rov';
111        case FUSION_NOMAG:
112            return '_gar';
113        case FUSION_NOGYRO:
114            return '_geo';
115        default:
116            assert(0);
117            return 0;
118    }
119}
120
121// ---------------------------------------------------------------------------
122
123GyroDriftSensor::GyroDriftSensor()
124    : mSensorDevice(SensorDevice::getInstance()),
125      mSensorFusion(SensorFusion::getInstance())
126{
127}
128
129bool GyroDriftSensor::process(sensors_event_t* outEvent,
130        const sensors_event_t& event)
131{
132    if (event.type == SENSOR_TYPE_ACCELEROMETER) {
133        if (mSensorFusion.hasEstimate()) {
134            const vec3_t b(mSensorFusion.getGyroBias());
135            *outEvent = event;
136            outEvent->data[0] = b.x;
137            outEvent->data[1] = b.y;
138            outEvent->data[2] = b.z;
139            outEvent->sensor = '_gbs';
140            outEvent->type = SENSOR_TYPE_ACCELEROMETER;
141            return true;
142        }
143    }
144    return false;
145}
146
147status_t GyroDriftSensor::activate(void* ident, bool enabled) {
148    return mSensorFusion.activate(FUSION_9AXIS, ident, enabled);
149}
150
151status_t GyroDriftSensor::setDelay(void* ident, int /*handle*/, int64_t ns) {
152    return mSensorFusion.setDelay(FUSION_9AXIS, ident, ns);
153}
154
155Sensor GyroDriftSensor::getSensor() const {
156    sensor_t hwSensor;
157    hwSensor.name       = "Gyroscope Bias (debug)";
158    hwSensor.vendor     = "AOSP";
159    hwSensor.version    = 1;
160    hwSensor.handle     = '_gbs';
161    hwSensor.type       = SENSOR_TYPE_ACCELEROMETER;
162    hwSensor.maxRange   = 1;
163    hwSensor.resolution = 1.0f / (1<<24);
164    hwSensor.power      = mSensorFusion.getPowerUsage();
165    hwSensor.minDelay   = mSensorFusion.getMinDelay();
166    Sensor sensor(&hwSensor);
167    return sensor;
168}
169
170// ---------------------------------------------------------------------------
171}; // namespace android
172
173