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
27 namespace android {
28 // ---------------------------------------------------------------------------
29
RotationVectorSensor(int mode)30 RotationVectorSensor::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
process(sensors_event_t * outEvent,const sensors_event_t & event)46 bool 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
activate(void * ident,bool enabled)65 status_t RotationVectorSensor::activate(void* ident, bool enabled) {
66 return mSensorFusion.activate(mMode, ident, enabled);
67 }
68
setDelay(void * ident,int,int64_t ns)69 status_t RotationVectorSensor::setDelay(void* ident, int /*handle*/, int64_t ns) {
70 return mSensorFusion.setDelay(mMode, ident, ns);
71 }
72
getSensorType() const73 int 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
getSensorName() const87 const 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
getSensorToken() const101 int 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
GyroDriftSensor()117 GyroDriftSensor::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
process(sensors_event_t * outEvent,const sensors_event_t & event)132 bool 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
activate(void * ident,bool enabled)150 status_t GyroDriftSensor::activate(void* ident, bool enabled) {
151 return mSensorFusion.activate(FUSION_9AXIS, ident, enabled);
152 }
153
setDelay(void * ident,int,int64_t ns)154 status_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