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