SensorFusion.cpp revision 92dc3fc52cf097bd105460cf377779bdcf146d62 
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      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
62void 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
85template <typename T> inline T min(T a, T b) { return a<b ? a : b; }
86template <typename T> inline T max(T a, T b) { return a>b ? a : b; }
87
88status_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    if (enabled) {
106        ALOGD_IF(DEBUG_CONNECTIONS, "SensorFusion calling batch ident=%p ", ident);
107        // Activating a sensor in continuous mode is equivalent to calling batch with the default
108        // period and timeout equal to ZERO, followed by a call to activate.
109        mSensorDevice.batch(ident, mAcc.getHandle(), 0, DEFAULT_EVENTS_PERIOD, 0);
110        mSensorDevice.batch(ident, mMag.getHandle(), 0, DEFAULT_EVENTS_PERIOD, 0);
111        mSensorDevice.batch(ident, mGyro.getHandle(), 0, DEFAULT_EVENTS_PERIOD, 0);
112    }
113
114    mSensorDevice.activate(ident, mAcc.getHandle(), enabled);
115    mSensorDevice.activate(ident, mMag.getHandle(), enabled);
116    mSensorDevice.activate(ident, mGyro.getHandle(), enabled);
117
118    const bool newState = mClients.size() != 0;
119    if (newState != mEnabled) {
120        mEnabled = newState;
121        if (newState) {
122            mFusion.init();
123            mGyroTime = 0;
124        }
125    }
126    return NO_ERROR;
127}
128
129status_t SensorFusion::setDelay(void* ident, int64_t ns) {
130    mSensorDevice.setDelay(ident, mAcc.getHandle(), ns);
131    mSensorDevice.setDelay(ident, mMag.getHandle(), ms2ns(20));
132    mSensorDevice.setDelay(ident, mGyro.getHandle(), mTargetDelayNs);
133    return NO_ERROR;
134}
135
136
137float SensorFusion::getPowerUsage() const {
138    float power =   mAcc.getPowerUsage() +
139                    mMag.getPowerUsage() +
140                    mGyro.getPowerUsage();
141    return power;
142}
143
144int32_t SensorFusion::getMinDelay() const {
145    return mAcc.getMinDelay();
146}
147
148void SensorFusion::dump(String8& result) {
149    const Fusion& fusion(mFusion);
150    result.appendFormat("9-axis fusion %s (%zd clients), gyro-rate=%7.2fHz, "
151            "q=< %g, %g, %g, %g > (%g), "
152            "b=< %g, %g, %g >\n",
153            mEnabled ? "enabled" : "disabled",
154            mClients.size(),
155            mEstimatedGyroRate,
156            fusion.getAttitude().x,
157            fusion.getAttitude().y,
158            fusion.getAttitude().z,
159            fusion.getAttitude().w,
160            length(fusion.getAttitude()),
161            fusion.getBias().x,
162            fusion.getBias().y,
163            fusion.getBias().z);
164}
165
166// ---------------------------------------------------------------------------
167}; // namespace android
168