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