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