1 /*
2 * Copyright (C) 2011 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 "SensorDevice.h"
18 #include "SensorFusion.h"
19 #include "SensorService.h"
20
21 namespace android {
22 // ---------------------------------------------------------------------------
23
ANDROID_SINGLETON_STATIC_INSTANCE(SensorFusion)24 ANDROID_SINGLETON_STATIC_INSTANCE(SensorFusion)
25
26 SensorFusion::SensorFusion()
27 : mSensorDevice(SensorDevice::getInstance()),
28 mAttitude(mAttitudes[FUSION_9AXIS]),
29 mGyroTime(0), mAccTime(0)
30 {
31 sensor_t const* list;
32 Sensor uncalibratedGyro;
33 ssize_t count = mSensorDevice.getSensorList(&list);
34
35 mEnabled[FUSION_9AXIS] = false;
36 mEnabled[FUSION_NOMAG] = false;
37 mEnabled[FUSION_NOGYRO] = false;
38
39 if (count > 0) {
40 for (size_t i=0 ; i<size_t(count) ; i++) {
41 if (list[i].type == SENSOR_TYPE_ACCELEROMETER) {
42 mAcc = Sensor(list + i);
43 }
44 if (list[i].type == SENSOR_TYPE_MAGNETIC_FIELD) {
45 mMag = Sensor(list + i);
46 }
47 if (list[i].type == SENSOR_TYPE_GYROSCOPE) {
48 mGyro = Sensor(list + i);
49 }
50 if (list[i].type == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) {
51 uncalibratedGyro = Sensor(list + i);
52 }
53 }
54
55 // Use the uncalibrated gyroscope for sensor fusion when available
56 if (uncalibratedGyro.getType() == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) {
57 mGyro = uncalibratedGyro;
58 }
59
60 // 200 Hz for gyro events is a good compromise between precision
61 // and power/cpu usage.
62 mEstimatedGyroRate = 200;
63 mTargetDelayNs = 1000000000LL/mEstimatedGyroRate;
64
65 for (int i = 0; i<NUM_FUSION_MODE; ++i) {
66 mFusions[i].init(i);
67 }
68 }
69 }
70
process(const sensors_event_t & event)71 void SensorFusion::process(const sensors_event_t& event) {
72
73 if (event.type == mGyro.getType()) {
74 float dT;
75 if ( event.timestamp - mGyroTime> 0 &&
76 event.timestamp - mGyroTime< (int64_t)(5e7) ) { //0.05sec
77
78 dT = (event.timestamp - mGyroTime) / 1000000000.0f;
79 // here we estimate the gyro rate (useful for debugging)
80 const float freq = 1 / dT;
81 if (freq >= 100 && freq<1000) { // filter values obviously wrong
82 const float alpha = 1 / (1 + dT); // 1s time-constant
83 mEstimatedGyroRate = freq + (mEstimatedGyroRate - freq)*alpha;
84 }
85
86 const vec3_t gyro(event.data);
87 for (int i = 0; i<NUM_FUSION_MODE; ++i) {
88 if (mEnabled[i]) {
89 // fusion in no gyro mode will ignore
90 mFusions[i].handleGyro(gyro, dT);
91 }
92 }
93 }
94 mGyroTime = event.timestamp;
95 } else if (event.type == SENSOR_TYPE_MAGNETIC_FIELD) {
96 const vec3_t mag(event.data);
97 for (int i = 0; i<NUM_FUSION_MODE; ++i) {
98 if (mEnabled[i]) {
99 mFusions[i].handleMag(mag);// fusion in no mag mode will ignore
100 }
101 }
102 } else if (event.type == SENSOR_TYPE_ACCELEROMETER) {
103 float dT;
104 if ( event.timestamp - mAccTime> 0 &&
105 event.timestamp - mAccTime< (int64_t)(1e8) ) { //0.1sec
106 dT = (event.timestamp - mAccTime) / 1000000000.0f;
107
108 const vec3_t acc(event.data);
109 for (int i = 0; i<NUM_FUSION_MODE; ++i) {
110 if (mEnabled[i]) {
111 mFusions[i].handleAcc(acc, dT);
112 mAttitudes[i] = mFusions[i].getAttitude();
113 }
114 }
115 }
116 mAccTime = event.timestamp;
117 }
118 }
119
min(T a,T b)120 template <typename T> inline T min(T a, T b) { return a<b ? a : b; }
max(T a,T b)121 template <typename T> inline T max(T a, T b) { return a>b ? a : b; }
122
activate(int mode,void * ident,bool enabled)123 status_t SensorFusion::activate(int mode, void* ident, bool enabled) {
124
125 ALOGD_IF(DEBUG_CONNECTIONS,
126 "SensorFusion::activate(mode=%d, ident=%p, enabled=%d)",
127 mode, ident, enabled);
128
129 const ssize_t idx = mClients[mode].indexOf(ident);
130 if (enabled) {
131 if (idx < 0) {
132 mClients[mode].add(ident);
133 }
134 } else {
135 if (idx >= 0) {
136 mClients[mode].removeItemsAt(idx);
137 }
138 }
139
140 const bool newState = mClients[mode].size() != 0;
141 if (newState != mEnabled[mode]) {
142 mEnabled[mode] = newState;
143 if (newState) {
144 mFusions[mode].init(mode);
145 }
146 }
147
148 mSensorDevice.activate(ident, mAcc.getHandle(), enabled);
149 if (mode != FUSION_NOMAG) {
150 mSensorDevice.activate(ident, mMag.getHandle(), enabled);
151 }
152 if (mode != FUSION_NOGYRO) {
153 mSensorDevice.activate(ident, mGyro.getHandle(), enabled);
154 }
155
156 return NO_ERROR;
157 }
158
setDelay(int mode,void * ident,int64_t ns)159 status_t SensorFusion::setDelay(int mode, void* ident, int64_t ns) {
160 // Call batch with timeout zero instead of setDelay().
161 if (ns > (int64_t)5e7) {
162 ns = (int64_t)(5e7);
163 }
164 mSensorDevice.batch(ident, mAcc.getHandle(), 0, ns, 0);
165 if (mode != FUSION_NOMAG) {
166 mSensorDevice.batch(ident, mMag.getHandle(), 0, ms2ns(10), 0);
167 }
168 if (mode != FUSION_NOGYRO) {
169 mSensorDevice.batch(ident, mGyro.getHandle(), 0, mTargetDelayNs, 0);
170 }
171 return NO_ERROR;
172 }
173
174
getPowerUsage(int mode) const175 float SensorFusion::getPowerUsage(int mode) const {
176 float power = mAcc.getPowerUsage() +
177 ((mode != FUSION_NOMAG) ? mMag.getPowerUsage() : 0) +
178 ((mode != FUSION_NOGYRO) ? mGyro.getPowerUsage() : 0);
179 return power;
180 }
181
getMinDelay() const182 int32_t SensorFusion::getMinDelay() const {
183 return mAcc.getMinDelay();
184 }
185
dump(String8 & result)186 void SensorFusion::dump(String8& result) {
187 const Fusion& fusion_9axis(mFusions[FUSION_9AXIS]);
188 result.appendFormat("9-axis fusion %s (%zd clients), gyro-rate=%7.2fHz, "
189 "q=< %g, %g, %g, %g > (%g), "
190 "b=< %g, %g, %g >\n",
191 mEnabled[FUSION_9AXIS] ? "enabled" : "disabled",
192 mClients[FUSION_9AXIS].size(),
193 mEstimatedGyroRate,
194 fusion_9axis.getAttitude().x,
195 fusion_9axis.getAttitude().y,
196 fusion_9axis.getAttitude().z,
197 fusion_9axis.getAttitude().w,
198 length(fusion_9axis.getAttitude()),
199 fusion_9axis.getBias().x,
200 fusion_9axis.getBias().y,
201 fusion_9axis.getBias().z);
202
203 const Fusion& fusion_nomag(mFusions[FUSION_NOMAG]);
204 result.appendFormat("game fusion(no mag) %s (%zd clients), "
205 "gyro-rate=%7.2fHz, "
206 "q=< %g, %g, %g, %g > (%g), "
207 "b=< %g, %g, %g >\n",
208 mEnabled[FUSION_NOMAG] ? "enabled" : "disabled",
209 mClients[FUSION_NOMAG].size(),
210 mEstimatedGyroRate,
211 fusion_nomag.getAttitude().x,
212 fusion_nomag.getAttitude().y,
213 fusion_nomag.getAttitude().z,
214 fusion_nomag.getAttitude().w,
215 length(fusion_nomag.getAttitude()),
216 fusion_nomag.getBias().x,
217 fusion_nomag.getBias().y,
218 fusion_nomag.getBias().z);
219
220 const Fusion& fusion_nogyro(mFusions[FUSION_NOGYRO]);
221 result.appendFormat("geomag fusion (no gyro) %s (%zd clients), "
222 "gyro-rate=%7.2fHz, "
223 "q=< %g, %g, %g, %g > (%g), "
224 "b=< %g, %g, %g >\n",
225 mEnabled[FUSION_NOGYRO] ? "enabled" : "disabled",
226 mClients[FUSION_NOGYRO].size(),
227 mEstimatedGyroRate,
228 fusion_nogyro.getAttitude().x,
229 fusion_nogyro.getAttitude().y,
230 fusion_nogyro.getAttitude().z,
231 fusion_nogyro.getAttitude().w,
232 length(fusion_nogyro.getAttitude()),
233 fusion_nogyro.getBias().x,
234 fusion_nogyro.getBias().y,
235 fusion_nogyro.getBias().z);
236 }
237
238 // ---------------------------------------------------------------------------
239 }; // namespace android
240