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
21namespace android {
22// ---------------------------------------------------------------------------
23
24ANDROID_SINGLETON_STATIC_INSTANCE(SensorFusion)
25
26SensorFusion::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
71void 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
120template <typename T> inline T min(T a, T b) { return a<b ? a : b; }
121template <typename T> inline T max(T a, T b) { return a>b ? a : b; }
122
123status_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
159status_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
175float 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
182int32_t SensorFusion::getMinDelay() const {
183    return mAcc.getMinDelay();
184}
185
186void 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