SensorFusion.cpp revision 88509090387eeb400771a786171866710cca230c
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 mSensorDevice.activate(ident, mAcc.getHandle(), enabled); 106 mSensorDevice.activate(ident, mMag.getHandle(), enabled); 107 mSensorDevice.activate(ident, mGyro.getHandle(), enabled); 108 109 const bool newState = mClients.size() != 0; 110 if (newState != mEnabled) { 111 mEnabled = newState; 112 if (newState) { 113 mFusion.init(); 114 mGyroTime = 0; 115 } 116 } 117 return NO_ERROR; 118} 119 120status_t SensorFusion::setDelay(void* ident, int64_t ns) { 121 // Call batch with timeout zero instead of setDelay(). 122 mSensorDevice.batch(ident, mAcc.getHandle(), 0, ns, 0); 123 mSensorDevice.batch(ident, mMag.getHandle(), 0, ms2ns(20), 0); 124 mSensorDevice.batch(ident, mGyro.getHandle(), 0, mTargetDelayNs, 0); 125 return NO_ERROR; 126} 127 128 129float SensorFusion::getPowerUsage() const { 130 float power = mAcc.getPowerUsage() + 131 mMag.getPowerUsage() + 132 mGyro.getPowerUsage(); 133 return power; 134} 135 136int32_t SensorFusion::getMinDelay() const { 137 return mAcc.getMinDelay(); 138} 139 140void SensorFusion::dump(String8& result) { 141 const Fusion& fusion(mFusion); 142 result.appendFormat("9-axis fusion %s (%d clients), gyro-rate=%7.2fHz, " 143 "q=< %g, %g, %g, %g > (%g), " 144 "b=< %g, %g, %g >\n", 145 mEnabled ? "enabled" : "disabled", 146 mClients.size(), 147 mEstimatedGyroRate, 148 fusion.getAttitude().x, 149 fusion.getAttitude().y, 150 fusion.getAttitude().z, 151 fusion.getAttitude().w, 152 length(fusion.getAttitude()), 153 fusion.getBias().x, 154 fusion.getBias().y, 155 fusion.getBias().z); 156} 157 158// --------------------------------------------------------------------------- 159}; // namespace android 160