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), 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 52 void 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 74 template <typename T> inline T min(T a, T b) { return a<b ? a : b; } 75 template <typename T> inline T max(T a, T b) { return a>b ? a : b; } 76 77 status_t SensorFusion::activate(void* ident, bool enabled) { 78 79 LOGD_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 109 status_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 117 float SensorFusion::getPowerUsage() const { 118 float power = mAcc.getPowerUsage() + 119 mMag.getPowerUsage() + 120 mGyro.getPowerUsage(); 121 return power; 122 } 123 124 int32_t SensorFusion::getMinDelay() const { 125 return mAcc.getMinDelay(); 126 } 127 128 void 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