173e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian/* 273e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian * Copyright (C) 2011 The Android Open Source Project 373e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian * 473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian * Licensed under the Apache License, Version 2.0 (the "License"); 573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian * you may not use this file except in compliance with the License. 673e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian * You may obtain a copy of the License at 773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian * 873e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian * http://www.apache.org/licenses/LICENSE-2.0 973e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian * 1073e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian * Unless required by applicable law or agreed to in writing, software 1173e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian * distributed under the License is distributed on an "AS IS" BASIS, 1273e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 1373e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian * See the License for the specific language governing permissions and 1473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian * limitations under the License. 1573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian */ 1673e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 1773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian#include "SensorDevice.h" 1873e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian#include "SensorFusion.h" 1973e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian#include "SensorService.h" 2073e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 2173e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopiannamespace android { 2273e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian// --------------------------------------------------------------------------- 2373e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 2473e0bc805a143d8cc2202fccb73230459edc6869Mathias AgopianANDROID_SINGLETON_STATIC_INSTANCE(SensorFusion) 2573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 2673e0bc805a143d8cc2202fccb73230459edc6869Mathias AgopianSensorFusion::SensorFusion() 2773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian : mSensorDevice(SensorDevice::getInstance()), 286043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian mEnabled(false), mGyroTime(0) 2973e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian{ 3073e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian sensor_t const* list; 3129c176f043333c2d286bbe7d5e6a82d2af97b535Mathias Agopian ssize_t count = mSensorDevice.getSensorList(&list); 3229c176f043333c2d286bbe7d5e6a82d2af97b535Mathias Agopian if (count > 0) { 3329c176f043333c2d286bbe7d5e6a82d2af97b535Mathias Agopian for (size_t i=0 ; i<size_t(count) ; i++) { 3429c176f043333c2d286bbe7d5e6a82d2af97b535Mathias Agopian if (list[i].type == SENSOR_TYPE_ACCELEROMETER) { 3529c176f043333c2d286bbe7d5e6a82d2af97b535Mathias Agopian mAcc = Sensor(list + i); 3629c176f043333c2d286bbe7d5e6a82d2af97b535Mathias Agopian } 3729c176f043333c2d286bbe7d5e6a82d2af97b535Mathias Agopian if (list[i].type == SENSOR_TYPE_MAGNETIC_FIELD) { 3829c176f043333c2d286bbe7d5e6a82d2af97b535Mathias Agopian mMag = Sensor(list + i); 3929c176f043333c2d286bbe7d5e6a82d2af97b535Mathias Agopian } 4029c176f043333c2d286bbe7d5e6a82d2af97b535Mathias Agopian if (list[i].type == SENSOR_TYPE_GYROSCOPE) { 4129c176f043333c2d286bbe7d5e6a82d2af97b535Mathias Agopian mGyro = Sensor(list + i); 4229c176f043333c2d286bbe7d5e6a82d2af97b535Mathias Agopian // 200 Hz for gyro events is a good compromise between precision 4329c176f043333c2d286bbe7d5e6a82d2af97b535Mathias Agopian // and power/cpu usage. 4429c176f043333c2d286bbe7d5e6a82d2af97b535Mathias Agopian mGyroRate = 200; 4529c176f043333c2d286bbe7d5e6a82d2af97b535Mathias Agopian mTargetDelayNs = 1000000000LL/mGyroRate; 4629c176f043333c2d286bbe7d5e6a82d2af97b535Mathias Agopian } 4773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian } 4829c176f043333c2d286bbe7d5e6a82d2af97b535Mathias Agopian mFusion.init(); 4973e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian } 5073e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian} 5173e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 5273e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopianvoid SensorFusion::process(const sensors_event_t& event) { 536043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian if (event.type == SENSOR_TYPE_GYROSCOPE) { 5473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian if (mGyroTime != 0) { 5573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian const float dT = (event.timestamp - mGyroTime) / 1000000000.0f; 5673e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian const float freq = 1 / dT; 576043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian if (freq >= 100 && freq<1000) { // filter values obviously wrong 586043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian const float alpha = 1 / (1 + dT); // 1s time-constant 596043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian mGyroRate = freq + (mGyroRate - freq)*alpha; 606043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian } 6173e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian } 6273e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian mGyroTime = event.timestamp; 6373e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian mFusion.handleGyro(vec3_t(event.data), 1.0f/mGyroRate); 6473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian } else if (event.type == SENSOR_TYPE_MAGNETIC_FIELD) { 6573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian const vec3_t mag(event.data); 666043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian mFusion.handleMag(mag); 6773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian } else if (event.type == SENSOR_TYPE_ACCELEROMETER) { 6873e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian const vec3_t acc(event.data); 696043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian mFusion.handleAcc(acc); 706043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian mAttitude = mFusion.getAttitude(); 7173e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian } 7273e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian} 7373e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 7473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopiantemplate <typename T> inline T min(T a, T b) { return a<b ? a : b; } 7573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopiantemplate <typename T> inline T max(T a, T b) { return a>b ? a : b; } 7673e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 7773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopianstatus_t SensorFusion::activate(void* ident, bool enabled) { 7873e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 7973e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian LOGD_IF(DEBUG_CONNECTIONS, 8073e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian "SensorFusion::activate(ident=%p, enabled=%d)", 8173e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian ident, enabled); 8273e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 8373e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian const ssize_t idx = mClients.indexOf(ident); 8473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian if (enabled) { 8573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian if (idx < 0) { 8673e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian mClients.add(ident); 8773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian } 8873e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian } else { 8973e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian if (idx >= 0) { 9073e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian mClients.removeItemsAt(idx); 9173e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian } 9273e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian } 9373e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 9473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian mSensorDevice.activate(ident, mAcc.getHandle(), enabled); 9573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian mSensorDevice.activate(ident, mMag.getHandle(), enabled); 966043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian mSensorDevice.activate(ident, mGyro.getHandle(), enabled); 9773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 9873e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian const bool newState = mClients.size() != 0; 9973e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian if (newState != mEnabled) { 10073e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian mEnabled = newState; 10173e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian if (newState) { 10273e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian mFusion.init(); 1036043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian mGyroTime = 0; 10473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian } 10573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian } 10673e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian return NO_ERROR; 10773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian} 10873e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 10973e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopianstatus_t SensorFusion::setDelay(void* ident, int64_t ns) { 1106043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian mSensorDevice.setDelay(ident, mAcc.getHandle(), ns); 1116043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian mSensorDevice.setDelay(ident, mMag.getHandle(), ms2ns(20)); 1126043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian mSensorDevice.setDelay(ident, mGyro.getHandle(), mTargetDelayNs); 11373e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian return NO_ERROR; 11473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian} 11573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 11673e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 11773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopianfloat SensorFusion::getPowerUsage() const { 1186043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian float power = mAcc.getPowerUsage() + 1196043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian mMag.getPowerUsage() + 1206043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian mGyro.getPowerUsage(); 12173e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian return power; 12273e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian} 12373e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 12473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopianint32_t SensorFusion::getMinDelay() const { 12573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian return mAcc.getMinDelay(); 12673e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian} 12773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 12873e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopianvoid SensorFusion::dump(String8& result, char* buffer, size_t SIZE) { 12973e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian const Fusion& fusion(mFusion); 1306043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian snprintf(buffer, SIZE, "9-axis fusion %s (%d clients), gyro-rate=%7.2fHz, " 1316043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian "q=< %g, %g, %g, %g > (%g), " 1326043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian "b=< %g, %g, %g >\n", 13373e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian mEnabled ? "enabled" : "disabled", 13473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian mClients.size(), 13573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian mGyroRate, 13673e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian fusion.getAttitude().x, 13773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian fusion.getAttitude().y, 13873e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian fusion.getAttitude().z, 1396043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian fusion.getAttitude().w, 1406043e5329cc023ae1bf6c0b7b750e584c1ebfbf4Mathias Agopian length(fusion.getAttitude()), 14173e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian fusion.getBias().x, 14273e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian fusion.getBias().y, 14373e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian fusion.getBias().z); 14473e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian result.append(buffer); 14573e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian} 14673e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian 14773e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian// --------------------------------------------------------------------------- 14873e0bc805a143d8cc2202fccb73230459edc6869Mathias Agopian}; // namespace android 149