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()), 283301542828febc768e1df42892cfac4992c35474Mathias Agopian mEnabled(false), mGyroTime(0) 29984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian{ 30984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian sensor_t const* list; 310319306670b0344da99efa606b6f172dde575a39Mathias Agopian Sensor uncalibratedGyro; 327b2b32f2e761a919deb6f82d978b379429f77b05Mathias Agopian ssize_t count = mSensorDevice.getSensorList(&list); 337b2b32f2e761a919deb6f82d978b379429f77b05Mathias Agopian if (count > 0) { 347b2b32f2e761a919deb6f82d978b379429f77b05Mathias Agopian for (size_t i=0 ; i<size_t(count) ; i++) { 357b2b32f2e761a919deb6f82d978b379429f77b05Mathias Agopian if (list[i].type == SENSOR_TYPE_ACCELEROMETER) { 367b2b32f2e761a919deb6f82d978b379429f77b05Mathias Agopian mAcc = Sensor(list + i); 377b2b32f2e761a919deb6f82d978b379429f77b05Mathias Agopian } 387b2b32f2e761a919deb6f82d978b379429f77b05Mathias Agopian if (list[i].type == SENSOR_TYPE_MAGNETIC_FIELD) { 397b2b32f2e761a919deb6f82d978b379429f77b05Mathias Agopian mMag = Sensor(list + i); 407b2b32f2e761a919deb6f82d978b379429f77b05Mathias Agopian } 417b2b32f2e761a919deb6f82d978b379429f77b05Mathias Agopian if (list[i].type == SENSOR_TYPE_GYROSCOPE) { 427b2b32f2e761a919deb6f82d978b379429f77b05Mathias Agopian mGyro = Sensor(list + i); 437b2b32f2e761a919deb6f82d978b379429f77b05Mathias Agopian } 440319306670b0344da99efa606b6f172dde575a39Mathias Agopian if (list[i].type == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) { 450319306670b0344da99efa606b6f172dde575a39Mathias Agopian uncalibratedGyro = Sensor(list + i); 460319306670b0344da99efa606b6f172dde575a39Mathias Agopian } 470319306670b0344da99efa606b6f172dde575a39Mathias Agopian } 480319306670b0344da99efa606b6f172dde575a39Mathias Agopian 490319306670b0344da99efa606b6f172dde575a39Mathias Agopian // Use the uncalibrated gyroscope for sensor fusion when available 500319306670b0344da99efa606b6f172dde575a39Mathias Agopian if (uncalibratedGyro.getType() == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) { 510319306670b0344da99efa606b6f172dde575a39Mathias Agopian mGyro = uncalibratedGyro; 52984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 530319306670b0344da99efa606b6f172dde575a39Mathias Agopian 540319306670b0344da99efa606b6f172dde575a39Mathias Agopian // 200 Hz for gyro events is a good compromise between precision 550319306670b0344da99efa606b6f172dde575a39Mathias Agopian // and power/cpu usage. 562e2a560c4b60c24258e0eaadc1189eb9dcc1a0b4Mathias Agopian mEstimatedGyroRate = 200; 572e2a560c4b60c24258e0eaadc1189eb9dcc1a0b4Mathias Agopian mTargetDelayNs = 1000000000LL/mEstimatedGyroRate; 587b2b32f2e761a919deb6f82d978b379429f77b05Mathias Agopian mFusion.init(); 59984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 60984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 61984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 62984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopianvoid SensorFusion::process(const sensors_event_t& event) { 630319306670b0344da99efa606b6f172dde575a39Mathias Agopian if (event.type == mGyro.getType()) { 64984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian if (mGyroTime != 0) { 65984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian const float dT = (event.timestamp - mGyroTime) / 1000000000.0f; 662e2a560c4b60c24258e0eaadc1189eb9dcc1a0b4Mathias Agopian mFusion.handleGyro(vec3_t(event.data), dT); 672e2a560c4b60c24258e0eaadc1189eb9dcc1a0b4Mathias Agopian // here we estimate the gyro rate (useful for debugging) 68984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian const float freq = 1 / dT; 693301542828febc768e1df42892cfac4992c35474Mathias Agopian if (freq >= 100 && freq<1000) { // filter values obviously wrong 703301542828febc768e1df42892cfac4992c35474Mathias Agopian const float alpha = 1 / (1 + dT); // 1s time-constant 712e2a560c4b60c24258e0eaadc1189eb9dcc1a0b4Mathias Agopian mEstimatedGyroRate = freq + (mEstimatedGyroRate - freq)*alpha; 723301542828febc768e1df42892cfac4992c35474Mathias Agopian } 73984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 74984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mGyroTime = event.timestamp; 75984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } else if (event.type == SENSOR_TYPE_MAGNETIC_FIELD) { 76984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian const vec3_t mag(event.data); 773301542828febc768e1df42892cfac4992c35474Mathias Agopian mFusion.handleMag(mag); 78984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } else if (event.type == SENSOR_TYPE_ACCELEROMETER) { 79984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian const vec3_t acc(event.data); 803301542828febc768e1df42892cfac4992c35474Mathias Agopian mFusion.handleAcc(acc); 813301542828febc768e1df42892cfac4992c35474Mathias Agopian mAttitude = mFusion.getAttitude(); 82984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 83984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 84984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 85984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopiantemplate <typename T> inline T min(T a, T b) { return a<b ? a : b; } 86984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopiantemplate <typename T> inline T max(T a, T b) { return a>b ? a : b; } 87984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 88984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopianstatus_t SensorFusion::activate(void* ident, bool enabled) { 89984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 90a551237de142549fb8a6608ee9d2fbf4b7ca2ebfSteve Block ALOGD_IF(DEBUG_CONNECTIONS, 91984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian "SensorFusion::activate(ident=%p, enabled=%d)", 92984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian ident, enabled); 93984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 94984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian const ssize_t idx = mClients.indexOf(ident); 95984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian if (enabled) { 96984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian if (idx < 0) { 97984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mClients.add(ident); 98984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 99984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } else { 100984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian if (idx >= 0) { 101984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mClients.removeItemsAt(idx); 102984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 103984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 104984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 105724d91d778e71c8186399f4955de14b54812b3edAravind Akella if (enabled) { 106bf72deea2f9982a09c6a95f94cfa1654bc8c684fAravind Akella ALOGD_IF(DEBUG_CONNECTIONS, "SensorFusion calling batch ident=%p ", ident); 107724d91d778e71c8186399f4955de14b54812b3edAravind Akella // Activating a sensor in continuous mode is equivalent to calling batch with the default 108724d91d778e71c8186399f4955de14b54812b3edAravind Akella // period and timeout equal to ZERO, followed by a call to activate. 109724d91d778e71c8186399f4955de14b54812b3edAravind Akella mSensorDevice.batch(ident, mAcc.getHandle(), 0, DEFAULT_EVENTS_PERIOD, 0); 110724d91d778e71c8186399f4955de14b54812b3edAravind Akella mSensorDevice.batch(ident, mMag.getHandle(), 0, DEFAULT_EVENTS_PERIOD, 0); 111724d91d778e71c8186399f4955de14b54812b3edAravind Akella mSensorDevice.batch(ident, mGyro.getHandle(), 0, DEFAULT_EVENTS_PERIOD, 0); 112724d91d778e71c8186399f4955de14b54812b3edAravind Akella } 113724d91d778e71c8186399f4955de14b54812b3edAravind Akella 114984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mSensorDevice.activate(ident, mAcc.getHandle(), enabled); 115984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mSensorDevice.activate(ident, mMag.getHandle(), enabled); 1163301542828febc768e1df42892cfac4992c35474Mathias Agopian mSensorDevice.activate(ident, mGyro.getHandle(), enabled); 117984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 118984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian const bool newState = mClients.size() != 0; 119984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian if (newState != mEnabled) { 120984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mEnabled = newState; 121984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian if (newState) { 122984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mFusion.init(); 1233301542828febc768e1df42892cfac4992c35474Mathias Agopian mGyroTime = 0; 124984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 125984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 126984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian return NO_ERROR; 127984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 128984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 129984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopianstatus_t SensorFusion::setDelay(void* ident, int64_t ns) { 1303301542828febc768e1df42892cfac4992c35474Mathias Agopian mSensorDevice.setDelay(ident, mAcc.getHandle(), ns); 1313301542828febc768e1df42892cfac4992c35474Mathias Agopian mSensorDevice.setDelay(ident, mMag.getHandle(), ms2ns(20)); 1323301542828febc768e1df42892cfac4992c35474Mathias Agopian mSensorDevice.setDelay(ident, mGyro.getHandle(), mTargetDelayNs); 133984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian return NO_ERROR; 134984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 135984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 136984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 137984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopianfloat SensorFusion::getPowerUsage() const { 1383301542828febc768e1df42892cfac4992c35474Mathias Agopian float power = mAcc.getPowerUsage() + 1393301542828febc768e1df42892cfac4992c35474Mathias Agopian mMag.getPowerUsage() + 1403301542828febc768e1df42892cfac4992c35474Mathias Agopian mGyro.getPowerUsage(); 141984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian return power; 142984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 143984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 144984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopianint32_t SensorFusion::getMinDelay() const { 145984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian return mAcc.getMinDelay(); 146984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 147984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 148ba02cd2f6cc3f59adf66cb2b9176bfe6c9e382d1Mathias Agopianvoid SensorFusion::dump(String8& result) { 149984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian const Fusion& fusion(mFusion); 150ba02cd2f6cc3f59adf66cb2b9176bfe6c9e382d1Mathias Agopian result.appendFormat("9-axis fusion %s (%d clients), gyro-rate=%7.2fHz, " 1513301542828febc768e1df42892cfac4992c35474Mathias Agopian "q=< %g, %g, %g, %g > (%g), " 1523301542828febc768e1df42892cfac4992c35474Mathias Agopian "b=< %g, %g, %g >\n", 153984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mEnabled ? "enabled" : "disabled", 154984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mClients.size(), 1552e2a560c4b60c24258e0eaadc1189eb9dcc1a0b4Mathias Agopian mEstimatedGyroRate, 156984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian fusion.getAttitude().x, 157984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian fusion.getAttitude().y, 158984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian fusion.getAttitude().z, 1593301542828febc768e1df42892cfac4992c35474Mathias Agopian fusion.getAttitude().w, 1603301542828febc768e1df42892cfac4992c35474Mathias Agopian length(fusion.getAttitude()), 161984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian fusion.getBias().x, 162984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian fusion.getBias().y, 163984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian fusion.getBias().z); 164984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 165984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 166984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian// --------------------------------------------------------------------------- 167984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian}; // namespace android 168