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()), 28f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu mAttitude(mAttitudes[FUSION_9AXIS]), 29f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu mGyroTime(0), mAccTime(0) 30984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian{ 31984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian sensor_t const* list; 320319306670b0344da99efa606b6f172dde575a39Mathias Agopian Sensor uncalibratedGyro; 337b2b32f2e761a919deb6f82d978b379429f77b05Mathias Agopian ssize_t count = mSensorDevice.getSensorList(&list); 34f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu 35f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu mEnabled[FUSION_9AXIS] = false; 36f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu mEnabled[FUSION_NOMAG] = false; 37f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu mEnabled[FUSION_NOGYRO] = false; 38f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu 397b2b32f2e761a919deb6f82d978b379429f77b05Mathias Agopian if (count > 0) { 407b2b32f2e761a919deb6f82d978b379429f77b05Mathias Agopian for (size_t i=0 ; i<size_t(count) ; i++) { 417b2b32f2e761a919deb6f82d978b379429f77b05Mathias Agopian if (list[i].type == SENSOR_TYPE_ACCELEROMETER) { 427b2b32f2e761a919deb6f82d978b379429f77b05Mathias Agopian mAcc = Sensor(list + i); 437b2b32f2e761a919deb6f82d978b379429f77b05Mathias Agopian } 447b2b32f2e761a919deb6f82d978b379429f77b05Mathias Agopian if (list[i].type == SENSOR_TYPE_MAGNETIC_FIELD) { 457b2b32f2e761a919deb6f82d978b379429f77b05Mathias Agopian mMag = Sensor(list + i); 467b2b32f2e761a919deb6f82d978b379429f77b05Mathias Agopian } 477b2b32f2e761a919deb6f82d978b379429f77b05Mathias Agopian if (list[i].type == SENSOR_TYPE_GYROSCOPE) { 487b2b32f2e761a919deb6f82d978b379429f77b05Mathias Agopian mGyro = Sensor(list + i); 497b2b32f2e761a919deb6f82d978b379429f77b05Mathias Agopian } 500319306670b0344da99efa606b6f172dde575a39Mathias Agopian if (list[i].type == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) { 510319306670b0344da99efa606b6f172dde575a39Mathias Agopian uncalibratedGyro = Sensor(list + i); 520319306670b0344da99efa606b6f172dde575a39Mathias Agopian } 530319306670b0344da99efa606b6f172dde575a39Mathias Agopian } 540319306670b0344da99efa606b6f172dde575a39Mathias Agopian 550319306670b0344da99efa606b6f172dde575a39Mathias Agopian // Use the uncalibrated gyroscope for sensor fusion when available 560319306670b0344da99efa606b6f172dde575a39Mathias Agopian if (uncalibratedGyro.getType() == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) { 570319306670b0344da99efa606b6f172dde575a39Mathias Agopian mGyro = uncalibratedGyro; 58984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 590319306670b0344da99efa606b6f172dde575a39Mathias Agopian 600319306670b0344da99efa606b6f172dde575a39Mathias Agopian // 200 Hz for gyro events is a good compromise between precision 610319306670b0344da99efa606b6f172dde575a39Mathias Agopian // and power/cpu usage. 622e2a560c4b60c24258e0eaadc1189eb9dcc1a0b4Mathias Agopian mEstimatedGyroRate = 200; 632e2a560c4b60c24258e0eaadc1189eb9dcc1a0b4Mathias Agopian mTargetDelayNs = 1000000000LL/mEstimatedGyroRate; 64f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu 65f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu for (int i = 0; i<NUM_FUSION_MODE; ++i) { 66f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu mFusions[i].init(i); 67f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu } 68984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 69984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 70984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 71984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopianvoid SensorFusion::process(const sensors_event_t& event) { 72f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu 730319306670b0344da99efa606b6f172dde575a39Mathias Agopian if (event.type == mGyro.getType()) { 74f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu float dT; 75f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu if ( event.timestamp - mGyroTime> 0 && 76f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu event.timestamp - mGyroTime< (int64_t)(5e7) ) { //0.05sec 77f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu 78f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu dT = (event.timestamp - mGyroTime) / 1000000000.0f; 792e2a560c4b60c24258e0eaadc1189eb9dcc1a0b4Mathias Agopian // here we estimate the gyro rate (useful for debugging) 80984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian const float freq = 1 / dT; 813301542828febc768e1df42892cfac4992c35474Mathias Agopian if (freq >= 100 && freq<1000) { // filter values obviously wrong 823301542828febc768e1df42892cfac4992c35474Mathias Agopian const float alpha = 1 / (1 + dT); // 1s time-constant 832e2a560c4b60c24258e0eaadc1189eb9dcc1a0b4Mathias Agopian mEstimatedGyroRate = freq + (mEstimatedGyroRate - freq)*alpha; 843301542828febc768e1df42892cfac4992c35474Mathias Agopian } 85f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu 86f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu const vec3_t gyro(event.data); 87f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu for (int i = 0; i<NUM_FUSION_MODE; ++i) { 88f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu if (mEnabled[i]) { 89f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu // fusion in no gyro mode will ignore 90f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu mFusions[i].handleGyro(gyro, dT); 91f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu } 92f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu } 93984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 94984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian mGyroTime = event.timestamp; 95984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } else if (event.type == SENSOR_TYPE_MAGNETIC_FIELD) { 96984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian const vec3_t mag(event.data); 97f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu for (int i = 0; i<NUM_FUSION_MODE; ++i) { 98f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu if (mEnabled[i]) { 99f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu mFusions[i].handleMag(mag);// fusion in no mag mode will ignore 100f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu } 101f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu } 102984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } else if (event.type == SENSOR_TYPE_ACCELEROMETER) { 103f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu float dT; 104f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu if ( event.timestamp - mAccTime> 0 && 105f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu event.timestamp - mAccTime< (int64_t)(1e8) ) { //0.1sec 106f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu dT = (event.timestamp - mAccTime) / 1000000000.0f; 107f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu 108f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu const vec3_t acc(event.data); 109f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu for (int i = 0; i<NUM_FUSION_MODE; ++i) { 110f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu if (mEnabled[i]) { 111f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu mFusions[i].handleAcc(acc, dT); 112f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu mAttitudes[i] = mFusions[i].getAttitude(); 113f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu } 114f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu } 115f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu } 116f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu mAccTime = event.timestamp; 117984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 118984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 119984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 120984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopiantemplate <typename T> inline T min(T a, T b) { return a<b ? a : b; } 121984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopiantemplate <typename T> inline T max(T a, T b) { return a>b ? a : b; } 122984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 123f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xustatus_t SensorFusion::activate(int mode, void* ident, bool enabled) { 124984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 125a551237de142549fb8a6608ee9d2fbf4b7ca2ebfSteve Block ALOGD_IF(DEBUG_CONNECTIONS, 126f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu "SensorFusion::activate(mode=%d, ident=%p, enabled=%d)", 127f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu mode, ident, enabled); 128984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 129f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu const ssize_t idx = mClients[mode].indexOf(ident); 130984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian if (enabled) { 131984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian if (idx < 0) { 132f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu mClients[mode].add(ident); 133984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 134984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } else { 135984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian if (idx >= 0) { 136f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu mClients[mode].removeItemsAt(idx); 137984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 138984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 139984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 140f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu const bool newState = mClients[mode].size() != 0; 141f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu if (newState != mEnabled[mode]) { 142f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu mEnabled[mode] = newState; 143984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian if (newState) { 144f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu mFusions[mode].init(mode); 145984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 146984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian } 147f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu 148f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu mSensorDevice.activate(ident, mAcc.getHandle(), enabled); 149f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu if (mode != FUSION_NOMAG) { 150f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu mSensorDevice.activate(ident, mMag.getHandle(), enabled); 151f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu } 152f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu if (mode != FUSION_NOGYRO) { 153f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu mSensorDevice.activate(ident, mGyro.getHandle(), enabled); 154f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu } 155f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu 156984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian return NO_ERROR; 157984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 158984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 159f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xustatus_t SensorFusion::setDelay(int mode, void* ident, int64_t ns) { 160ebff73c37d5f1581702430f4a0348c160b99b57eAravind Akella // Call batch with timeout zero instead of setDelay(). 161f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu if (ns > (int64_t)5e7) { 162f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu ns = (int64_t)(5e7); 163f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu } 164ebff73c37d5f1581702430f4a0348c160b99b57eAravind Akella mSensorDevice.batch(ident, mAcc.getHandle(), 0, ns, 0); 165f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu if (mode != FUSION_NOMAG) { 1664998c16c927003f5bd98427db5f58853b170aa8aGrigory Dzhavadyan mSensorDevice.batch(ident, mMag.getHandle(), 0, ms2ns(10), 0); 167f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu } 168f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu if (mode != FUSION_NOGYRO) { 169f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu mSensorDevice.batch(ident, mGyro.getHandle(), 0, mTargetDelayNs, 0); 170f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu } 171984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian return NO_ERROR; 172984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 173984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 174984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 175f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xufloat SensorFusion::getPowerUsage(int mode) const { 1763301542828febc768e1df42892cfac4992c35474Mathias Agopian float power = mAcc.getPowerUsage() + 177f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu ((mode != FUSION_NOMAG) ? mMag.getPowerUsage() : 0) + 178f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu ((mode != FUSION_NOGYRO) ? mGyro.getPowerUsage() : 0); 179984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian return power; 180984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 181984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 182984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopianint32_t SensorFusion::getMinDelay() const { 183984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian return mAcc.getMinDelay(); 184984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 185984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 186ba02cd2f6cc3f59adf66cb2b9176bfe6c9e382d1Mathias Agopianvoid SensorFusion::dump(String8& result) { 187f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu const Fusion& fusion_9axis(mFusions[FUSION_9AXIS]); 18892dc3fc52cf097bd105460cf377779bdcf146d62Mark Salyzyn result.appendFormat("9-axis fusion %s (%zd clients), gyro-rate=%7.2fHz, " 1893301542828febc768e1df42892cfac4992c35474Mathias Agopian "q=< %g, %g, %g, %g > (%g), " 1903301542828febc768e1df42892cfac4992c35474Mathias Agopian "b=< %g, %g, %g >\n", 191f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu mEnabled[FUSION_9AXIS] ? "enabled" : "disabled", 192f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu mClients[FUSION_9AXIS].size(), 193f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu mEstimatedGyroRate, 194f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu fusion_9axis.getAttitude().x, 195f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu fusion_9axis.getAttitude().y, 196f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu fusion_9axis.getAttitude().z, 197f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu fusion_9axis.getAttitude().w, 198f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu length(fusion_9axis.getAttitude()), 199f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu fusion_9axis.getBias().x, 200f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu fusion_9axis.getBias().y, 201f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu fusion_9axis.getBias().z); 202f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu 203f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu const Fusion& fusion_nomag(mFusions[FUSION_NOMAG]); 204f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu result.appendFormat("game fusion(no mag) %s (%zd clients), " 205f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu "gyro-rate=%7.2fHz, " 206f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu "q=< %g, %g, %g, %g > (%g), " 207f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu "b=< %g, %g, %g >\n", 208f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu mEnabled[FUSION_NOMAG] ? "enabled" : "disabled", 209f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu mClients[FUSION_NOMAG].size(), 210f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu mEstimatedGyroRate, 211f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu fusion_nomag.getAttitude().x, 212f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu fusion_nomag.getAttitude().y, 213f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu fusion_nomag.getAttitude().z, 214f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu fusion_nomag.getAttitude().w, 215f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu length(fusion_nomag.getAttitude()), 216f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu fusion_nomag.getBias().x, 217f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu fusion_nomag.getBias().y, 218f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu fusion_nomag.getBias().z); 219f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu 220f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu const Fusion& fusion_nogyro(mFusions[FUSION_NOGYRO]); 221f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu result.appendFormat("geomag fusion (no gyro) %s (%zd clients), " 222f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu "gyro-rate=%7.2fHz, " 223f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu "q=< %g, %g, %g, %g > (%g), " 224f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu "b=< %g, %g, %g >\n", 225f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu mEnabled[FUSION_NOGYRO] ? "enabled" : "disabled", 226f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu mClients[FUSION_NOGYRO].size(), 2272e2a560c4b60c24258e0eaadc1189eb9dcc1a0b4Mathias Agopian mEstimatedGyroRate, 228f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu fusion_nogyro.getAttitude().x, 229f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu fusion_nogyro.getAttitude().y, 230f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu fusion_nogyro.getAttitude().z, 231f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu fusion_nogyro.getAttitude().w, 232f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu length(fusion_nogyro.getAttitude()), 233f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu fusion_nogyro.getBias().x, 234f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu fusion_nogyro.getBias().y, 235f66684a6fb2a2991e84a085673629db2a0494fc6Peng Xu fusion_nogyro.getBias().z); 236984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian} 237984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian 238984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian// --------------------------------------------------------------------------- 239984826cc158193e61e3a00359ef4f6699c7d748aMathias Agopian}; // namespace android 240