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