Mathias Agopian | 984826c | 2011-05-17 22:54:42 -0700 | [diff] [blame^] | 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 | |
| 24 | ANDROID_SINGLETON_STATIC_INSTANCE(SensorFusion) |
| 25 | |
| 26 | SensorFusion::SensorFusion() |
| 27 | : mSensorDevice(SensorDevice::getInstance()), |
| 28 | mEnabled(false), mHasGyro(false), mGyroTime(0), mRotationMatrix(1), |
| 29 | mLowPass(M_SQRT1_2, 1.0f), mAccData(mLowPass), |
| 30 | mFilteredMag(0.0f), mFilteredAcc(0.0f) |
| 31 | { |
| 32 | sensor_t const* list; |
| 33 | size_t count = mSensorDevice.getSensorList(&list); |
| 34 | for (size_t i=0 ; i<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 | // 200 Hz for gyro events is a good compromise between precision |
| 44 | // and power/cpu usage. |
| 45 | mTargetDelayNs = 1000000000LL/200; |
| 46 | mGyroRate = 1000000000.0f / mTargetDelayNs; |
| 47 | mHasGyro = true; |
| 48 | } |
| 49 | } |
| 50 | mFusion.init(); |
| 51 | mAccData.init(vec3_t(0.0f)); |
| 52 | } |
| 53 | |
| 54 | void SensorFusion::process(const sensors_event_t& event) { |
| 55 | |
| 56 | if (event.type == SENSOR_TYPE_GYROSCOPE && mHasGyro) { |
| 57 | if (mGyroTime != 0) { |
| 58 | const float dT = (event.timestamp - mGyroTime) / 1000000000.0f; |
| 59 | const float freq = 1 / dT; |
| 60 | const float alpha = 2 / (2 + dT); // 2s time-constant |
| 61 | mGyroRate = mGyroRate*alpha + freq*(1 - alpha); |
| 62 | } |
| 63 | mGyroTime = event.timestamp; |
| 64 | mFusion.handleGyro(vec3_t(event.data), 1.0f/mGyroRate); |
| 65 | } else if (event.type == SENSOR_TYPE_MAGNETIC_FIELD) { |
| 66 | const vec3_t mag(event.data); |
| 67 | if (mHasGyro) { |
| 68 | mFusion.handleMag(mag); |
| 69 | } else { |
| 70 | const float l(length(mag)); |
| 71 | if (l>5 && l<100) { |
| 72 | mFilteredMag = mag * (1/l); |
| 73 | } |
| 74 | } |
| 75 | } else if (event.type == SENSOR_TYPE_ACCELEROMETER) { |
| 76 | const vec3_t acc(event.data); |
| 77 | if (mHasGyro) { |
| 78 | mFusion.handleAcc(acc); |
| 79 | mRotationMatrix = mFusion.getRotationMatrix(); |
| 80 | } else { |
| 81 | const float l(length(acc)); |
| 82 | if (l > 0.981f) { |
| 83 | // remove the linear-acceleration components |
| 84 | mFilteredAcc = mAccData(acc * (1/l)); |
| 85 | } |
| 86 | if (length(mFilteredAcc)>0 && length(mFilteredMag)>0) { |
| 87 | vec3_t up(mFilteredAcc); |
| 88 | vec3_t east(cross_product(mFilteredMag, up)); |
| 89 | east *= 1/length(east); |
| 90 | vec3_t north(cross_product(up, east)); |
| 91 | mRotationMatrix << east << north << up; |
| 92 | } |
| 93 | } |
| 94 | } |
| 95 | } |
| 96 | |
| 97 | template <typename T> inline T min(T a, T b) { return a<b ? a : b; } |
| 98 | template <typename T> inline T max(T a, T b) { return a>b ? a : b; } |
| 99 | |
| 100 | status_t SensorFusion::activate(void* ident, bool enabled) { |
| 101 | |
| 102 | LOGD_IF(DEBUG_CONNECTIONS, |
| 103 | "SensorFusion::activate(ident=%p, enabled=%d)", |
| 104 | ident, enabled); |
| 105 | |
| 106 | const ssize_t idx = mClients.indexOf(ident); |
| 107 | if (enabled) { |
| 108 | if (idx < 0) { |
| 109 | mClients.add(ident); |
| 110 | } |
| 111 | } else { |
| 112 | if (idx >= 0) { |
| 113 | mClients.removeItemsAt(idx); |
| 114 | } |
| 115 | } |
| 116 | |
| 117 | mSensorDevice.activate(ident, mAcc.getHandle(), enabled); |
| 118 | mSensorDevice.activate(ident, mMag.getHandle(), enabled); |
| 119 | if (mHasGyro) { |
| 120 | mSensorDevice.activate(ident, mGyro.getHandle(), enabled); |
| 121 | } |
| 122 | |
| 123 | const bool newState = mClients.size() != 0; |
| 124 | if (newState != mEnabled) { |
| 125 | mEnabled = newState; |
| 126 | if (newState) { |
| 127 | mFusion.init(); |
| 128 | } |
| 129 | } |
| 130 | return NO_ERROR; |
| 131 | } |
| 132 | |
| 133 | status_t SensorFusion::setDelay(void* ident, int64_t ns) { |
| 134 | if (mHasGyro) { |
| 135 | mSensorDevice.setDelay(ident, mAcc.getHandle(), ns); |
| 136 | mSensorDevice.setDelay(ident, mMag.getHandle(), ms2ns(20)); |
| 137 | mSensorDevice.setDelay(ident, mGyro.getHandle(), mTargetDelayNs); |
| 138 | } else { |
| 139 | const static double NS2S = 1.0 / 1000000000.0; |
| 140 | mSensorDevice.setDelay(ident, mAcc.getHandle(), ns); |
| 141 | mSensorDevice.setDelay(ident, mMag.getHandle(), max(ns, mMag.getMinDelayNs())); |
| 142 | mLowPass.setSamplingPeriod(ns*NS2S); |
| 143 | } |
| 144 | return NO_ERROR; |
| 145 | } |
| 146 | |
| 147 | |
| 148 | float SensorFusion::getPowerUsage() const { |
| 149 | float power = mAcc.getPowerUsage() + mMag.getPowerUsage(); |
| 150 | if (mHasGyro) { |
| 151 | power += mGyro.getPowerUsage(); |
| 152 | } |
| 153 | return power; |
| 154 | } |
| 155 | |
| 156 | int32_t SensorFusion::getMinDelay() const { |
| 157 | return mAcc.getMinDelay(); |
| 158 | } |
| 159 | |
| 160 | void SensorFusion::dump(String8& result, char* buffer, size_t SIZE) { |
| 161 | const Fusion& fusion(mFusion); |
| 162 | snprintf(buffer, SIZE, "Fusion (%s) %s (%d clients), gyro-rate=%7.2fHz, " |
| 163 | "MRPS=< %g, %g, %g > (%g), " |
| 164 | "BIAS=< %g, %g, %g >\n", |
| 165 | mHasGyro ? "9-axis" : "6-axis", |
| 166 | mEnabled ? "enabled" : "disabled", |
| 167 | mClients.size(), |
| 168 | mGyroRate, |
| 169 | fusion.getAttitude().x, |
| 170 | fusion.getAttitude().y, |
| 171 | fusion.getAttitude().z, |
| 172 | dot_product(fusion.getAttitude(), fusion.getAttitude()), |
| 173 | fusion.getBias().x, |
| 174 | fusion.getBias().y, |
| 175 | fusion.getBias().z); |
| 176 | result.append(buffer); |
| 177 | } |
| 178 | |
| 179 | // --------------------------------------------------------------------------- |
| 180 | }; // namespace android |