SensorFusion.cpp revision 984826cc158193e61e3a00359ef4f6699c7d748a
1984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian/* 2984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * Copyright (C) 2011 The Android Open Source Project 3984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * 4984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * Licensed under the Apache License, Version 2.0 (the "License"); 5984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * you may not use this file except in compliance with the License. 6984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * You may obtain a copy of the License at 7984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * 8984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * http://www.apache.org/licenses/LICENSE-2.0 9984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * 10984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * Unless required by applicable law or agreed to in writing, software 11984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * distributed under the License is distributed on an "AS IS" BASIS, 12984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * See the License for the specific language governing permissions and 14984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian * limitations under the License. 15984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian */ 16984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 17984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian#include "SensorDevice.h" 18984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian#include "SensorFusion.h" 19984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian#include "SensorService.h" 20984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 21984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopiannamespace android { 22984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian// --------------------------------------------------------------------------- 23984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 24984826cc158193e61e3a00359ef4f6699c7d748aMathias AgopianANDROID_SINGLETON_STATIC_INSTANCE(SensorFusion) 25984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 26984826cc158193e61e3a00359ef4f6699c7d748aMathias AgopianSensorFusion::SensorFusion() 27984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian : mSensorDevice(SensorDevice::getInstance()), 28984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mEnabled(false), mHasGyro(false), mGyroTime(0), mRotationMatrix(1), 29984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mLowPass(M_SQRT1_2, 1.0f), mAccData(mLowPass), 30984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mFilteredMag(0.0f), mFilteredAcc(0.0f) 31984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian{ 32984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian sensor_t const* list; 33984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian size_t count = mSensorDevice.getSensorList(&list); 34984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian for (size_t i=0 ; i<count ; i++) { 35984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian if (list[i].type == SENSOR_TYPE_ACCELEROMETER) { 36984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mAcc = Sensor(list + i); 37984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 38984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian if (list[i].type == SENSOR_TYPE_MAGNETIC_FIELD) { 39984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mMag = Sensor(list + i); 40984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 41984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian if (list[i].type == SENSOR_TYPE_GYROSCOPE) { 42984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mGyro = Sensor(list + i); 43984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian // 200 Hz for gyro events is a good compromise between precision 44984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian // and power/cpu usage. 45984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mTargetDelayNs = 1000000000LL/200; 46984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mGyroRate = 1000000000.0f / mTargetDelayNs; 47984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mHasGyro = true; 48984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 49984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 50984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mFusion.init(); 51984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mAccData.init(vec3_t(0.0f)); 52984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 53984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 54984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopianvoid SensorFusion::process(const sensors_event_t& event) { 55984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 56984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian if (event.type == SENSOR_TYPE_GYROSCOPE && mHasGyro) { 57984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian if (mGyroTime != 0) { 58984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian const float dT = (event.timestamp - mGyroTime) / 1000000000.0f; 59984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian const float freq = 1 / dT; 60984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian const float alpha = 2 / (2 + dT); // 2s time-constant 61984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mGyroRate = mGyroRate*alpha + freq*(1 - alpha); 62984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 63984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mGyroTime = event.timestamp; 64984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mFusion.handleGyro(vec3_t(event.data), 1.0f/mGyroRate); 65984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } else if (event.type == SENSOR_TYPE_MAGNETIC_FIELD) { 66984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian const vec3_t mag(event.data); 67984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian if (mHasGyro) { 68984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mFusion.handleMag(mag); 69984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } else { 70984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian const float l(length(mag)); 71984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian if (l>5 && l<100) { 72984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mFilteredMag = mag * (1/l); 73984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 74984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 75984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } else if (event.type == SENSOR_TYPE_ACCELEROMETER) { 76984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian const vec3_t acc(event.data); 77984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian if (mHasGyro) { 78984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mFusion.handleAcc(acc); 79984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mRotationMatrix = mFusion.getRotationMatrix(); 80984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } else { 81984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian const float l(length(acc)); 82984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian if (l > 0.981f) { 83984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian // remove the linear-acceleration components 84984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mFilteredAcc = mAccData(acc * (1/l)); 85984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 86984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian if (length(mFilteredAcc)>0 && length(mFilteredMag)>0) { 87984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian vec3_t up(mFilteredAcc); 88984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian vec3_t east(cross_product(mFilteredMag, up)); 89984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian east *= 1/length(east); 90984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian vec3_t north(cross_product(up, east)); 91984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mRotationMatrix << east << north << up; 92984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 93984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 94984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 95984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 96984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 97984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopiantemplate <typename T> inline T min(T a, T b) { return a<b ? a : b; } 98984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopiantemplate <typename T> inline T max(T a, T b) { return a>b ? a : b; } 99984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 100984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopianstatus_t SensorFusion::activate(void* ident, bool enabled) { 101984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 102984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian LOGD_IF(DEBUG_CONNECTIONS, 103984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian "SensorFusion::activate(ident=%p, enabled=%d)", 104984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian ident, enabled); 105984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 106984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian const ssize_t idx = mClients.indexOf(ident); 107984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian if (enabled) { 108984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian if (idx < 0) { 109984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mClients.add(ident); 110984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 111984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } else { 112984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian if (idx >= 0) { 113984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mClients.removeItemsAt(idx); 114984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 115984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 116984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 117984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mSensorDevice.activate(ident, mAcc.getHandle(), enabled); 118984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mSensorDevice.activate(ident, mMag.getHandle(), enabled); 119984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian if (mHasGyro) { 120984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mSensorDevice.activate(ident, mGyro.getHandle(), enabled); 121984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 122984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 123984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian const bool newState = mClients.size() != 0; 124984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian if (newState != mEnabled) { 125984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mEnabled = newState; 126984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian if (newState) { 127984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mFusion.init(); 128984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 129984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 130984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian return NO_ERROR; 131984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 132984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 133984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopianstatus_t SensorFusion::setDelay(void* ident, int64_t ns) { 134984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian if (mHasGyro) { 135984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mSensorDevice.setDelay(ident, mAcc.getHandle(), ns); 136984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mSensorDevice.setDelay(ident, mMag.getHandle(), ms2ns(20)); 137984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mSensorDevice.setDelay(ident, mGyro.getHandle(), mTargetDelayNs); 138984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } else { 139984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian const static double NS2S = 1.0 / 1000000000.0; 140984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mSensorDevice.setDelay(ident, mAcc.getHandle(), ns); 141984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mSensorDevice.setDelay(ident, mMag.getHandle(), max(ns, mMag.getMinDelayNs())); 142984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mLowPass.setSamplingPeriod(ns*NS2S); 143984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 144984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian return NO_ERROR; 145984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 146984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 147984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 148984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopianfloat SensorFusion::getPowerUsage() const { 149984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian float power = mAcc.getPowerUsage() + mMag.getPowerUsage(); 150984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian if (mHasGyro) { 151984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian power += mGyro.getPowerUsage(); 152984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 153984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian return power; 154984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 155984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 156984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopianint32_t SensorFusion::getMinDelay() const { 157984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian return mAcc.getMinDelay(); 158984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 159984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 160984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopianvoid SensorFusion::dump(String8& result, char* buffer, size_t SIZE) { 161984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian const Fusion& fusion(mFusion); 162984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian snprintf(buffer, SIZE, "Fusion (%s) %s (%d clients), gyro-rate=%7.2fHz, " 163984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian "MRPS=< %g, %g, %g > (%g), " 164984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian "BIAS=< %g, %g, %g >\n", 165984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mHasGyro ? "9-axis" : "6-axis", 166984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mEnabled ? "enabled" : "disabled", 167984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mClients.size(), 168984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mGyroRate, 169984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian fusion.getAttitude().x, 170984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian fusion.getAttitude().y, 171984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian fusion.getAttitude().z, 172984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian dot_product(fusion.getAttitude(), fusion.getAttitude()), 173984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian fusion.getBias().x, 174984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian fusion.getBias().y, 175984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian fusion.getBias().z); 176984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian result.append(buffer); 177984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 178984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 179984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian// --------------------------------------------------------------------------- 180984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian}; // namespace android 181