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       mEnabled(false), mGyroTime(0)
29 {
30     sensor_t const* list;
31     Sensor uncalibratedGyro;
32     ssize_t count = mSensorDevice.getSensorList(&list);
33     if (count > 0) {
34         for (size_t i=0 ; i<size_t(count) ; i++) {
35             if (list[i].type == SENSOR_TYPE_ACCELEROMETER) {
36                 mAcc = Sensor(list + i);
37             }
38             if (list[i].type == SENSOR_TYPE_MAGNETIC_FIELD) {
39                 mMag = Sensor(list + i);
40             }
41             if (list[i].type == SENSOR_TYPE_GYROSCOPE) {
42                 mGyro = Sensor(list + i);
43             }
44             if (list[i].type == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) {
45                 uncalibratedGyro = Sensor(list + i);
46             }
47         }
48 
49         // Use the uncalibrated gyroscope for sensor fusion when available
50         if (uncalibratedGyro.getType() == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) {
51             mGyro = uncalibratedGyro;
52         }
53 
54         // 200 Hz for gyro events is a good compromise between precision
55         // and power/cpu usage.
56         mEstimatedGyroRate = 200;
57         mTargetDelayNs = 1000000000LL/mEstimatedGyroRate;
58         mFusion.init();
59     }
60 }
61 
process(const sensors_event_t & event)62 void SensorFusion::process(const sensors_event_t& event) {
63     if (event.type == mGyro.getType()) {
64         if (mGyroTime != 0) {
65             const float dT = (event.timestamp - mGyroTime) / 1000000000.0f;
66             mFusion.handleGyro(vec3_t(event.data), dT);
67             // here we estimate the gyro rate (useful for debugging)
68             const float freq = 1 / dT;
69             if (freq >= 100 && freq<1000) { // filter values obviously wrong
70                 const float alpha = 1 / (1 + dT); // 1s time-constant
71                 mEstimatedGyroRate = freq + (mEstimatedGyroRate - freq)*alpha;
72             }
73         }
74         mGyroTime = event.timestamp;
75     } else if (event.type == SENSOR_TYPE_MAGNETIC_FIELD) {
76         const vec3_t mag(event.data);
77         mFusion.handleMag(mag);
78     } else if (event.type == SENSOR_TYPE_ACCELEROMETER) {
79         const vec3_t acc(event.data);
80         mFusion.handleAcc(acc);
81         mAttitude = mFusion.getAttitude();
82     }
83 }
84 
min(T a,T b)85 template <typename T> inline T min(T a, T b) { return a<b ? a : b; }
max(T a,T b)86 template <typename T> inline T max(T a, T b) { return a>b ? a : b; }
87 
activate(void * ident,bool enabled)88 status_t SensorFusion::activate(void* ident, bool enabled) {
89 
90     ALOGD_IF(DEBUG_CONNECTIONS,
91             "SensorFusion::activate(ident=%p, enabled=%d)",
92             ident, enabled);
93 
94     const ssize_t idx = mClients.indexOf(ident);
95     if (enabled) {
96         if (idx < 0) {
97             mClients.add(ident);
98         }
99     } else {
100         if (idx >= 0) {
101             mClients.removeItemsAt(idx);
102         }
103     }
104 
105     mSensorDevice.activate(ident, mAcc.getHandle(), enabled);
106     mSensorDevice.activate(ident, mMag.getHandle(), enabled);
107     mSensorDevice.activate(ident, mGyro.getHandle(), enabled);
108 
109     const bool newState = mClients.size() != 0;
110     if (newState != mEnabled) {
111         mEnabled = newState;
112         if (newState) {
113             mFusion.init();
114             mGyroTime = 0;
115         }
116     }
117     return NO_ERROR;
118 }
119 
setDelay(void * ident,int64_t ns)120 status_t SensorFusion::setDelay(void* ident, int64_t ns) {
121     // Call batch with timeout zero instead of setDelay().
122     mSensorDevice.batch(ident, mAcc.getHandle(), 0, ns, 0);
123     mSensorDevice.batch(ident, mMag.getHandle(), 0, ms2ns(20), 0);
124     mSensorDevice.batch(ident, mGyro.getHandle(), 0, mTargetDelayNs, 0);
125     return NO_ERROR;
126 }
127 
128 
getPowerUsage() const129 float SensorFusion::getPowerUsage() const {
130     float power =   mAcc.getPowerUsage() +
131                     mMag.getPowerUsage() +
132                     mGyro.getPowerUsage();
133     return power;
134 }
135 
getMinDelay() const136 int32_t SensorFusion::getMinDelay() const {
137     return mAcc.getMinDelay();
138 }
139 
dump(String8 & result)140 void SensorFusion::dump(String8& result) {
141     const Fusion& fusion(mFusion);
142     result.appendFormat("9-axis fusion %s (%zd clients), gyro-rate=%7.2fHz, "
143             "q=< %g, %g, %g, %g > (%g), "
144             "b=< %g, %g, %g >\n",
145             mEnabled ? "enabled" : "disabled",
146             mClients.size(),
147             mEstimatedGyroRate,
148             fusion.getAttitude().x,
149             fusion.getAttitude().y,
150             fusion.getAttitude().z,
151             fusion.getAttitude().w,
152             length(fusion.getAttitude()),
153             fusion.getBias().x,
154             fusion.getBias().y,
155             fusion.getBias().z);
156 }
157 
158 // ---------------------------------------------------------------------------
159 }; // namespace android
160