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