From fc32881fcc68640d008c7515cdd1bcd866f72cd5 Mon Sep 17 00:00:00 2001 From: Mathias Agopian Date: Wed, 14 Jul 2010 23:41:37 -0700 Subject: [PATCH 01/51] new SensorService remove old sensor service and implement SensorManager on top of the new (native) SensorManger API. Change-Id: Iddb77d498755da3e11646473a44d651f12f40281 --- services/sensorservice/Android.mk | 28 ++ services/sensorservice/SensorService.cpp | 359 ++++++++++++++++++ services/sensorservice/SensorService.h | 119 ++++++ services/sensorservice/tests/Android.mk | 14 + .../sensorservice/tests/sensorservicetest.cpp | 90 +++++ 5 files changed, 610 insertions(+) create mode 100644 services/sensorservice/Android.mk create mode 100644 services/sensorservice/SensorService.cpp create mode 100644 services/sensorservice/SensorService.h create mode 100644 services/sensorservice/tests/Android.mk create mode 100644 services/sensorservice/tests/sensorservicetest.cpp diff --git a/services/sensorservice/Android.mk b/services/sensorservice/Android.mk new file mode 100644 index 000000000..75f690f18 --- /dev/null +++ b/services/sensorservice/Android.mk @@ -0,0 +1,28 @@ +LOCAL_PATH:= $(call my-dir) +include $(CLEAR_VARS) + +LOCAL_SRC_FILES:= \ + SensorService.cpp + +LOCAL_CFLAGS:= -DLOG_TAG=\"SensorService\" + +# need "-lrt" on Linux simulator to pick up clock_gettime +ifeq ($(TARGET_SIMULATOR),true) + ifeq ($(HOST_OS),linux) + LOCAL_LDLIBS += -lrt -lpthread + endif +endif + +LOCAL_SHARED_LIBRARIES := \ + libcutils \ + libhardware \ + libutils \ + libbinder \ + libui \ + libgui + +LOCAL_PRELINK_MODULE := false + +LOCAL_MODULE:= libsensorservice + +include $(BUILD_SHARED_LIBRARY) diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp new file mode 100644 index 000000000..5410cbc52 --- /dev/null +++ b/services/sensorservice/SensorService.cpp @@ -0,0 +1,359 @@ +/* + * Copyright (C) 2010 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include + +#include "SensorService.h" + +namespace android { +// --------------------------------------------------------------------------- + +/* + * TODO: + * - handle per-connection event rate + * - filter events per connection + * - make sure to keep the last value of each event type so we can quickly + * send something to application when they enable a sensor that is already + * active (the issue here is that it can take time before a value is + * produced by the h/w if the rate is low or if it's a one-shot sensor). + */ + +SensorService::SensorService() + : Thread(false), + mDump("android.permission.DUMP") +{ +} + +void SensorService::onFirstRef() +{ + status_t err = hw_get_module(SENSORS_HARDWARE_MODULE_ID, + (hw_module_t const**)&mSensorModule); + + LOGE_IF(err, "couldn't load %s module (%s)", + SENSORS_HARDWARE_MODULE_ID, strerror(-err)); + + err = sensors_open(&mSensorModule->common, &mSensorDevice); + + LOGE_IF(err, "couldn't open device for module %s (%s)", + SENSORS_HARDWARE_MODULE_ID, strerror(-err)); + + LOGD("nuSensorService starting..."); + + struct sensor_t const* list; + int count = mSensorModule->get_sensors_list(mSensorModule, &list); + for (int i=0 ; iactivate(mSensorDevice, sensor.getHandle(), 0); + } + + run("SensorService", PRIORITY_URGENT_DISPLAY); +} + +SensorService::~SensorService() +{ +} + +status_t SensorService::dump(int fd, const Vector& args) +{ + const size_t SIZE = 1024; + char buffer[SIZE]; + String8 result; + if (!mDump.checkCalling()) { + snprintf(buffer, SIZE, "Permission Denial: " + "can't dump SurfaceFlinger from pid=%d, uid=%d\n", + IPCThreadState::self()->getCallingPid(), + IPCThreadState::self()->getCallingUid()); + result.append(buffer); + } else { + Mutex::Autolock _l(mLock); + snprintf(buffer, SIZE, "%d connections / %d active\n", + mConnections.size(), mActiveConnections.size()); + result.append(buffer); + snprintf(buffer, SIZE, "Active sensors:\n"); + result.append(buffer); + for (size_t i=0 ; igetNumConnections()); + result.append(buffer); + } + } + write(fd, result.string(), result.size()); + return NO_ERROR; +} + +bool SensorService::threadLoop() +{ + LOGD("nuSensorService thread starting..."); + + sensors_event_t buffer[16]; + struct sensors_poll_device_t* device = mSensorDevice; + ssize_t count; + + do { + count = device->poll(device, buffer, sizeof(buffer)/sizeof(*buffer)); + if (count<0) { + LOGE("sensor poll failed (%s)", strerror(-count)); + break; + } + + const SortedVector< wp > activeConnections( + getActiveConnections()); + + size_t numConnections = activeConnections.size(); + if (numConnections) { + for (size_t i=0 ; i connection(activeConnections[i].promote()); + if (connection != 0) { + connection->sendEvents(buffer, count); + } + } + } + + } while (count >= 0 || Thread::exitPending()); + + LOGW("Exiting SensorService::threadLoop!"); + return false; +} + +SortedVector< wp > +SensorService::getActiveConnections() const +{ + Mutex::Autolock _l(mLock); + return mActiveConnections; +} + +Vector SensorService::getSensorList() +{ + return mSensorList; +} + +sp SensorService::createSensorEventConnection() +{ + sp result(new SensorEventConnection(this)); + Mutex::Autolock _l(mLock); + mConnections.add(result); + return result; +} + +void SensorService::cleanupConnection(const wp& connection) +{ + Mutex::Autolock _l(mLock); + ssize_t index = mConnections.indexOf(connection); + if (index >= 0) { + + size_t size = mActiveSensors.size(); + for (size_t i=0 ; iremoveConnection(connection)) { + mSensorDevice->activate(mSensorDevice, mActiveSensors.keyAt(i), 0); + mActiveSensors.removeItemsAt(i, 1); + delete rec; + size--; + } else { + i++; + } + } + + mActiveConnections.remove(connection); + mConnections.removeItemsAt(index, 1); + } +} + +status_t SensorService::enable(const sp& connection, + int handle) +{ + status_t err = NO_ERROR; + Mutex::Autolock _l(mLock); + SensorRecord* rec = mActiveSensors.valueFor(handle); + if (rec == 0) { + rec = new SensorRecord(connection); + mActiveSensors.add(handle, rec); + err = mSensorDevice->activate(mSensorDevice, handle, 1); + LOGE_IF(err, "Error activating sensor %d (%s)", handle, strerror(-err)); + } else { + err = rec->addConnection(connection); + } + if (err == NO_ERROR) { + // connection now active + connection->addSensor(handle); + if (mActiveConnections.indexOf(connection) < 0) { + mActiveConnections.add(connection); + } + } + return err; +} + +status_t SensorService::disable(const sp& connection, + int handle) +{ + status_t err = NO_ERROR; + Mutex::Autolock _l(mLock); + SensorRecord* rec = mActiveSensors.valueFor(handle); + LOGW("sensor (handle=%d) is not enabled", handle); + if (rec) { + // see if this connection becomes inactive + connection->removeSensor(handle); + if (connection->hasAnySensor() == false) { + mActiveConnections.remove(connection); + } + // see if this sensor becomes inactive + if (rec->removeConnection(connection)) { + mActiveSensors.removeItem(handle); + delete rec; + err = mSensorDevice->activate(mSensorDevice, handle, 0); + } + } + return err; +} + +status_t SensorService::setRate(const sp& connection, + int handle, nsecs_t ns) +{ + status_t err = NO_ERROR; + Mutex::Autolock _l(mLock); + + err = mSensorDevice->setDelay(mSensorDevice, handle, ns); + + // TODO: handle rate per connection + return err; +} + +// --------------------------------------------------------------------------- + +SensorService::SensorRecord::SensorRecord( + const sp& connection) +{ + mConnections.add(connection); +} + +status_t SensorService::SensorRecord::addConnection( + const sp& connection) +{ + if (mConnections.indexOf(connection) < 0) { + mConnections.add(connection); + } + return NO_ERROR; +} + +bool SensorService::SensorRecord::removeConnection( + const wp& connection) +{ + ssize_t index = mConnections.indexOf(connection); + if (index >= 0) { + mConnections.removeItemsAt(index, 1); + } + return mConnections.size() ? false : true; +} + +// --------------------------------------------------------------------------- + +SensorService::SensorEventConnection::SensorEventConnection( + const sp& service) + : mService(service), mChannel(new SensorChannel()) +{ +} + +SensorService::SensorEventConnection::~SensorEventConnection() +{ + mService->cleanupConnection(this); +} + +void SensorService::SensorEventConnection::onFirstRef() +{ +} + +void SensorService::SensorEventConnection::addSensor(int32_t handle) { + if (mSensorList.indexOf(handle) <= 0) { + mSensorList.add(handle); + } +} + +void SensorService::SensorEventConnection::removeSensor(int32_t handle) { + mSensorList.remove(handle); +} + +bool SensorService::SensorEventConnection::hasSensor(int32_t handle) const { + return mSensorList.indexOf(handle) >= 0; +} + +bool SensorService::SensorEventConnection::hasAnySensor() const { + return mSensorList.size() ? true : false; +} + +status_t SensorService::SensorEventConnection::sendEvents( + sensors_event_t const* buffer, size_t count) +{ + // TODO: we should only send the events for the sensors this connection + // is registered for. + + ssize_t size = mChannel->write(buffer, count*sizeof(sensors_event_t)); + if (size == -EAGAIN) { + // the destination doesn't accept events anymore, it's probably + // full. For now, we just drop the events on the floor. + LOGW("dropping %d events on the floor", count); + return size; + } + + LOGE_IF(size<0, "dropping %d events on the floor (%s)", + count, strerror(-size)); + + return size < 0 ? size : NO_ERROR; +} + +sp SensorService::SensorEventConnection::getSensorChannel() const +{ + return mChannel; +} + +status_t SensorService::SensorEventConnection::enableDisable( + int handle, bool enabled) +{ + status_t err; + if (enabled) { + err = mService->enable(this, handle); + } else { + err = mService->disable(this, handle); + } + return err; +} + +status_t SensorService::SensorEventConnection::setEventRate( + int handle, nsecs_t ns) +{ + return mService->setRate(this, handle, ns); +} + +// --------------------------------------------------------------------------- +}; // namespace android + diff --git a/services/sensorservice/SensorService.h b/services/sensorservice/SensorService.h new file mode 100644 index 000000000..d5e321ce2 --- /dev/null +++ b/services/sensorservice/SensorService.h @@ -0,0 +1,119 @@ +/* + * Copyright (C) 2010 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ANDROID_SENSOR_SERVICE_H +#define ANDROID_SENSOR_SERVICE_H + +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +// --------------------------------------------------------------------------- + +struct sensors_poll_device_t; +struct sensors_module_t; + +namespace android { +// --------------------------------------------------------------------------- + +class SensorService : + public BinderService, + public BnSensorServer, + protected Thread +{ + friend class BinderService; + + SensorService(); + virtual ~SensorService(); + + virtual void onFirstRef(); + + // Thread interface + virtual bool threadLoop(); + + // ISensorServer interface + virtual Vector getSensorList(); + virtual sp createSensorEventConnection(); + virtual status_t dump(int fd, const Vector& args); + + + class SensorEventConnection : public BnSensorEventConnection { + virtual sp getSensorChannel() const; + virtual status_t enableDisable(int handle, bool enabled); + virtual status_t setEventRate(int handle, nsecs_t ns); + sp const mService; + sp const mChannel; + SortedVector mSensorList; + public: + SensorEventConnection(const sp& service); + virtual ~SensorEventConnection(); + virtual void onFirstRef(); + status_t sendEvents(sensors_event_t const* buffer, size_t count); + bool hasSensor(int32_t handle) const; + bool hasAnySensor() const; + void addSensor(int32_t handle); + void removeSensor(int32_t handle); + }; + + class SensorRecord { + SortedVector< wp > mConnections; + public: + SensorRecord(const sp& connection); + status_t addConnection(const sp& connection); + bool removeConnection(const wp& connection); + size_t getNumConnections() const { return mConnections.size(); } + }; + + SortedVector< wp > getActiveConnections() const; + + // constants + Vector mSensorList; + struct sensors_poll_device_t* mSensorDevice; + struct sensors_module_t* mSensorModule; + Permission mDump; + + // protected by mLock + mutable Mutex mLock; + SortedVector< wp > mConnections; + DefaultKeyedVector mActiveSensors; + SortedVector< wp > mActiveConnections; + +public: + static char const* getServiceName() { return "sensorservice"; } + + void cleanupConnection(const wp& connection); + status_t enable(const sp& connection, int handle); + status_t disable(const sp& connection, int handle); + status_t setRate(const sp& connection, int handle, nsecs_t ns); +}; + +// --------------------------------------------------------------------------- +}; // namespace android + +#endif // ANDROID_SENSOR_SERVICE_H diff --git a/services/sensorservice/tests/Android.mk b/services/sensorservice/tests/Android.mk new file mode 100644 index 000000000..45296dd6c --- /dev/null +++ b/services/sensorservice/tests/Android.mk @@ -0,0 +1,14 @@ +LOCAL_PATH:= $(call my-dir) +include $(CLEAR_VARS) + +LOCAL_SRC_FILES:= \ + sensorservicetest.cpp + +LOCAL_SHARED_LIBRARIES := \ + libcutils libutils libui libgui + +LOCAL_MODULE:= test-sensorservice + +LOCAL_MODULE_TAGS := optional + +include $(BUILD_EXECUTABLE) diff --git a/services/sensorservice/tests/sensorservicetest.cpp b/services/sensorservice/tests/sensorservicetest.cpp new file mode 100644 index 000000000..e464713dd --- /dev/null +++ b/services/sensorservice/tests/sensorservicetest.cpp @@ -0,0 +1,90 @@ +/* + * Copyright (C) 2010 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include +#include + +using namespace android; + +bool receiver(int fd, int events, void* data) +{ + sp q((SensorEventQueue*)data); + ssize_t n; + ASensorEvent buffer[8]; + while ((n = q->read(buffer, 8)) > 0) { + for (int i=0 ; i\n", + buffer[i].timestamp, + buffer[i].acceleration.x, + buffer[i].acceleration.y, + buffer[i].acceleration.z); + } + } + } + if (n<0 && n != -EAGAIN) { + printf("error reading events (%s)\n", strerror(-n)); + } + return true; +} + + +int main(int argc, char** argv) +{ + SensorManager& mgr(SensorManager::getInstance()); + + Sensor const* const* list; + ssize_t count = mgr.getSensorList(&list); + printf("numSensors=%d\n", count); + + sp q = mgr.createEventQueue(); + printf("queue=%p\n", q.get()); + + Sensor const* accelerometer = mgr.getDefaultSensor(Sensor::TYPE_ACCELEROMETER); + printf("accelerometer=%p (%s)\n", + accelerometer, accelerometer->getName().string()); + q->enableSensor(accelerometer); + + q->setEventRate(accelerometer, ms2ns(10)); + + sp loop = new PollLoop(false); + loop->setCallback(q->getFd(), POLLIN, receiver, q.get()); + + do { + //printf("about to poll...\n"); + int32_t ret = loop->pollOnce(-1, 0, 0); + switch (ret) { + case ALOOPER_POLL_CALLBACK: + //("ALOOPER_POLL_CALLBACK\n"); + break; + case ALOOPER_POLL_TIMEOUT: + printf("ALOOPER_POLL_TIMEOUT\n"); + break; + case ALOOPER_POLL_ERROR: + printf("ALOOPER_POLL_TIMEOUT\n"); + break; + default: + printf("ugh? poll returned %d\n", ret); + break; + } + } while (1); + + + return 0; +} From 451beee076cac09f817abae78a990dea108a9482 Mon Sep 17 00:00:00 2001 From: Mathias Agopian Date: Mon, 19 Jul 2010 15:03:55 -0700 Subject: [PATCH 02/51] Added partial support for repporting sensor activity to IBatteryStats Change-Id: I2af319d89e49b0f2349ec9d8b0fccac80e9bc047 --- services/sensorservice/SensorService.cpp | 37 ++++++++++++++++++++++++ 1 file changed, 37 insertions(+) diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp index 5410cbc52..0ab4a20b5 100644 --- a/services/sensorservice/SensorService.cpp +++ b/services/sensorservice/SensorService.cpp @@ -23,8 +23,10 @@ #include #include #include +#include #include +#include #include #include @@ -44,8 +46,37 @@ namespace android { * send something to application when they enable a sensor that is already * active (the issue here is that it can take time before a value is * produced by the h/w if the rate is low or if it's a one-shot sensor). + * - send sensor info to battery service */ +// --------------------------------------------------------------------------- + +class BatteryService : public Singleton { + friend class Singleton; + sp mBatteryStatService; + BatteryService() { + const String16 name("batteryinfo"); + //getService(name, &mBatteryStatService); + } +public: + void enableSensor(int handle) { + if (mBatteryStatService != 0) { + int uid = IPCThreadState::self()->getCallingUid(); + //mBatteryStatService->noteStartSensor(uid, handle); + } + } + void disableSensor(int handle) { + if (mBatteryStatService != 0) { + int uid = IPCThreadState::self()->getCallingUid(); + //mBatteryStatService->noteStopSensor(uid, handle); + } + } +}; + +ANDROID_SINGLETON_STATIC_INSTANCE(BatteryService) + +// --------------------------------------------------------------------------- + SensorService::SensorService() : Thread(false), mDump("android.permission.DUMP") @@ -201,6 +232,9 @@ status_t SensorService::enable(const sp& connection, mActiveSensors.add(handle, rec); err = mSensorDevice->activate(mSensorDevice, handle, 1); LOGE_IF(err, "Error activating sensor %d (%s)", handle, strerror(-err)); + if (err == 0) { + BatteryService::getInstance().enableSensor(handle); + } } else { err = rec->addConnection(connection); } @@ -232,6 +266,9 @@ status_t SensorService::disable(const sp& connection, mActiveSensors.removeItem(handle); delete rec; err = mSensorDevice->activate(mSensorDevice, handle, 0); + if (err == 0) { + BatteryService::getInstance().disableSensor(handle); + } } } return err; From 5d2707214dfb97bd8dfcc6620be36841d3c82420 Mon Sep 17 00:00:00 2001 From: Mathias Agopian Date: Mon, 19 Jul 2010 15:20:39 -0700 Subject: [PATCH 03/51] Better dumpsys logs Change-Id: Iae65a8547ee5815cc4c3b74d2c9ef17bed7f565d --- services/sensorservice/SensorService.cpp | 18 ++++++++++++++++-- services/sensorservice/SensorService.h | 1 + 2 files changed, 17 insertions(+), 2 deletions(-) diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp index 0ab4a20b5..3fe3a5d30 100644 --- a/services/sensorservice/SensorService.cpp +++ b/services/sensorservice/SensorService.cpp @@ -133,8 +133,10 @@ status_t SensorService::dump(int fd, const Vector& args) snprintf(buffer, SIZE, "Active sensors:\n"); result.append(buffer); for (size_t i=0 ; igetNumConnections()); result.append(buffer); } @@ -184,6 +186,18 @@ SensorService::getActiveConnections() const return mActiveConnections; } +String8 SensorService::getSensorName(int handle) const { + size_t count = mSensorList.size(); + for (size_t i=0 ; i SensorService::getSensorList() { return mSensorList; diff --git a/services/sensorservice/SensorService.h b/services/sensorservice/SensorService.h index d5e321ce2..88b84ecb2 100644 --- a/services/sensorservice/SensorService.h +++ b/services/sensorservice/SensorService.h @@ -91,6 +91,7 @@ class SensorService : }; SortedVector< wp > getActiveConnections() const; + String8 getSensorName(int handle) const; // constants Vector mSensorList; From 50df2959e58fc7408f98d11d77c8428397dca445 Mon Sep 17 00:00:00 2001 From: Mathias Agopian Date: Mon, 19 Jul 2010 19:09:10 -0700 Subject: [PATCH 04/51] SensorService doesn't crash if correct HAL is not present Change-Id: I83700b1a1b43390f5830e1056572bfb16e58e8e4 --- services/sensorservice/SensorService.cpp | 47 +++++++++++++++++------- services/sensorservice/SensorService.h | 1 + 2 files changed, 34 insertions(+), 14 deletions(-) diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp index 3fe3a5d30..fec915362 100644 --- a/services/sensorservice/SensorService.cpp +++ b/services/sensorservice/SensorService.cpp @@ -79,35 +79,45 @@ ANDROID_SINGLETON_STATIC_INSTANCE(BatteryService) SensorService::SensorService() : Thread(false), - mDump("android.permission.DUMP") + mSensorDevice(0), + mSensorModule(0), + mDump("android.permission.DUMP"), + mInitCheck(NO_INIT) { } void SensorService::onFirstRef() { + LOGD("nuSensorService starting..."); + status_t err = hw_get_module(SENSORS_HARDWARE_MODULE_ID, (hw_module_t const**)&mSensorModule); LOGE_IF(err, "couldn't load %s module (%s)", SENSORS_HARDWARE_MODULE_ID, strerror(-err)); - err = sensors_open(&mSensorModule->common, &mSensorDevice); + if (mSensorModule) { + err = sensors_open(&mSensorModule->common, &mSensorDevice); - LOGE_IF(err, "couldn't open device for module %s (%s)", - SENSORS_HARDWARE_MODULE_ID, strerror(-err)); + LOGE_IF(err, "couldn't open device for module %s (%s)", + SENSORS_HARDWARE_MODULE_ID, strerror(-err)); - LOGD("nuSensorService starting..."); + struct sensor_t const* list; + int count = mSensorModule->get_sensors_list(mSensorModule, &list); + for (int i=0 ; iactivate(mSensorDevice, sensor.getHandle(), 0); + } + } - struct sensor_t const* list; - int count = mSensorModule->get_sensors_list(mSensorModule, &list); - for (int i=0 ; iactivate(mSensorDevice, sensor.getHandle(), 0); + if (mSensorDevice) { + run("SensorService", PRIORITY_URGENT_DISPLAY); + mInitCheck = NO_ERROR; + } } - - run("SensorService", PRIORITY_URGENT_DISPLAY); } SensorService::~SensorService() @@ -238,6 +248,9 @@ void SensorService::cleanupConnection(const wp& connectio status_t SensorService::enable(const sp& connection, int handle) { + if (mInitCheck != NO_ERROR) + return mInitCheck; + status_t err = NO_ERROR; Mutex::Autolock _l(mLock); SensorRecord* rec = mActiveSensors.valueFor(handle); @@ -265,6 +278,9 @@ status_t SensorService::enable(const sp& connection, status_t SensorService::disable(const sp& connection, int handle) { + if (mInitCheck != NO_ERROR) + return mInitCheck; + status_t err = NO_ERROR; Mutex::Autolock _l(mLock); SensorRecord* rec = mActiveSensors.valueFor(handle); @@ -291,6 +307,9 @@ status_t SensorService::disable(const sp& connection, status_t SensorService::setRate(const sp& connection, int handle, nsecs_t ns) { + if (mInitCheck != NO_ERROR) + return mInitCheck; + status_t err = NO_ERROR; Mutex::Autolock _l(mLock); diff --git a/services/sensorservice/SensorService.h b/services/sensorservice/SensorService.h index 88b84ecb2..873195615 100644 --- a/services/sensorservice/SensorService.h +++ b/services/sensorservice/SensorService.h @@ -98,6 +98,7 @@ class SensorService : struct sensors_poll_device_t* mSensorDevice; struct sensors_module_t* mSensorModule; Permission mDump; + status_t mInitCheck; // protected by mLock mutable Mutex mLock; From 1cd700015318727d6d42236ab6274f1949fb08ba Mon Sep 17 00:00:00 2001 From: Mathias Agopian Date: Wed, 21 Jul 2010 15:59:50 -0700 Subject: [PATCH 05/51] propagate sensor event rate properly Change-Id: I32e67d30e4295285a6827956cc8161b2025d70bc --- services/sensorservice/SensorService.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp index fec915362..a4f654976 100644 --- a/services/sensorservice/SensorService.cpp +++ b/services/sensorservice/SensorService.cpp @@ -77,6 +77,9 @@ ANDROID_SINGLETON_STATIC_INSTANCE(BatteryService) // --------------------------------------------------------------------------- +// 100 events/s max +static const nsecs_t MINIMUM_EVENT_PERIOD = ms2ns(10); + SensorService::SensorService() : Thread(false), mSensorDevice(0), @@ -284,7 +287,6 @@ status_t SensorService::disable(const sp& connection, status_t err = NO_ERROR; Mutex::Autolock _l(mLock); SensorRecord* rec = mActiveSensors.valueFor(handle); - LOGW("sensor (handle=%d) is not enabled", handle); if (rec) { // see if this connection becomes inactive connection->removeSensor(handle); @@ -310,6 +312,12 @@ status_t SensorService::setRate(const sp& connection, if (mInitCheck != NO_ERROR) return mInitCheck; + if (ns < 0) + return BAD_VALUE; + + if (ns < MINIMUM_EVENT_PERIOD) + ns = MINIMUM_EVENT_PERIOD; + status_t err = NO_ERROR; Mutex::Autolock _l(mLock); From 7c1c531872a95051cb11ec829e3daf890d9bb58a Mon Sep 17 00:00:00 2001 From: Mathias Agopian Date: Wed, 21 Jul 2010 15:59:50 -0700 Subject: [PATCH 06/51] propagate sensor event rate properly to the sensor HAL Change-Id: I1abe4c0fcce423caab79208353fded4d57398227 --- services/sensorservice/SensorService.cpp | 121 +++++++++++++++-------- services/sensorservice/SensorService.h | 34 +++++-- 2 files changed, 104 insertions(+), 51 deletions(-) diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp index a4f654976..82cdf3d40 100644 --- a/services/sensorservice/SensorService.cpp +++ b/services/sensorservice/SensorService.cpp @@ -40,7 +40,6 @@ namespace android { /* * TODO: - * - handle per-connection event rate * - filter events per connection * - make sure to keep the last value of each event type so we can quickly * send something to application when they enable a sensor that is already @@ -140,8 +139,8 @@ status_t SensorService::dump(int fd, const Vector& args) result.append(buffer); } else { Mutex::Autolock _l(mLock); - snprintf(buffer, SIZE, "%d connections / %d active\n", - mConnections.size(), mActiveConnections.size()); + snprintf(buffer, SIZE, "%d active connections\n", + mActiveConnections.size()); result.append(buffer); snprintf(buffer, SIZE, "Active sensors:\n"); result.append(buffer); @@ -219,33 +218,25 @@ Vector SensorService::getSensorList() sp SensorService::createSensorEventConnection() { sp result(new SensorEventConnection(this)); - Mutex::Autolock _l(mLock); - mConnections.add(result); return result; } void SensorService::cleanupConnection(const wp& connection) { Mutex::Autolock _l(mLock); - ssize_t index = mConnections.indexOf(connection); - if (index >= 0) { - - size_t size = mActiveSensors.size(); - for (size_t i=0 ; iremoveConnection(connection)) { - mSensorDevice->activate(mSensorDevice, mActiveSensors.keyAt(i), 0); - mActiveSensors.removeItemsAt(i, 1); - delete rec; - size--; - } else { - i++; - } + size_t size = mActiveSensors.size(); + for (size_t i=0 ; iremoveConnection(connection)) { + mSensorDevice->activate(mSensorDevice, mActiveSensors.keyAt(i), 0); + mActiveSensors.removeItemsAt(i, 1); + delete rec; + size--; + } else { + i++; } - - mActiveConnections.remove(connection); - mConnections.removeItemsAt(index, 1); } + mActiveConnections.remove(connection); } status_t SensorService::enable(const sp& connection, @@ -266,13 +257,18 @@ status_t SensorService::enable(const sp& connection, BatteryService::getInstance().enableSensor(handle); } } else { - err = rec->addConnection(connection); + rec->addConnection(connection); } if (err == NO_ERROR) { // connection now active - connection->addSensor(handle); - if (mActiveConnections.indexOf(connection) < 0) { - mActiveConnections.add(connection); + if (connection->addSensor(handle)) { + // the sensor was added (which means it wasn't already there) + // so, see if this connection becomes active + if (mActiveConnections.indexOf(connection) < 0) { + mActiveConnections.add(connection); + } + // this could change the sensor event delivery speed + recomputeEventsPeriodLocked(handle); } } return err; @@ -303,10 +299,13 @@ status_t SensorService::disable(const sp& connection, } } } + if (err == NO_ERROR) { + recomputeEventsPeriodLocked(handle); + } return err; } -status_t SensorService::setRate(const sp& connection, +status_t SensorService::setEventRate(const sp& connection, int handle, nsecs_t ns) { if (mInitCheck != NO_ERROR) @@ -315,15 +314,32 @@ status_t SensorService::setRate(const sp& connection, if (ns < 0) return BAD_VALUE; - if (ns < MINIMUM_EVENT_PERIOD) - ns = MINIMUM_EVENT_PERIOD; + if (ns < MINIMUM_EVENTS_PERIOD) + ns = MINIMUM_EVENTS_PERIOD; - status_t err = NO_ERROR; Mutex::Autolock _l(mLock); + status_t err = connection->setEventRateLocked(handle, ns); + if (err == NO_ERROR) { + recomputeEventsPeriodLocked(handle); + } + return err; +} - err = mSensorDevice->setDelay(mSensorDevice, handle, ns); - - // TODO: handle rate per connection +status_t SensorService::recomputeEventsPeriodLocked(int32_t handle) +{ + status_t err = NO_ERROR; + nsecs_t wanted = ms2ns(1000); + size_t count = mActiveConnections.size(); + for (size_t i=0 ; i connection(mActiveConnections[i].promote()); + if (connection != NULL) { + nsecs_t ns = connection->getEventRateForSensor(handle); + if (ns) { + wanted = wanted < ns ? wanted : ns; + } + } + } + err = mSensorDevice->setDelay(mSensorDevice, handle, wanted); return err; } @@ -335,13 +351,14 @@ SensorService::SensorRecord::SensorRecord( mConnections.add(connection); } -status_t SensorService::SensorRecord::addConnection( +bool SensorService::SensorRecord::addConnection( const sp& connection) { if (mConnections.indexOf(connection) < 0) { mConnections.add(connection); + return true; } - return NO_ERROR; + return false; } bool SensorService::SensorRecord::removeConnection( @@ -371,22 +388,40 @@ void SensorService::SensorEventConnection::onFirstRef() { } -void SensorService::SensorEventConnection::addSensor(int32_t handle) { - if (mSensorList.indexOf(handle) <= 0) { - mSensorList.add(handle); +bool SensorService::SensorEventConnection::addSensor(int32_t handle) { + if (mSensorInfo.indexOfKey(handle) <= 0) { + SensorInfo info; + mSensorInfo.add(handle, info); + return true; } + return false; } -void SensorService::SensorEventConnection::removeSensor(int32_t handle) { - mSensorList.remove(handle); +bool SensorService::SensorEventConnection::removeSensor(int32_t handle) { + if (mSensorInfo.removeItem(handle) >= 0) { + return true; + } + return false; } bool SensorService::SensorEventConnection::hasSensor(int32_t handle) const { - return mSensorList.indexOf(handle) >= 0; + return mSensorInfo.indexOfKey(handle) >= 0; } bool SensorService::SensorEventConnection::hasAnySensor() const { - return mSensorList.size() ? true : false; + return mSensorInfo.size() ? true : false; +} + +status_t SensorService::SensorEventConnection::setEventRateLocked( + int handle, nsecs_t ns) +{ + ssize_t index = mSensorInfo.indexOfKey(handle); + if (index >= 0) { + SensorInfo& info = mSensorInfo.editValueFor(handle); + info.ns = ns; + return NO_ERROR; + } + return status_t(index); } status_t SensorService::SensorEventConnection::sendEvents( @@ -429,7 +464,7 @@ status_t SensorService::SensorEventConnection::enableDisable( status_t SensorService::SensorEventConnection::setEventRate( int handle, nsecs_t ns) { - return mService->setRate(this, handle, ns); + return mService->setEventRate(this, handle, ns); } // --------------------------------------------------------------------------- diff --git a/services/sensorservice/SensorService.h b/services/sensorservice/SensorService.h index 873195615..b8dda8477 100644 --- a/services/sensorservice/SensorService.h +++ b/services/sensorservice/SensorService.h @@ -49,6 +49,9 @@ class SensorService : { friend class BinderService; + static const nsecs_t MINIMUM_EVENTS_PERIOD = 10000000; // 10ms + static const nsecs_t DEFAULT_EVENTS_PERIOD = 200000000; // 200 ms + SensorService(); virtual ~SensorService(); @@ -64,34 +67,50 @@ class SensorService : class SensorEventConnection : public BnSensorEventConnection { + virtual ~SensorEventConnection(); + virtual void onFirstRef(); virtual sp getSensorChannel() const; virtual status_t enableDisable(int handle, bool enabled); virtual status_t setEventRate(int handle, nsecs_t ns); + sp const mService; sp const mChannel; - SortedVector mSensorList; + + // protected by SensorService::mLock + //SortedVector mSensorList; + + struct SensorInfo { + SensorInfo() : ns(DEFAULT_EVENTS_PERIOD) { } + nsecs_t ns; + }; + DefaultKeyedVector mSensorInfo; + public: SensorEventConnection(const sp& service); - virtual ~SensorEventConnection(); - virtual void onFirstRef(); + status_t sendEvents(sensors_event_t const* buffer, size_t count); bool hasSensor(int32_t handle) const; bool hasAnySensor() const; - void addSensor(int32_t handle); - void removeSensor(int32_t handle); + bool addSensor(int32_t handle); + bool removeSensor(int32_t handle); + status_t setEventRateLocked(int handle, nsecs_t ns); + nsecs_t getEventRateForSensor(int32_t handle) const { + return mSensorInfo.valueFor(handle).ns; + } }; class SensorRecord { SortedVector< wp > mConnections; public: SensorRecord(const sp& connection); - status_t addConnection(const sp& connection); + bool addConnection(const sp& connection); bool removeConnection(const wp& connection); size_t getNumConnections() const { return mConnections.size(); } }; SortedVector< wp > getActiveConnections() const; String8 getSensorName(int handle) const; + status_t recomputeEventsPeriodLocked(int32_t handle); // constants Vector mSensorList; @@ -102,7 +121,6 @@ class SensorService : // protected by mLock mutable Mutex mLock; - SortedVector< wp > mConnections; DefaultKeyedVector mActiveSensors; SortedVector< wp > mActiveConnections; @@ -112,7 +130,7 @@ public: void cleanupConnection(const wp& connection); status_t enable(const sp& connection, int handle); status_t disable(const sp& connection, int handle); - status_t setRate(const sp& connection, int handle, nsecs_t ns); + status_t setEventRate(const sp& connection, int handle, nsecs_t ns); }; // --------------------------------------------------------------------------- From cf51001dbf28e9885fcacd4048902f1c75768fe9 Mon Sep 17 00:00:00 2001 From: Mathias Agopian Date: Thu, 22 Jul 2010 16:18:10 -0700 Subject: [PATCH 07/51] filter sensor event by connection we now don't send events down to a connection that has not registered for this event. Change-Id: I3fe507974d3e99293749bfec2ef871e8a0ee9600 --- services/sensorservice/SensorService.cpp | 24 ++++++++++++++++++------ services/sensorservice/SensorService.h | 5 ++--- 2 files changed, 20 insertions(+), 9 deletions(-) diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp index 82cdf3d40..b79373d80 100644 --- a/services/sensorservice/SensorService.cpp +++ b/services/sensorservice/SensorService.cpp @@ -40,7 +40,6 @@ namespace android { /* * TODO: - * - filter events per connection * - make sure to keep the last value of each event type so we can quickly * send something to application when they enable a sensor that is already * active (the issue here is that it can take time before a value is @@ -162,6 +161,7 @@ bool SensorService::threadLoop() LOGD("nuSensorService thread starting..."); sensors_event_t buffer[16]; + sensors_event_t scratch[16]; struct sensors_poll_device_t* device = mSensorDevice; ssize_t count; @@ -177,10 +177,11 @@ bool SensorService::threadLoop() size_t numConnections = activeConnections.size(); if (numConnections) { + Mutex::Autolock _l(mLock); for (size_t i=0 ; i connection(activeConnections[i].promote()); if (connection != 0) { - connection->sendEvents(buffer, count); + connection->sendEvents(buffer, count, scratch); } } } @@ -425,12 +426,23 @@ status_t SensorService::SensorEventConnection::setEventRateLocked( } status_t SensorService::SensorEventConnection::sendEvents( - sensors_event_t const* buffer, size_t count) + sensors_event_t const* buffer, size_t numEvents, + sensors_event_t* scratch) { - // TODO: we should only send the events for the sensors this connection - // is registered for. + // filter out events not for this connection + size_t count=0, i=0; + while (i= 0) { + do { + scratch[count++] = buffer[i++]; + } while ((iwrite(buffer, count*sizeof(sensors_event_t)); + ssize_t size = mChannel->write(scratch, count*sizeof(sensors_event_t)); if (size == -EAGAIN) { // the destination doesn't accept events anymore, it's probably // full. For now, we just drop the events on the floor. diff --git a/services/sensorservice/SensorService.h b/services/sensorservice/SensorService.h index b8dda8477..f77652da0 100644 --- a/services/sensorservice/SensorService.h +++ b/services/sensorservice/SensorService.h @@ -77,8 +77,6 @@ class SensorService : sp const mChannel; // protected by SensorService::mLock - //SortedVector mSensorList; - struct SensorInfo { SensorInfo() : ns(DEFAULT_EVENTS_PERIOD) { } nsecs_t ns; @@ -88,7 +86,8 @@ class SensorService : public: SensorEventConnection(const sp& service); - status_t sendEvents(sensors_event_t const* buffer, size_t count); + status_t sendEvents(sensors_event_t const* buffer, size_t count, + sensors_event_t* scratch); bool hasSensor(int32_t handle) const; bool hasAnySensor() const; bool addSensor(int32_t handle); From 3560fb24b668675627934356f210d84d19bf4e56 Mon Sep 17 00:00:00 2001 From: Mathias Agopian Date: Thu, 22 Jul 2010 21:24:39 -0700 Subject: [PATCH 08/51] SensorService handles last known state properly SensorService now correctly sends the last known state of a sensor as soon as a new connection is made. This fixes the issue where, for instance, an application could wait a long time before getting the light or proximity sensor initial state. Change-Id: Ic41392f3626e26c4f15746c7e17c7ecd44bbb10b --- services/sensorservice/SensorService.cpp | 84 ++++++++++++++++++++---- services/sensorservice/SensorService.h | 5 +- 2 files changed, 74 insertions(+), 15 deletions(-) diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp index b79373d80..7fcab4cf7 100644 --- a/services/sensorservice/SensorService.cpp +++ b/services/sensorservice/SensorService.cpp @@ -40,11 +40,18 @@ namespace android { /* * TODO: - * - make sure to keep the last value of each event type so we can quickly - * send something to application when they enable a sensor that is already - * active (the issue here is that it can take time before a value is - * produced by the h/w if the rate is low or if it's a one-shot sensor). * - send sensor info to battery service + * + +static final int TRANSACTION_noteStartSensor = (android.os.IBinder.FIRST_CALL_TRANSACTION + 3); +static final int TRANSACTION_noteStopSensor = (android.os.IBinder.FIRST_CALL_TRANSACTION + 4); + + _data.writeInterfaceToken(DESCRIPTOR); + _data.writeInt(uid); + _data.writeInt(sensor); + mRemote.transact(Stub.TRANSACTION_noteStartSensor, _data, _reply, 0); + _reply.readException(); + * */ // --------------------------------------------------------------------------- @@ -103,8 +110,12 @@ void SensorService::onFirstRef() LOGE_IF(err, "couldn't open device for module %s (%s)", SENSORS_HARDWARE_MODULE_ID, strerror(-err)); + sensors_event_t event; + memset(&event, 0, sizeof(event)); + struct sensor_t const* list; int count = mSensorModule->get_sensors_list(mSensorModule, &list); + mLastEventSeen.setCapacity(count); for (int i=0 ; iactivate(mSensorDevice, sensor.getHandle(), 0); } + mLastEventSeen.add(sensor.getHandle(), event); } if (mSensorDevice) { @@ -138,6 +150,19 @@ status_t SensorService::dump(int fd, const Vector& args) result.append(buffer); } else { Mutex::Autolock _l(mLock); + snprintf(buffer, SIZE, "Sensor List:\n"); + result.append(buffer); + for (size_t i=0 ; i)\n", + s.getName().string(), + s.getVendor().string(), + s.getHandle(), + e.data[0], e.data[1], e.data[2]); + result.append(buffer); + } + snprintf(buffer, SIZE, "%d active connections\n", mActiveConnections.size()); result.append(buffer); @@ -178,6 +203,19 @@ bool SensorService::threadLoop() size_t numConnections = activeConnections.size(); if (numConnections) { Mutex::Autolock _l(mLock); + + // record the last event for each sensor + int32_t prev = buffer[0].sensor; + for (ssize_t i=1 ; i connection(activeConnections[i].promote()); if (connection != 0) { @@ -258,7 +296,16 @@ status_t SensorService::enable(const sp& connection, BatteryService::getInstance().enableSensor(handle); } } else { - rec->addConnection(connection); + if (rec->addConnection(connection)) { + // this sensor is already activated, but we are adding a + // connection that uses it. Immediately send down the last + // known value of the requested sensor. + sensors_event_t scratch; + sensors_event_t& event(mLastEventSeen.editValueFor(handle)); + if (event.version == sizeof(sensors_event_t)) { + connection->sendEvents(&event, 1); + } + } } if (err == NO_ERROR) { // connection now active @@ -430,18 +477,27 @@ status_t SensorService::SensorEventConnection::sendEvents( sensors_event_t* scratch) { // filter out events not for this connection - size_t count=0, i=0; - while (i= 0) { - do { - scratch[count++] = buffer[i++]; - } while ((i= 0) { + do { + scratch[count++] = buffer[i++]; + } while ((i(buffer); + count = numEvents; } + if (count == 0) + return 0; + ssize_t size = mChannel->write(scratch, count*sizeof(sensors_event_t)); if (size == -EAGAIN) { // the destination doesn't accept events anymore, it's probably diff --git a/services/sensorservice/SensorService.h b/services/sensorservice/SensorService.h index f77652da0..9f37799fe 100644 --- a/services/sensorservice/SensorService.h +++ b/services/sensorservice/SensorService.h @@ -87,7 +87,7 @@ class SensorService : SensorEventConnection(const sp& service); status_t sendEvents(sensors_event_t const* buffer, size_t count, - sensors_event_t* scratch); + sensors_event_t* scratch = NULL); bool hasSensor(int32_t handle) const; bool hasAnySensor() const; bool addSensor(int32_t handle); @@ -123,6 +123,9 @@ class SensorService : DefaultKeyedVector mActiveSensors; SortedVector< wp > mActiveConnections; + // The size of this vector is constant, only the items are mutable + KeyedVector mLastEventSeen; + public: static char const* getServiceName() { return "sensorservice"; } From c4a930d1d5a432a1f302763ac55460d6e83fe7e0 Mon Sep 17 00:00:00 2001 From: Mathias Agopian Date: Thu, 22 Jul 2010 22:19:43 -0700 Subject: [PATCH 09/51] Report sensor events to BatteryStats service Change-Id: I9b83aa709887aa658bc474391573f2d45b6c4eb2 --- services/sensorservice/SensorService.cpp | 64 ++++++++++++++++-------- 1 file changed, 42 insertions(+), 22 deletions(-) diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp index 7fcab4cf7..3025f7732 100644 --- a/services/sensorservice/SensorService.cpp +++ b/services/sensorservice/SensorService.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include @@ -38,46 +39,65 @@ namespace android { // --------------------------------------------------------------------------- -/* - * TODO: - * - send sensor info to battery service - * - -static final int TRANSACTION_noteStartSensor = (android.os.IBinder.FIRST_CALL_TRANSACTION + 3); -static final int TRANSACTION_noteStopSensor = (android.os.IBinder.FIRST_CALL_TRANSACTION + 4); - - _data.writeInterfaceToken(DESCRIPTOR); - _data.writeInt(uid); - _data.writeInt(sensor); - mRemote.transact(Stub.TRANSACTION_noteStartSensor, _data, _reply, 0); - _reply.readException(); - * - */ - -// --------------------------------------------------------------------------- - class BatteryService : public Singleton { + static const int TRANSACTION_noteStartSensor = IBinder::FIRST_CALL_TRANSACTION + 3; + static const int TRANSACTION_noteStopSensor = IBinder::FIRST_CALL_TRANSACTION + 4; + static const String16 DESCRIPTOR; + friend class Singleton; sp mBatteryStatService; + BatteryService() { - const String16 name("batteryinfo"); - //getService(name, &mBatteryStatService); + const sp sm(defaultServiceManager()); + if (sm != NULL) { + const String16 name("batteryinfo"); + mBatteryStatService = sm->getService(name); + } } + + status_t noteStartSensor(int uid, int handle) { + Parcel data, reply; + data.writeInterfaceToken(DESCRIPTOR); + data.writeInt32(uid); + data.writeInt32(handle); + status_t err = mBatteryStatService->transact( + TRANSACTION_noteStartSensor, data, &reply, 0); + err = reply.readExceptionCode(); + return err; + } + + status_t noteStopSensor(int uid, int handle) { + Parcel data, reply; + data.writeInterfaceToken(DESCRIPTOR); + data.writeInt32(uid); + data.writeInt32(handle); + status_t err = mBatteryStatService->transact( + TRANSACTION_noteStopSensor, data, &reply, 0); + err = reply.readExceptionCode(); + return err; + } + public: void enableSensor(int handle) { if (mBatteryStatService != 0) { int uid = IPCThreadState::self()->getCallingUid(); - //mBatteryStatService->noteStartSensor(uid, handle); + int64_t identity = IPCThreadState::self()->clearCallingIdentity(); + noteStartSensor(uid, handle); + IPCThreadState::self()->restoreCallingIdentity(identity); } } void disableSensor(int handle) { if (mBatteryStatService != 0) { int uid = IPCThreadState::self()->getCallingUid(); - //mBatteryStatService->noteStopSensor(uid, handle); + int64_t identity = IPCThreadState::self()->clearCallingIdentity(); + noteStopSensor(uid, handle); + IPCThreadState::self()->restoreCallingIdentity(identity); } } }; +const String16 BatteryService::DESCRIPTOR("com.android.internal.app.IBatteryStats"); + ANDROID_SINGLETON_STATIC_INSTANCE(BatteryService) // --------------------------------------------------------------------------- From 1e0b1e8491e5f6dc59faabe70cbfa942853150e0 Mon Sep 17 00:00:00 2001 From: Jeff Brown Date: Mon, 13 Sep 2010 23:17:30 -0700 Subject: [PATCH 10/51] Replace epoll() with poll() and rename PollLoop to Looper. As part of this change, consolidated and cleaned up the Looper API so that there are fewer distinctions between the NDK and non-NDK declarations (no need for two callback types, etc.). Removed the dependence on specific constants from sys/poll.h such as POLLIN. Instead looper.h defines events like LOOPER_EVENT_INPUT for the events that it supports. That should help make any future under-the-hood implementation changes easier. Fixed a couple of compiler warnings along the way. Change-Id: I449a7ec780bf061bdd325452f823673e2b39b6ae --- services/sensorservice/SensorService.cpp | 2 +- .../sensorservice/tests/sensorservicetest.cpp | 17 ++++++++++------- 2 files changed, 11 insertions(+), 8 deletions(-) diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp index 3025f7732..e204e0402 100644 --- a/services/sensorservice/SensorService.cpp +++ b/services/sensorservice/SensorService.cpp @@ -529,7 +529,7 @@ status_t SensorService::SensorEventConnection::sendEvents( LOGE_IF(size<0, "dropping %d events on the floor (%s)", count, strerror(-size)); - return size < 0 ? size : NO_ERROR; + return size < 0 ? status_t(size) : status_t(NO_ERROR); } sp SensorService::SensorEventConnection::getSensorChannel() const diff --git a/services/sensorservice/tests/sensorservicetest.cpp b/services/sensorservice/tests/sensorservicetest.cpp index e464713dd..42bf983fe 100644 --- a/services/sensorservice/tests/sensorservicetest.cpp +++ b/services/sensorservice/tests/sensorservicetest.cpp @@ -18,11 +18,11 @@ #include #include #include -#include +#include using namespace android; -bool receiver(int fd, int events, void* data) +int receiver(int fd, int events, void* data) { sp q((SensorEventQueue*)data); ssize_t n; @@ -41,7 +41,7 @@ bool receiver(int fd, int events, void* data) if (n<0 && n != -EAGAIN) { printf("error reading events (%s)\n", strerror(-n)); } - return true; + return 1; } @@ -51,7 +51,7 @@ int main(int argc, char** argv) Sensor const* const* list; ssize_t count = mgr.getSensorList(&list); - printf("numSensors=%d\n", count); + printf("numSensors=%d\n", int(count)); sp q = mgr.createEventQueue(); printf("queue=%p\n", q.get()); @@ -63,13 +63,16 @@ int main(int argc, char** argv) q->setEventRate(accelerometer, ms2ns(10)); - sp loop = new PollLoop(false); - loop->setCallback(q->getFd(), POLLIN, receiver, q.get()); + sp loop = new Looper(false); + loop->addFd(q->getFd(), 0, ALOOPER_EVENT_INPUT, receiver, q.get()); do { //printf("about to poll...\n"); - int32_t ret = loop->pollOnce(-1, 0, 0); + int32_t ret = loop->pollOnce(-1); switch (ret) { + case ALOOPER_POLL_WAKE: + //("ALOOPER_POLL_WAKE\n"); + break; case ALOOPER_POLL_CALLBACK: //("ALOOPER_POLL_CALLBACK\n"); break; From 24d72350f321c17b0bfe1ef3fd52d4070a1c02c3 Mon Sep 17 00:00:00 2001 From: Mathias Agopian Date: Fri, 5 Nov 2010 19:12:58 -0700 Subject: [PATCH 11/51] improve sensorservice dumpsys and increase the max sensor rate to 1 ms (1000Hz) the increased maximum rate is needed for proper gyro integration, current gyro parts can sample at up to 800Hz Change-Id: Ide75f6d5bc7a0fdafeb2dafd72db39e7afb9e794 --- services/sensorservice/SensorService.cpp | 3 ++- services/sensorservice/SensorService.h | 4 ++-- services/sensorservice/tests/sensorservicetest.cpp | 14 ++++++++++++-- 3 files changed, 16 insertions(+), 5 deletions(-) diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp index e204e0402..c9ab99224 100644 --- a/services/sensorservice/SensorService.cpp +++ b/services/sensorservice/SensorService.cpp @@ -175,10 +175,11 @@ status_t SensorService::dump(int fd, const Vector& args) for (size_t i=0 ; i)\n", + snprintf(buffer, SIZE, "%s (vendor=%s, handle=%d, maxRate=%.2fHz, last=<%5.1f,%5.1f,%5.1f>)\n", s.getName().string(), s.getVendor().string(), s.getHandle(), + s.getMinDelay() ? (1000000.0f / s.getMinDelay()) : 0.0f, e.data[0], e.data[1], e.data[2]); result.append(buffer); } diff --git a/services/sensorservice/SensorService.h b/services/sensorservice/SensorService.h index 9f37799fe..dfb1c0e15 100644 --- a/services/sensorservice/SensorService.h +++ b/services/sensorservice/SensorService.h @@ -49,8 +49,8 @@ class SensorService : { friend class BinderService; - static const nsecs_t MINIMUM_EVENTS_PERIOD = 10000000; // 10ms - static const nsecs_t DEFAULT_EVENTS_PERIOD = 200000000; // 200 ms + static const nsecs_t MINIMUM_EVENTS_PERIOD = 1000000; // 1000 Hz + static const nsecs_t DEFAULT_EVENTS_PERIOD = 200000000; // 5 Hz SensorService(); virtual ~SensorService(); diff --git a/services/sensorservice/tests/sensorservicetest.cpp b/services/sensorservice/tests/sensorservicetest.cpp index 42bf983fe..aea106224 100644 --- a/services/sensorservice/tests/sensorservicetest.cpp +++ b/services/sensorservice/tests/sensorservicetest.cpp @@ -27,15 +27,25 @@ int receiver(int fd, int events, void* data) sp q((SensorEventQueue*)data); ssize_t n; ASensorEvent buffer[8]; + + static nsecs_t oldTimeStamp = 0; + while ((n = q->read(buffer, 8)) > 0) { for (int i=0 ; i\n", buffer[i].timestamp, buffer[i].acceleration.x, buffer[i].acceleration.y, buffer[i].acceleration.z); } + + if (oldTimeStamp) { + float t = float(buffer[i].timestamp - oldTimeStamp) / s2ns(1); + printf("%f ms (%f Hz)\n", t*1000, 1.0/t); + } + oldTimeStamp = buffer[i].timestamp; + } } if (n<0 && n != -EAGAIN) { @@ -56,7 +66,7 @@ int main(int argc, char** argv) sp q = mgr.createEventQueue(); printf("queue=%p\n", q.get()); - Sensor const* accelerometer = mgr.getDefaultSensor(Sensor::TYPE_ACCELEROMETER); + Sensor const* accelerometer = mgr.getDefaultSensor(Sensor::TYPE_GYROSCOPE); printf("accelerometer=%p (%s)\n", accelerometer, accelerometer->getName().string()); q->enableSensor(accelerometer); From 94e8f6813f3fb2beaa9bcbfb1ad9b4ae2eb46949 Mon Sep 17 00:00:00 2001 From: Mathias Agopian Date: Wed, 10 Nov 2010 17:50:28 -0800 Subject: [PATCH 12/51] record the last event received regardless of having clients or not We only recorded the last received event (which is needed when a sensor is activated on a connection) when there was some connection active. This should fix an issue where sometimes the light sensor doesn't return an event whent activated. we also didn't need to hold the main lock while dispatching events to clients. Change-Id: I6c6386c040051ce205e3c0516c678e0603fa45e1 --- services/sensorservice/SensorService.cpp | 34 ++++++++++++++---------- services/sensorservice/SensorService.h | 2 ++ 2 files changed, 22 insertions(+), 14 deletions(-) diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp index c9ab99224..b5e73ac5e 100644 --- a/services/sensorservice/SensorService.cpp +++ b/services/sensorservice/SensorService.cpp @@ -218,25 +218,13 @@ bool SensorService::threadLoop() break; } + recordLastValue(buffer, count); + const SortedVector< wp > activeConnections( getActiveConnections()); size_t numConnections = activeConnections.size(); if (numConnections) { - Mutex::Autolock _l(mLock); - - // record the last event for each sensor - int32_t prev = buffer[0].sensor; - for (ssize_t i=1 ; i connection(activeConnections[i].promote()); if (connection != 0) { @@ -251,6 +239,24 @@ bool SensorService::threadLoop() return false; } +void SensorService::recordLastValue( + sensors_event_t const * buffer, size_t count) +{ + Mutex::Autolock _l(mLock); + + // record the last event for each sensor + int32_t prev = buffer[0].sensor; + for (size_t i=1 ; i > SensorService::getActiveConnections() const { diff --git a/services/sensorservice/SensorService.h b/services/sensorservice/SensorService.h index dfb1c0e15..b442779d5 100644 --- a/services/sensorservice/SensorService.h +++ b/services/sensorservice/SensorService.h @@ -111,6 +111,8 @@ class SensorService : String8 getSensorName(int handle) const; status_t recomputeEventsPeriodLocked(int32_t handle); + void recordLastValue(sensors_event_t const * buffer, size_t count); + // constants Vector mSensorList; struct sensors_poll_device_t* mSensorDevice; From 71d7a5c289c6ef6b5fc86dd4784a075ca6470e38 Mon Sep 17 00:00:00 2001 From: Mathias Agopian Date: Sun, 14 Nov 2010 20:55:25 -0800 Subject: [PATCH 13/51] Fix a race condition in sensormanager the per-connection state assumed the main sensorservice lock was held during access. This is however not true while pre-processing the events just before sending them to clients. Therefore, there was a small window during which this state could be modified while being used. we now have an internal lock that protects this state. Change-Id: I594680f20f09d6a4f1f38f093a1d3f650dcef1be --- services/sensorservice/SensorService.cpp | 6 ++++++ services/sensorservice/SensorService.h | 3 ++- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp index b5e73ac5e..22a45df06 100644 --- a/services/sensorservice/SensorService.cpp +++ b/services/sensorservice/SensorService.cpp @@ -464,6 +464,7 @@ void SensorService::SensorEventConnection::onFirstRef() } bool SensorService::SensorEventConnection::addSensor(int32_t handle) { + Mutex::Autolock _l(mConnectionLock); if (mSensorInfo.indexOfKey(handle) <= 0) { SensorInfo info; mSensorInfo.add(handle, info); @@ -473,6 +474,7 @@ bool SensorService::SensorEventConnection::addSensor(int32_t handle) { } bool SensorService::SensorEventConnection::removeSensor(int32_t handle) { + Mutex::Autolock _l(mConnectionLock); if (mSensorInfo.removeItem(handle) >= 0) { return true; } @@ -480,16 +482,19 @@ bool SensorService::SensorEventConnection::removeSensor(int32_t handle) { } bool SensorService::SensorEventConnection::hasSensor(int32_t handle) const { + Mutex::Autolock _l(mConnectionLock); return mSensorInfo.indexOfKey(handle) >= 0; } bool SensorService::SensorEventConnection::hasAnySensor() const { + Mutex::Autolock _l(mConnectionLock); return mSensorInfo.size() ? true : false; } status_t SensorService::SensorEventConnection::setEventRateLocked( int handle, nsecs_t ns) { + Mutex::Autolock _l(mConnectionLock); ssize_t index = mSensorInfo.indexOfKey(handle); if (index >= 0) { SensorInfo& info = mSensorInfo.editValueFor(handle); @@ -506,6 +511,7 @@ status_t SensorService::SensorEventConnection::sendEvents( // filter out events not for this connection size_t count = 0; if (scratch) { + Mutex::Autolock _l(mConnectionLock); size_t i=0; while (i const mService; sp const mChannel; + mutable Mutex mConnectionLock; - // protected by SensorService::mLock + // protected mConnectionLock struct SensorInfo { SensorInfo() : ns(DEFAULT_EVENTS_PERIOD) { } nsecs_t ns; From f001c92436b4a66eb7687286325ced7f10c9f917 Mon Sep 17 00:00:00 2001 From: Mathias Agopian Date: Thu, 11 Nov 2010 17:58:51 -0800 Subject: [PATCH 14/51] Add support for virtual sensors. Rework sensorservice to allow "virtual sensors", that is sensors that report a synthetized value based on real sensors. the main change to sensorservice is around managing which real sensor need to be activated and which rate to use. The logic for all this has been moved into SensorDevice, which essentially wraps the sensor HAL but adds two features to it: - it keeps track of which sensors need to be activated - it keeps track of what rate needs to be used For this purpose an "identity" is associated with each real sensor activation, so we can track them. On start-up we check for gravity, linear-acceleration and rotation-vector sensors, if they're not present in the HAL, we synthetize them in sensor-service. Change-Id: I841db2c1b37ef127ed571efa21732ecc5adf1800 --- services/sensorservice/Android.mk | 8 +- services/sensorservice/GravitySensor.cpp | 112 ++++++ services/sensorservice/GravitySensor.h | 56 +++ .../LinearAccelerationSensor.cpp | 86 +++++ .../sensorservice/LinearAccelerationSensor.h | 53 +++ .../sensorservice/RotationVectorSensor.cpp | 174 +++++++++ services/sensorservice/RotationVectorSensor.h | 60 +++ .../SecondOrderLowPassFilter.cpp | 70 ++++ .../sensorservice/SecondOrderLowPassFilter.h | 61 +++ services/sensorservice/SensorDevice.cpp | 239 ++++++++++++ services/sensorservice/SensorDevice.h | 62 ++++ services/sensorservice/SensorInterface.cpp | 70 ++++ services/sensorservice/SensorInterface.h | 75 ++++ services/sensorservice/SensorService.cpp | 351 ++++++++---------- services/sensorservice/SensorService.h | 27 +- 15 files changed, 1298 insertions(+), 206 deletions(-) create mode 100644 services/sensorservice/GravitySensor.cpp create mode 100644 services/sensorservice/GravitySensor.h create mode 100644 services/sensorservice/LinearAccelerationSensor.cpp create mode 100644 services/sensorservice/LinearAccelerationSensor.h create mode 100644 services/sensorservice/RotationVectorSensor.cpp create mode 100644 services/sensorservice/RotationVectorSensor.h create mode 100644 services/sensorservice/SecondOrderLowPassFilter.cpp create mode 100644 services/sensorservice/SecondOrderLowPassFilter.h create mode 100644 services/sensorservice/SensorDevice.cpp create mode 100644 services/sensorservice/SensorDevice.h create mode 100644 services/sensorservice/SensorInterface.cpp create mode 100644 services/sensorservice/SensorInterface.h diff --git a/services/sensorservice/Android.mk b/services/sensorservice/Android.mk index 75f690f18..7e17fdd58 100644 --- a/services/sensorservice/Android.mk +++ b/services/sensorservice/Android.mk @@ -2,7 +2,13 @@ LOCAL_PATH:= $(call my-dir) include $(CLEAR_VARS) LOCAL_SRC_FILES:= \ - SensorService.cpp + GravitySensor.cpp \ + LinearAccelerationSensor.cpp \ + RotationVectorSensor.cpp \ + SensorService.cpp \ + SensorInterface.cpp \ + SensorDevice.cpp \ + SecondOrderLowPassFilter.cpp LOCAL_CFLAGS:= -DLOG_TAG=\"SensorService\" diff --git a/services/sensorservice/GravitySensor.cpp b/services/sensorservice/GravitySensor.cpp new file mode 100644 index 000000000..18bd359ea --- /dev/null +++ b/services/sensorservice/GravitySensor.cpp @@ -0,0 +1,112 @@ +/* + * Copyright (C) 2010 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include + +#include + +#include + +#include "GravitySensor.h" + +namespace android { +// --------------------------------------------------------------------------- + +GravitySensor::GravitySensor(sensor_t const* list, size_t count) + : mSensorDevice(SensorDevice::getInstance()), + mEnabled(false), mAccTime(0), + mLowPass(M_SQRT1_2, 1), + mX(mLowPass), mY(mLowPass), mZ(mLowPass) + +{ + for (size_t i=0 ; idata[0] = x; + outEvent->data[1] = y; + outEvent->data[2] = z; + outEvent->sensor = '_grv'; + outEvent->type = SENSOR_TYPE_GRAVITY; + return true; + } + return false; +} + +bool GravitySensor::isEnabled() const { + return mEnabled; +} + +status_t GravitySensor::activate(void* ident, bool enabled) { + status_t err = mSensorDevice.activate(this, mAccelerometer.getHandle(), enabled); + if (err == NO_ERROR) { + mEnabled = enabled; + if (enabled) { + mAccTime = 0; + } + } + return err; +} + +status_t GravitySensor::setDelay(void* ident, int handle, int64_t ns) +{ + return mSensorDevice.setDelay(this, mAccelerometer.getHandle(), ns); +} + +Sensor GravitySensor::getSensor() const { + sensor_t hwSensor; + hwSensor.name = "Gravity Sensor"; + hwSensor.vendor = "Google Inc."; + hwSensor.version = 1; + hwSensor.handle = '_grv'; + hwSensor.type = SENSOR_TYPE_GRAVITY; + hwSensor.maxRange = mAccelerometer.getMaxValue(); + hwSensor.resolution = mAccelerometer.getResolution(); + hwSensor.power = mAccelerometer.getPowerUsage(); + hwSensor.minDelay = mAccelerometer.getMinDelay(); + Sensor sensor(&hwSensor); + return sensor; +} + +// --------------------------------------------------------------------------- +}; // namespace android + diff --git a/services/sensorservice/GravitySensor.h b/services/sensorservice/GravitySensor.h new file mode 100644 index 000000000..f9850b75f --- /dev/null +++ b/services/sensorservice/GravitySensor.h @@ -0,0 +1,56 @@ +/* + * Copyright (C) 2010 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ANDROID_GRAVITY_SENSOR_H +#define ANDROID_GRAVITY_SENSOR_H + +#include +#include + +#include + +#include "SensorDevice.h" +#include "SensorInterface.h" +#include "SecondOrderLowPassFilter.h" + +// --------------------------------------------------------------------------- +namespace android { +// --------------------------------------------------------------------------- + +class GravitySensor : public SensorInterface { + SensorDevice& mSensorDevice; + Sensor mAccelerometer; + bool mEnabled; + double mAccTime; + + SecondOrderLowPassFilter mLowPass; + BiquadFilter mX, mY, mZ; + +public: + GravitySensor(sensor_t const* list, size_t count); + virtual bool process(sensors_event_t* outEvent, + const sensors_event_t& event); + virtual bool isEnabled() const; + virtual status_t activate(void* ident, bool enabled); + virtual status_t setDelay(void* ident, int handle, int64_t ns); + virtual Sensor getSensor() const; + virtual bool isVirtual() const { return true; } +}; + +// --------------------------------------------------------------------------- +}; // namespace android + +#endif // ANDROID_GRAVITY_SENSOR_H diff --git a/services/sensorservice/LinearAccelerationSensor.cpp b/services/sensorservice/LinearAccelerationSensor.cpp new file mode 100644 index 000000000..2dc12dc50 --- /dev/null +++ b/services/sensorservice/LinearAccelerationSensor.cpp @@ -0,0 +1,86 @@ +/* + * Copyright (C) 2010 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include + +#include + +#include + +#include "LinearAccelerationSensor.h" + +namespace android { +// --------------------------------------------------------------------------- + +LinearAccelerationSensor::LinearAccelerationSensor(sensor_t const* list, size_t count) + : mSensorDevice(SensorDevice::getInstance()), + mGravitySensor(list, count) +{ + mData[0] = mData[1] = mData[2] = 0; +} + +bool LinearAccelerationSensor::process(sensors_event_t* outEvent, + const sensors_event_t& event) +{ + bool result = mGravitySensor.process(outEvent, event); + if (result) { + if (event.type == SENSOR_TYPE_ACCELEROMETER) { + mData[0] = event.acceleration.x; + mData[1] = event.acceleration.y; + mData[2] = event.acceleration.z; + } + outEvent->data[0] = mData[0] - outEvent->data[0]; + outEvent->data[1] = mData[1] - outEvent->data[1]; + outEvent->data[2] = mData[2] - outEvent->data[2]; + outEvent->sensor = '_lin'; + outEvent->type = SENSOR_TYPE_LINEAR_ACCELERATION; + } + return result; +} + +bool LinearAccelerationSensor::isEnabled() const { + return mGravitySensor.isEnabled(); +} + +status_t LinearAccelerationSensor::activate(void* ident, bool enabled) { + return mGravitySensor.activate(ident, enabled); +} + +status_t LinearAccelerationSensor::setDelay(void* ident, int handle, int64_t ns) { + return mGravitySensor.setDelay(ident, handle, ns); +} + +Sensor LinearAccelerationSensor::getSensor() const { + Sensor gsensor(mGravitySensor.getSensor()); + sensor_t hwSensor; + hwSensor.name = "Linear Acceleration Sensor"; + hwSensor.vendor = "Google Inc."; + hwSensor.version = 1; + hwSensor.handle = '_lin'; + hwSensor.type = SENSOR_TYPE_LINEAR_ACCELERATION; + hwSensor.maxRange = gsensor.getMaxValue(); + hwSensor.resolution = gsensor.getResolution(); + hwSensor.power = gsensor.getPowerUsage(); + hwSensor.minDelay = gsensor.getMinDelay(); + Sensor sensor(&hwSensor); + return sensor; +} + +// --------------------------------------------------------------------------- +}; // namespace android + diff --git a/services/sensorservice/LinearAccelerationSensor.h b/services/sensorservice/LinearAccelerationSensor.h new file mode 100644 index 000000000..ee918ce1c --- /dev/null +++ b/services/sensorservice/LinearAccelerationSensor.h @@ -0,0 +1,53 @@ +/* + * Copyright (C) 2010 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ANDROID_LINEAR_ACCELERATION_SENSOR_H +#define ANDROID_LINEAR_ACCELERATION_SENSOR_H + +#include +#include + +#include + +#include "SensorDevice.h" +#include "SensorInterface.h" +#include "GravitySensor.h" + +// --------------------------------------------------------------------------- + +namespace android { +// --------------------------------------------------------------------------- + +class LinearAccelerationSensor : public SensorInterface { + SensorDevice& mSensorDevice; + GravitySensor mGravitySensor; + float mData[3]; + + virtual bool process(sensors_event_t* outEvent, + const sensors_event_t& event); +public: + LinearAccelerationSensor(sensor_t const* list, size_t count); + virtual bool isEnabled() const; + virtual status_t activate(void* ident, bool enabled); + virtual status_t setDelay(void* ident, int handle, int64_t ns); + virtual Sensor getSensor() const; + virtual bool isVirtual() const { return true; } +}; + +// --------------------------------------------------------------------------- +}; // namespace android + +#endif // ANDROID_LINEAR_ACCELERATION_SENSOR_H diff --git a/services/sensorservice/RotationVectorSensor.cpp b/services/sensorservice/RotationVectorSensor.cpp new file mode 100644 index 000000000..6f4b8be45 --- /dev/null +++ b/services/sensorservice/RotationVectorSensor.cpp @@ -0,0 +1,174 @@ +/* + * Copyright (C) 2010 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include + +#include + +#include + +#include "RotationVectorSensor.h" + +namespace android { +// --------------------------------------------------------------------------- + +template +static inline T clamp(T v) { + return v < 0 ? 0 : v; +} + +RotationVectorSensor::RotationVectorSensor(sensor_t const* list, size_t count) + : mSensorDevice(SensorDevice::getInstance()), + mEnabled(false), + mALowPass(M_SQRT1_2, 5.0f), + mAX(mALowPass), mAY(mALowPass), mAZ(mALowPass), + mMLowPass(M_SQRT1_2, 2.5f), + mMX(mMLowPass), mMY(mMLowPass), mMZ(mMLowPass) +{ + for (size_t i=0 ; i 100. + return false; + } + const float invH = 1.0f / normH; + const float invA = 1.0f / sqrtf(Ax*Ax + Ay*Ay + Az*Az); + Hx *= invH; + Hy *= invH; + Hz *= invH; + Ax *= invA; + Ay *= invA; + Az *= invA; + const float Mx = Ay*Hz - Az*Hy; + const float My = Az*Hx - Ax*Hz; + const float Mz = Ax*Hy - Ay*Hx; + + // matrix to rotation vector (normalized quaternion) + float qw = sqrtf( clamp( Hx + My + Az + 1) * 0.25f ); + float qx = sqrtf( clamp( Hx - My - Az + 1) * 0.25f ); + float qy = sqrtf( clamp(-Hx + My - Az + 1) * 0.25f ); + float qz = sqrtf( clamp(-Hx - My + Az + 1) * 0.25f ); + const float n = 1.0f / (qw*qw + qx*qx + qy*qy + qz*qz); + qx = copysignf(qx, Ay - Mz) * n; + qy = copysignf(qy, Hz - Ax) * n; + qz = copysignf(qz, Mx - Hy) * n; + + *outEvent = event; + outEvent->data[0] = qx; + outEvent->data[1] = qy; + outEvent->data[2] = qz; + outEvent->sensor = '_rov'; + outEvent->type = SENSOR_TYPE_ROTATION_VECTOR; + return true; + } + return false; +} + +bool RotationVectorSensor::isEnabled() const { + return mEnabled; +} + +status_t RotationVectorSensor::activate(void* ident, bool enabled) { + if (mEnabled != enabled) { + mSensorDevice.activate(this, mAcc.getHandle(), enabled); + mSensorDevice.activate(this, mMag.getHandle(), enabled); + mEnabled = enabled; + if (enabled) { + mMagTime = 0; + mAccTime = 0; + } + } + return NO_ERROR; +} + +status_t RotationVectorSensor::setDelay(void* ident, int handle, int64_t ns) +{ + mSensorDevice.setDelay(this, mAcc.getHandle(), ns); + mSensorDevice.setDelay(this, mMag.getHandle(), ns); + return NO_ERROR; +} + +Sensor RotationVectorSensor::getSensor() const { + sensor_t hwSensor; + hwSensor.name = "Rotation Vector Sensor"; + hwSensor.vendor = "Google Inc."; + hwSensor.version = 1; + hwSensor.handle = '_rov'; + hwSensor.type = SENSOR_TYPE_ROTATION_VECTOR; + hwSensor.maxRange = 1; + hwSensor.resolution = 1.0f / (1<<24); + hwSensor.power = mAcc.getPowerUsage() + mMag.getPowerUsage(); + hwSensor.minDelay = mAcc.getMinDelay(); + Sensor sensor(&hwSensor); + return sensor; +} + +// --------------------------------------------------------------------------- +}; // namespace android + diff --git a/services/sensorservice/RotationVectorSensor.h b/services/sensorservice/RotationVectorSensor.h new file mode 100644 index 000000000..e7f28c93a --- /dev/null +++ b/services/sensorservice/RotationVectorSensor.h @@ -0,0 +1,60 @@ +/* + * Copyright (C) 2010 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ANDROID_ROTATION_VECTOR_SENSOR_H +#define ANDROID_ROTATION_VECTOR_SENSOR_H + +#include +#include + +#include + +#include "SensorDevice.h" +#include "SensorInterface.h" +#include "SecondOrderLowPassFilter.h" + +// --------------------------------------------------------------------------- +namespace android { +// --------------------------------------------------------------------------- + +class RotationVectorSensor : public SensorInterface { + SensorDevice& mSensorDevice; + Sensor mAcc; + Sensor mMag; + bool mEnabled; + float mMagData[3]; + double mAccTime; + double mMagTime; + SecondOrderLowPassFilter mALowPass; + BiquadFilter mAX, mAY, mAZ; + SecondOrderLowPassFilter mMLowPass; + BiquadFilter mMX, mMY, mMZ; + +public: + RotationVectorSensor(sensor_t const* list, size_t count); + virtual bool process(sensors_event_t* outEvent, + const sensors_event_t& event); + virtual bool isEnabled() const; + virtual status_t activate(void* ident, bool enabled); + virtual status_t setDelay(void* ident, int handle, int64_t ns); + virtual Sensor getSensor() const; + virtual bool isVirtual() const { return true; } +}; + +// --------------------------------------------------------------------------- +}; // namespace android + +#endif // ANDROID_ROTATION_VECTOR_SENSOR_H diff --git a/services/sensorservice/SecondOrderLowPassFilter.cpp b/services/sensorservice/SecondOrderLowPassFilter.cpp new file mode 100644 index 000000000..e13e1362d --- /dev/null +++ b/services/sensorservice/SecondOrderLowPassFilter.cpp @@ -0,0 +1,70 @@ +/* + * Copyright (C) 2010 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include + +#include + +#include "SecondOrderLowPassFilter.h" + +// --------------------------------------------------------------------------- + +namespace android { +// --------------------------------------------------------------------------- + +SecondOrderLowPassFilter::SecondOrderLowPassFilter(float Q, float fc) + : iQ(1.0f / Q), fc(fc) +{ +} + +void SecondOrderLowPassFilter::setSamplingPeriod(float dT) +{ + K = tanf(float(M_PI) * fc * dT); + iD = 1.0f / (K*K + K*iQ + 1); + a0 = K*K*iD; + a1 = 2.0f * a0; + b1 = 2.0f*(K*K - 1)*iD; + b2 = (K*K - K*iQ + 1)*iD; +} + +// --------------------------------------------------------------------------- + +BiquadFilter::BiquadFilter(const SecondOrderLowPassFilter& s) + : s(s) +{ +} + +float BiquadFilter::init(float x) +{ + x1 = x2 = x; + y1 = y2 = x; + return x; +} + +float BiquadFilter::operator()(float x) +{ + float y = (x + x2)*s.a0 + x1*s.a1 - y1*s.b1 - y2*s.b2; + x2 = x1; + y2 = y1; + x1 = x; + y1 = y; + return y; +} + +// --------------------------------------------------------------------------- +}; // namespace android diff --git a/services/sensorservice/SecondOrderLowPassFilter.h b/services/sensorservice/SecondOrderLowPassFilter.h new file mode 100644 index 000000000..998ca35b9 --- /dev/null +++ b/services/sensorservice/SecondOrderLowPassFilter.h @@ -0,0 +1,61 @@ +/* + * Copyright (C) 2010 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ANDROID_SECOND_ORDER_LOW_PASS_FILTER_H +#define ANDROID_SECOND_ORDER_LOW_PASS_FILTER_H + +#include +#include + +// --------------------------------------------------------------------------- + +namespace android { +// --------------------------------------------------------------------------- + +class BiquadFilter; + +/* + * State of a 2nd order low-pass IIR filter + */ +class SecondOrderLowPassFilter { + friend class BiquadFilter; + float iQ, fc; + float K, iD; + float a0, a1; + float b1, b2; +public: + SecondOrderLowPassFilter(float Q, float fc); + void setSamplingPeriod(float dT); +}; + +/* + * Implements a Biquad IIR filter + */ +class BiquadFilter { + float x1, x2; + float y1, y2; + const SecondOrderLowPassFilter& s; +public: + BiquadFilter(const SecondOrderLowPassFilter& s); + float init(float in); + float operator()(float in); +}; + + +// --------------------------------------------------------------------------- +}; // namespace android + +#endif // ANDROID_SECOND_ORDER_LOW_PASS_FILTER_H diff --git a/services/sensorservice/SensorDevice.cpp b/services/sensorservice/SensorDevice.cpp new file mode 100644 index 000000000..73f85ba35 --- /dev/null +++ b/services/sensorservice/SensorDevice.cpp @@ -0,0 +1,239 @@ +/* + * Copyright (C) 2010 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include + +#include "SensorDevice.h" + +namespace android { +// --------------------------------------------------------------------------- +class BatteryService : public Singleton { + static const int TRANSACTION_noteStartSensor = IBinder::FIRST_CALL_TRANSACTION + 3; + static const int TRANSACTION_noteStopSensor = IBinder::FIRST_CALL_TRANSACTION + 4; + static const String16 DESCRIPTOR; + + friend class Singleton; + sp mBatteryStatService; + + BatteryService() { + const sp sm(defaultServiceManager()); + if (sm != NULL) { + const String16 name("batteryinfo"); + mBatteryStatService = sm->getService(name); + } + } + + status_t noteStartSensor(int uid, int handle) { + Parcel data, reply; + data.writeInterfaceToken(DESCRIPTOR); + data.writeInt32(uid); + data.writeInt32(handle); + status_t err = mBatteryStatService->transact( + TRANSACTION_noteStartSensor, data, &reply, 0); + err = reply.readExceptionCode(); + return err; + } + + status_t noteStopSensor(int uid, int handle) { + Parcel data, reply; + data.writeInterfaceToken(DESCRIPTOR); + data.writeInt32(uid); + data.writeInt32(handle); + status_t err = mBatteryStatService->transact( + TRANSACTION_noteStopSensor, data, &reply, 0); + err = reply.readExceptionCode(); + return err; + } + +public: + void enableSensor(int handle) { + if (mBatteryStatService != 0) { + int uid = IPCThreadState::self()->getCallingUid(); + int64_t identity = IPCThreadState::self()->clearCallingIdentity(); + noteStartSensor(uid, handle); + IPCThreadState::self()->restoreCallingIdentity(identity); + } + } + void disableSensor(int handle) { + if (mBatteryStatService != 0) { + int uid = IPCThreadState::self()->getCallingUid(); + int64_t identity = IPCThreadState::self()->clearCallingIdentity(); + noteStopSensor(uid, handle); + IPCThreadState::self()->restoreCallingIdentity(identity); + } + } +}; + +const String16 BatteryService::DESCRIPTOR("com.android.internal.app.IBatteryStats"); + +ANDROID_SINGLETON_STATIC_INSTANCE(BatteryService) + +// --------------------------------------------------------------------------- + +ANDROID_SINGLETON_STATIC_INSTANCE(SensorDevice) + +SensorDevice::SensorDevice() + : mSensorDevice(0), + mSensorModule(0) +{ + status_t err = hw_get_module(SENSORS_HARDWARE_MODULE_ID, + (hw_module_t const**)&mSensorModule); + + LOGE_IF(err, "couldn't load %s module (%s)", + SENSORS_HARDWARE_MODULE_ID, strerror(-err)); + + if (mSensorModule) { + err = sensors_open(&mSensorModule->common, &mSensorDevice); + + LOGE_IF(err, "couldn't open device for module %s (%s)", + SENSORS_HARDWARE_MODULE_ID, strerror(-err)); + + if (mSensorDevice) { + sensor_t const* list; + ssize_t count = mSensorModule->get_sensors_list(mSensorModule, &list); + mActivationCount.setCapacity(count); + Info model; + for (size_t i=0 ; iactivate(mSensorDevice, list[i].handle, 0); + } + } + } +} + +void SensorDevice::dump(String8& result, char* buffer, size_t SIZE) +{ + if (!mSensorModule) return; + sensor_t const* list; + ssize_t count = mSensorModule->get_sensors_list(mSensorModule, &list); + + snprintf(buffer, SIZE, "%d h/w sensors:\n", int(count)); + result.append(buffer); + + Mutex::Autolock _l(mLock); + for (size_t i=0 ; iget_sensors_list(mSensorModule, list); + return count; +} + +status_t SensorDevice::initCheck() const { + return mSensorDevice && mSensorModule ? NO_ERROR : NO_INIT; +} + +ssize_t SensorDevice::poll(sensors_event_t* buffer, size_t count) { + if (!mSensorDevice) return NO_INIT; + return mSensorDevice->poll(mSensorDevice, buffer, count); +} + +status_t SensorDevice::activate(void* ident, int handle, int enabled) +{ + if (!mSensorDevice) return NO_INIT; + status_t err(NO_ERROR); + bool actuateHardware = false; + + Info& info( mActivationCount.editValueFor(handle) ); + int32_t& count(info.count); + if (enabled) { + if (android_atomic_inc(&count) == 0) { + actuateHardware = true; + } + Mutex::Autolock _l(mLock); + if (info.rates.indexOfKey(ident) < 0) { + info.rates.add(ident, DEFAULT_EVENTS_PERIOD); + } + } else { + if (android_atomic_dec(&count) == 1) { + actuateHardware = true; + } + Mutex::Autolock _l(mLock); + info.rates.removeItem(ident); + } + if (actuateHardware) { + err = mSensorDevice->activate(mSensorDevice, handle, enabled); + if (enabled) { + LOGE_IF(err, "Error activating sensor %d (%s)", handle, strerror(-err)); + if (err == 0) { + BatteryService::getInstance().enableSensor(handle); + } + } else { + if (err == 0) { + BatteryService::getInstance().disableSensor(handle); + } + } + } + + if (!actuateHardware || enabled) { + Mutex::Autolock _l(mLock); + nsecs_t ns = info.rates.valueAt(0); + for (size_t i=1 ; isetDelay(mSensorDevice, handle, ns); + } + + return err; +} + +status_t SensorDevice::setDelay(void* ident, int handle, int64_t ns) +{ + if (!mSensorDevice) return NO_INIT; + Info& info( mActivationCount.editValueFor(handle) ); + { // scope for lock + Mutex::Autolock _l(mLock); + ssize_t index = info.rates.indexOfKey(ident); + if (index < 0) return BAD_INDEX; + info.rates.editValueAt(index) = ns; + ns = info.rates.valueAt(0); + for (size_t i=1 ; isetDelay(mSensorDevice, handle, ns); +} + +// --------------------------------------------------------------------------- +}; // namespace android + diff --git a/services/sensorservice/SensorDevice.h b/services/sensorservice/SensorDevice.h new file mode 100644 index 000000000..63ecbcdde --- /dev/null +++ b/services/sensorservice/SensorDevice.h @@ -0,0 +1,62 @@ +/* + * Copyright (C) 2010 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ANDROID_SENSOR_DEVICE_H +#define ANDROID_SENSOR_DEVICE_H + +#include +#include + +#include +#include +#include + +#include + +// --------------------------------------------------------------------------- + +namespace android { +// --------------------------------------------------------------------------- + +static const nsecs_t DEFAULT_EVENTS_PERIOD = 200000000; // 5 Hz + +class SensorDevice : public Singleton { + friend class Singleton; + struct sensors_poll_device_t* mSensorDevice; + struct sensors_module_t* mSensorModule; + Mutex mLock; // protect mActivationCount[].rates + // fixed-size array after construction + struct Info { + Info() : count(0) { } + int32_t count; + KeyedVector rates; + }; + DefaultKeyedVector mActivationCount; + + SensorDevice(); +public: + ssize_t getSensorList(sensor_t const** list); + status_t initCheck() const; + ssize_t poll(sensors_event_t* buffer, size_t count); + status_t activate(void* ident, int handle, int enabled); + status_t setDelay(void* ident, int handle, int64_t ns); + void dump(String8& result, char* buffer, size_t SIZE); +}; + +// --------------------------------------------------------------------------- +}; // namespace android + +#endif // ANDROID_SENSOR_DEVICE_H diff --git a/services/sensorservice/SensorInterface.cpp b/services/sensorservice/SensorInterface.cpp new file mode 100644 index 000000000..93d23d9a4 --- /dev/null +++ b/services/sensorservice/SensorInterface.cpp @@ -0,0 +1,70 @@ +/* + * Copyright (C) 2010 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include + +#include + +#include "SensorInterface.h" + +namespace android { +// --------------------------------------------------------------------------- + +SensorInterface::~SensorInterface() +{ +} + +// --------------------------------------------------------------------------- + +HardwareSensor::HardwareSensor(const sensor_t& sensor) + : mSensorDevice(SensorDevice::getInstance()), + mSensor(&sensor), mEnabled(false) +{ + LOGI("%s", sensor.name); +} + +HardwareSensor::~HardwareSensor() { +} + +bool HardwareSensor::process(sensors_event_t* outEvent, + const sensors_event_t& event) { + *outEvent = event; + return true; +} + +bool HardwareSensor::isEnabled() const { + return mEnabled; +} + +status_t HardwareSensor::activate(void* ident,bool enabled) { + status_t err = mSensorDevice.activate(ident, mSensor.getHandle(), enabled); + if (err == NO_ERROR) + mEnabled = enabled; + return err; +} + +status_t HardwareSensor::setDelay(void* ident, int handle, int64_t ns) { + return mSensorDevice.setDelay(ident, handle, ns); +} + +Sensor HardwareSensor::getSensor() const { + return mSensor; +} + + +// --------------------------------------------------------------------------- +}; // namespace android diff --git a/services/sensorservice/SensorInterface.h b/services/sensorservice/SensorInterface.h new file mode 100644 index 000000000..eebd5638f --- /dev/null +++ b/services/sensorservice/SensorInterface.h @@ -0,0 +1,75 @@ +/* + * Copyright (C) 2010 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ANDROID_SENSOR_INTERFACE_H +#define ANDROID_SENSOR_INTERFACE_H + +#include +#include + +#include + +#include + +#include "SensorDevice.h" + +// --------------------------------------------------------------------------- + +namespace android { +// --------------------------------------------------------------------------- + +class SensorInterface { +public: + virtual ~SensorInterface(); + + virtual bool process(sensors_event_t* outEvent, + const sensors_event_t& event) = 0; + + virtual bool isEnabled() const = 0; + virtual status_t activate(void* ident, bool enabled) = 0; + virtual status_t setDelay(void* ident, int handle, int64_t ns) = 0; + virtual Sensor getSensor() const = 0; + virtual bool isVirtual() const = 0; +}; + +// --------------------------------------------------------------------------- + +class HardwareSensor : public SensorInterface +{ + SensorDevice& mSensorDevice; + Sensor mSensor; + bool mEnabled; + +public: + HardwareSensor(const sensor_t& sensor); + + virtual ~HardwareSensor(); + + virtual bool process(sensors_event_t* outEvent, + const sensors_event_t& event); + + virtual bool isEnabled() const; + virtual status_t activate(void* ident, bool enabled); + virtual status_t setDelay(void* ident, int handle, int64_t ns); + virtual Sensor getSensor() const; + virtual bool isVirtual() const { return false; } +}; + + +// --------------------------------------------------------------------------- +}; // namespace android + +#endif // ANDROID_SENSOR_INTERFACE_H diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp index 22a45df06..ea5e5ccd8 100644 --- a/services/sensorservice/SensorService.cpp +++ b/services/sensorservice/SensorService.cpp @@ -15,6 +15,7 @@ */ #include +#include #include #include @@ -35,80 +36,15 @@ #include #include "SensorService.h" +#include "GravitySensor.h" +#include "LinearAccelerationSensor.h" +#include "RotationVectorSensor.h" namespace android { // --------------------------------------------------------------------------- -class BatteryService : public Singleton { - static const int TRANSACTION_noteStartSensor = IBinder::FIRST_CALL_TRANSACTION + 3; - static const int TRANSACTION_noteStopSensor = IBinder::FIRST_CALL_TRANSACTION + 4; - static const String16 DESCRIPTOR; - - friend class Singleton; - sp mBatteryStatService; - - BatteryService() { - const sp sm(defaultServiceManager()); - if (sm != NULL) { - const String16 name("batteryinfo"); - mBatteryStatService = sm->getService(name); - } - } - - status_t noteStartSensor(int uid, int handle) { - Parcel data, reply; - data.writeInterfaceToken(DESCRIPTOR); - data.writeInt32(uid); - data.writeInt32(handle); - status_t err = mBatteryStatService->transact( - TRANSACTION_noteStartSensor, data, &reply, 0); - err = reply.readExceptionCode(); - return err; - } - - status_t noteStopSensor(int uid, int handle) { - Parcel data, reply; - data.writeInterfaceToken(DESCRIPTOR); - data.writeInt32(uid); - data.writeInt32(handle); - status_t err = mBatteryStatService->transact( - TRANSACTION_noteStopSensor, data, &reply, 0); - err = reply.readExceptionCode(); - return err; - } - -public: - void enableSensor(int handle) { - if (mBatteryStatService != 0) { - int uid = IPCThreadState::self()->getCallingUid(); - int64_t identity = IPCThreadState::self()->clearCallingIdentity(); - noteStartSensor(uid, handle); - IPCThreadState::self()->restoreCallingIdentity(identity); - } - } - void disableSensor(int handle) { - if (mBatteryStatService != 0) { - int uid = IPCThreadState::self()->getCallingUid(); - int64_t identity = IPCThreadState::self()->clearCallingIdentity(); - noteStopSensor(uid, handle); - IPCThreadState::self()->restoreCallingIdentity(identity); - } - } -}; - -const String16 BatteryService::DESCRIPTOR("com.android.internal.app.IBatteryStats"); - -ANDROID_SINGLETON_STATIC_INSTANCE(BatteryService) - -// --------------------------------------------------------------------------- - -// 100 events/s max -static const nsecs_t MINIMUM_EVENT_PERIOD = ms2ns(10); - SensorService::SensorService() : Thread(false), - mSensorDevice(0), - mSensorModule(0), mDump("android.permission.DUMP"), mInitCheck(NO_INIT) { @@ -118,43 +54,66 @@ void SensorService::onFirstRef() { LOGD("nuSensorService starting..."); - status_t err = hw_get_module(SENSORS_HARDWARE_MODULE_ID, - (hw_module_t const**)&mSensorModule); + SensorDevice& dev(SensorDevice::getInstance()); - LOGE_IF(err, "couldn't load %s module (%s)", - SENSORS_HARDWARE_MODULE_ID, strerror(-err)); - - if (mSensorModule) { - err = sensors_open(&mSensorModule->common, &mSensorDevice); - - LOGE_IF(err, "couldn't open device for module %s (%s)", - SENSORS_HARDWARE_MODULE_ID, strerror(-err)); - - sensors_event_t event; - memset(&event, 0, sizeof(event)); - - struct sensor_t const* list; - int count = mSensorModule->get_sensors_list(mSensorModule, &list); + if (dev.initCheck() == NO_ERROR) { + uint32_t virtualSensorsNeeds = + (1<activate(mSensorDevice, sensor.getHandle(), 0); + registerSensor( new HardwareSensor(list[i]) ); + switch (list[i].type) { + case SENSOR_TYPE_GRAVITY: + case SENSOR_TYPE_LINEAR_ACCELERATION: + case SENSOR_TYPE_ROTATION_VECTOR: + virtualSensorsNeeds &= ~(1<getSensor()); + // add to the sensor list (returned to clients) + mSensorList.add(sensor); + // add to our handle->SensorInterface mapping + mSensorMap.add(sensor.getHandle(), s); + // create an entry in the mLastEventSeen array + mLastEventSeen.add(sensor.getHandle(), event); +} + +void SensorService::registerVirtualSensor(SensorInterface* s) +{ + registerSensor(s); + mVirtualSensorList.add( s ); +} + SensorService::~SensorService() { + for (size_t i=0 ; i& args) @@ -175,7 +134,7 @@ status_t SensorService::dump(int fd, const Vector& args) for (size_t i=0 ; i)\n", + snprintf(buffer, SIZE, "%-48s| %-32s | 0x%08x | maxRate=%7.2fHz | last=<%5.1f,%5.1f,%5.1f>\n", s.getName().string(), s.getVendor().string(), s.getHandle(), @@ -183,6 +142,7 @@ status_t SensorService::dump(int fd, const Vector& args) e.data[0], e.data[1], e.data[2]); result.append(buffer); } + SensorDevice::getInstance().dump(result, buffer, SIZE); snprintf(buffer, SIZE, "%d active connections\n", mActiveConnections.size()); @@ -191,7 +151,7 @@ status_t SensorService::dump(int fd, const Vector& args) result.append(buffer); for (size_t i=0 ; igetNumConnections()); @@ -206,13 +166,15 @@ bool SensorService::threadLoop() { LOGD("nuSensorService thread starting..."); - sensors_event_t buffer[16]; - sensors_event_t scratch[16]; - struct sensors_poll_device_t* device = mSensorDevice; - ssize_t count; + const size_t numEventMax = 16 * (1 + mVirtualSensorList.size()); + sensors_event_t buffer[numEventMax]; + sensors_event_t scratch[numEventMax]; + SensorDevice& device(SensorDevice::getInstance()); + const size_t vcount = mVirtualSensorList.size(); + ssize_t count; do { - count = device->poll(device, buffer, sizeof(buffer)/sizeof(*buffer)); + count = device.poll(buffer, numEventMax); if (count<0) { LOGE("sensor poll failed (%s)", strerror(-count)); break; @@ -220,19 +182,44 @@ bool SensorService::threadLoop() recordLastValue(buffer, count); - const SortedVector< wp > activeConnections( - getActiveConnections()); - - size_t numConnections = activeConnections.size(); - if (numConnections) { - for (size_t i=0 ; i connection(activeConnections[i].promote()); - if (connection != 0) { - connection->sendEvents(buffer, count, scratch); + // handle virtual sensors + if (count && vcount) { + const DefaultKeyedVector virtualSensors( + getActiveVirtualSensors()); + const size_t activeVirtualSensorCount = virtualSensors.size(); + if (activeVirtualSensorCount) { + size_t k = 0; + for (size_t i=0 ; iprocess(&out, event[i])) { + buffer[count + k] = out; + k++; + } + } + } + if (k) { + // record the last synthesized values + recordLastValue(&buffer[count], k); + count += k; + // sort the buffer by time-stamps + sortEventBuffer(buffer, count); } } } + // send our events to clients... + const SortedVector< wp > activeConnections( + getActiveConnections()); + size_t numConnections = activeConnections.size(); + for (size_t i=0 ; i connection( + activeConnections[i].promote()); + if (connection != 0) { + connection->sendEvents(buffer, count, scratch); + } + } } while (count >= 0 || Thread::exitPending()); LOGW("Exiting SensorService::threadLoop!"); @@ -257,6 +244,18 @@ void SensorService::recordLastValue( mLastEventSeen.editValueFor(prev) = buffer[count-1]; } +void SensorService::sortEventBuffer(sensors_event_t* buffer, size_t count) +{ + struct compar { + static int cmp(void const* lhs, void const* rhs) { + sensors_event_t const* l = static_cast(lhs); + sensors_event_t const* r = static_cast(rhs); + return r->timestamp - l->timestamp; + } + }; + qsort(buffer, count, sizeof(sensors_event_t), compar::cmp); +} + SortedVector< wp > SensorService::getActiveConnections() const { @@ -264,6 +263,13 @@ SensorService::getActiveConnections() const return mActiveConnections; } +DefaultKeyedVector +SensorService::getActiveVirtualSensors() const +{ + Mutex::Autolock _l(mLock); + return mActiveVirtualSensors; +} + String8 SensorService::getSensorName(int handle) const { size_t count = mSensorList.size(); for (size_t i=0 ; i& connectio for (size_t i=0 ; iremoveConnection(connection)) { - mSensorDevice->activate(mSensorDevice, mActiveSensors.keyAt(i), 0); + int handle = mActiveSensors.keyAt(i); + SensorInterface* sensor = mSensorMap.valueFor( handle ); + if (sensor) { + sensor->activate(connection.unsafe_get(), false); + } mActiveSensors.removeItemsAt(i, 1); + mActiveVirtualSensors.removeItem(handle); delete rec; size--; } else { @@ -311,39 +322,38 @@ status_t SensorService::enable(const sp& connection, if (mInitCheck != NO_ERROR) return mInitCheck; - status_t err = NO_ERROR; Mutex::Autolock _l(mLock); - SensorRecord* rec = mActiveSensors.valueFor(handle); - if (rec == 0) { - rec = new SensorRecord(connection); - mActiveSensors.add(handle, rec); - err = mSensorDevice->activate(mSensorDevice, handle, 1); - LOGE_IF(err, "Error activating sensor %d (%s)", handle, strerror(-err)); - if (err == 0) { - BatteryService::getInstance().enableSensor(handle); - } - } else { - if (rec->addConnection(connection)) { - // this sensor is already activated, but we are adding a - // connection that uses it. Immediately send down the last - // known value of the requested sensor. - sensors_event_t scratch; - sensors_event_t& event(mLastEventSeen.editValueFor(handle)); - if (event.version == sizeof(sensors_event_t)) { - connection->sendEvents(&event, 1); - } - } - } + SensorInterface* sensor = mSensorMap.valueFor(handle); + status_t err = sensor ? sensor->activate(connection.get(), true) : status_t(BAD_VALUE); if (err == NO_ERROR) { - // connection now active - if (connection->addSensor(handle)) { - // the sensor was added (which means it wasn't already there) - // so, see if this connection becomes active - if (mActiveConnections.indexOf(connection) < 0) { - mActiveConnections.add(connection); + SensorRecord* rec = mActiveSensors.valueFor(handle); + if (rec == 0) { + rec = new SensorRecord(connection); + mActiveSensors.add(handle, rec); + if (sensor->isVirtual()) { + mActiveVirtualSensors.add(handle, sensor); + } + } else { + if (rec->addConnection(connection)) { + // this sensor is already activated, but we are adding a + // connection that uses it. Immediately send down the last + // known value of the requested sensor. + sensors_event_t scratch; + sensors_event_t& event(mLastEventSeen.editValueFor(handle)); + if (event.version == sizeof(sensors_event_t)) { + connection->sendEvents(&event, 1); + } + } + } + if (err == NO_ERROR) { + // connection now active + if (connection->addSensor(handle)) { + // the sensor was added (which means it wasn't already there) + // so, see if this connection becomes active + if (mActiveConnections.indexOf(connection) < 0) { + mActiveConnections.add(connection); + } } - // this could change the sensor event delivery speed - recomputeEventsPeriodLocked(handle); } } return err; @@ -367,15 +377,11 @@ status_t SensorService::disable(const sp& connection, // see if this sensor becomes inactive if (rec->removeConnection(connection)) { mActiveSensors.removeItem(handle); + mActiveVirtualSensors.removeItem(handle); delete rec; - err = mSensorDevice->activate(mSensorDevice, handle, 0); - if (err == 0) { - BatteryService::getInstance().disableSensor(handle); - } } - } - if (err == NO_ERROR) { - recomputeEventsPeriodLocked(handle); + SensorInterface* sensor = mSensorMap.valueFor(handle); + err = sensor ? sensor->activate(connection.get(), false) : status_t(BAD_VALUE); } return err; } @@ -392,30 +398,9 @@ status_t SensorService::setEventRate(const sp& connection if (ns < MINIMUM_EVENTS_PERIOD) ns = MINIMUM_EVENTS_PERIOD; - Mutex::Autolock _l(mLock); - status_t err = connection->setEventRateLocked(handle, ns); - if (err == NO_ERROR) { - recomputeEventsPeriodLocked(handle); - } - return err; -} - -status_t SensorService::recomputeEventsPeriodLocked(int32_t handle) -{ - status_t err = NO_ERROR; - nsecs_t wanted = ms2ns(1000); - size_t count = mActiveConnections.size(); - for (size_t i=0 ; i connection(mActiveConnections[i].promote()); - if (connection != NULL) { - nsecs_t ns = connection->getEventRateForSensor(handle); - if (ns) { - wanted = wanted < ns ? wanted : ns; - } - } - } - err = mSensorDevice->setDelay(mSensorDevice, handle, wanted); - return err; + SensorInterface* sensor = mSensorMap.valueFor(handle); + if (!sensor) return BAD_VALUE; + return sensor->setDelay(connection.get(), handle, ns); } // --------------------------------------------------------------------------- @@ -465,9 +450,8 @@ void SensorService::SensorEventConnection::onFirstRef() bool SensorService::SensorEventConnection::addSensor(int32_t handle) { Mutex::Autolock _l(mConnectionLock); - if (mSensorInfo.indexOfKey(handle) <= 0) { - SensorInfo info; - mSensorInfo.add(handle, info); + if (mSensorInfo.indexOf(handle) <= 0) { + mSensorInfo.add(handle); return true; } return false; @@ -475,7 +459,7 @@ bool SensorService::SensorEventConnection::addSensor(int32_t handle) { bool SensorService::SensorEventConnection::removeSensor(int32_t handle) { Mutex::Autolock _l(mConnectionLock); - if (mSensorInfo.removeItem(handle) >= 0) { + if (mSensorInfo.remove(handle) >= 0) { return true; } return false; @@ -483,7 +467,7 @@ bool SensorService::SensorEventConnection::removeSensor(int32_t handle) { bool SensorService::SensorEventConnection::hasSensor(int32_t handle) const { Mutex::Autolock _l(mConnectionLock); - return mSensorInfo.indexOfKey(handle) >= 0; + return mSensorInfo.indexOf(handle) >= 0; } bool SensorService::SensorEventConnection::hasAnySensor() const { @@ -491,19 +475,6 @@ bool SensorService::SensorEventConnection::hasAnySensor() const { return mSensorInfo.size() ? true : false; } -status_t SensorService::SensorEventConnection::setEventRateLocked( - int handle, nsecs_t ns) -{ - Mutex::Autolock _l(mConnectionLock); - ssize_t index = mSensorInfo.indexOfKey(handle); - if (index >= 0) { - SensorInfo& info = mSensorInfo.editValueFor(handle); - info.ns = ns; - return NO_ERROR; - } - return status_t(index); -} - status_t SensorService::SensorEventConnection::sendEvents( sensors_event_t const* buffer, size_t numEvents, sensors_event_t* scratch) @@ -515,7 +486,7 @@ status_t SensorService::SensorEventConnection::sendEvents( size_t i=0; while (i= 0) { + if (mSensorInfo.indexOf(curr) >= 0) { do { scratch[count++] = buffer[i++]; } while ((i #include +#include "SensorInterface.h" + // --------------------------------------------------------------------------- struct sensors_poll_device_t; @@ -50,7 +52,6 @@ class SensorService : friend class BinderService; static const nsecs_t MINIMUM_EVENTS_PERIOD = 1000000; // 1000 Hz - static const nsecs_t DEFAULT_EVENTS_PERIOD = 200000000; // 5 Hz SensorService(); virtual ~SensorService(); @@ -77,12 +78,8 @@ class SensorService : sp const mChannel; mutable Mutex mConnectionLock; - // protected mConnectionLock - struct SensorInfo { - SensorInfo() : ns(DEFAULT_EVENTS_PERIOD) { } - nsecs_t ns; - }; - DefaultKeyedVector mSensorInfo; + // protected by SensorService::mLock + SortedVector mSensorInfo; public: SensorEventConnection(const sp& service); @@ -93,10 +90,6 @@ class SensorService : bool hasAnySensor() const; bool addSensor(int32_t handle); bool removeSensor(int32_t handle); - status_t setEventRateLocked(int handle, nsecs_t ns); - nsecs_t getEventRateForSensor(int32_t handle) const { - return mSensorInfo.valueFor(handle).ns; - } }; class SensorRecord { @@ -109,21 +102,25 @@ class SensorService : }; SortedVector< wp > getActiveConnections() const; - String8 getSensorName(int handle) const; - status_t recomputeEventsPeriodLocked(int32_t handle); + DefaultKeyedVector getActiveVirtualSensors() const; + String8 getSensorName(int handle) const; void recordLastValue(sensors_event_t const * buffer, size_t count); + static void sortEventBuffer(sensors_event_t* buffer, size_t count); + void registerSensor(SensorInterface* sensor); + void registerVirtualSensor(SensorInterface* sensor); // constants Vector mSensorList; - struct sensors_poll_device_t* mSensorDevice; - struct sensors_module_t* mSensorModule; + DefaultKeyedVector mSensorMap; + Vector mVirtualSensorList; Permission mDump; status_t mInitCheck; // protected by mLock mutable Mutex mLock; DefaultKeyedVector mActiveSensors; + DefaultKeyedVector mActiveVirtualSensors; SortedVector< wp > mActiveConnections; // The size of this vector is constant, only the items are mutable From 5c6d7ff3e643d0fb4b46d81a3a4e6edaba779edd Mon Sep 17 00:00:00 2001 From: Mathias Agopian Date: Mon, 22 Nov 2010 01:04:09 -0800 Subject: [PATCH 15/51] don't attempt to normalize the rotation vector indeed, by construction of the rotation matrix, it is guaranteed to have a length of 1. moreover, the normalization code was missing a square-root, fortunatelly, since the length is 1, this didn't cause any damage (since sqrt(1) = 1). Change-Id: I9facd668caaf5bb3bfccb139ab872f2bb2066365 --- services/sensorservice/RotationVectorSensor.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/services/sensorservice/RotationVectorSensor.cpp b/services/sensorservice/RotationVectorSensor.cpp index 6f4b8be45..eecf260a7 100644 --- a/services/sensorservice/RotationVectorSensor.cpp +++ b/services/sensorservice/RotationVectorSensor.cpp @@ -114,10 +114,12 @@ bool RotationVectorSensor::process(sensors_event_t* outEvent, float qx = sqrtf( clamp( Hx - My - Az + 1) * 0.25f ); float qy = sqrtf( clamp(-Hx + My - Az + 1) * 0.25f ); float qz = sqrtf( clamp(-Hx - My + Az + 1) * 0.25f ); - const float n = 1.0f / (qw*qw + qx*qx + qy*qy + qz*qz); - qx = copysignf(qx, Ay - Mz) * n; - qy = copysignf(qy, Hz - Ax) * n; - qz = copysignf(qz, Mx - Hy) * n; + qx = copysignf(qx, Ay - Mz); + qy = copysignf(qy, Hz - Ax); + qz = copysignf(qz, Mx - Hy); + + // this quaternion is guaranteed to be normalized, by construction + // of the rotation matrix. *outEvent = event; outEvent->data[0] = qx; From ddf1ceb647d029febfd007b389cd92c84668a03c Mon Sep 17 00:00:00 2001 From: Mathias Agopian Date: Mon, 22 Nov 2010 15:48:10 -0800 Subject: [PATCH 16/51] allow rotation-vector to have 4 components - upadte documentation for rotation vector - update method dealing with rotation vector to deal with 4 components - virtual rotation-vector sensor reports all four components - improve SensorManager documentation layout Whent he 4-th component of the rotation-vector is present, we can save a square-root when computing the quaternion or rotation matrix from it. Change-Id: Ia84d278dd5f0909fab1c5ba050f8df2679e2c7c8 --- services/sensorservice/RotationVectorSensor.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/services/sensorservice/RotationVectorSensor.cpp b/services/sensorservice/RotationVectorSensor.cpp index eecf260a7..50cd6beac 100644 --- a/services/sensorservice/RotationVectorSensor.cpp +++ b/services/sensorservice/RotationVectorSensor.cpp @@ -125,6 +125,7 @@ bool RotationVectorSensor::process(sensors_event_t* outEvent, outEvent->data[0] = qx; outEvent->data[1] = qy; outEvent->data[2] = qz; + outEvent->data[3] = qw; outEvent->sensor = '_rov'; outEvent->type = SENSOR_TYPE_ROTATION_VECTOR; return true; From 87c9dbb728febe9ce035874796c58f308043879d Mon Sep 17 00:00:00 2001 From: Mathias Agopian Date: Thu, 11 Nov 2010 17:58:51 -0800 Subject: [PATCH 17/51] Uses 4-th order low-pass for extracting gravity. Most accelerometers have 8-bits accuracy so we beed to reject 48dB in thestop-band, which requires a 4-th order filter at the cut-off frequency we're using. Change-Id: Ic00421d38d751641f86b1f3ad7663e6b44a91198 --- services/sensorservice/GravitySensor.cpp | 2 +- services/sensorservice/GravitySensor.h | 2 +- .../SecondOrderLowPassFilter.cpp | 19 +++++++++++++++++++ .../sensorservice/SecondOrderLowPassFilter.h | 12 ++++++++++++ 4 files changed, 33 insertions(+), 2 deletions(-) diff --git a/services/sensorservice/GravitySensor.cpp b/services/sensorservice/GravitySensor.cpp index 18bd359ea..da72f9c4b 100644 --- a/services/sensorservice/GravitySensor.cpp +++ b/services/sensorservice/GravitySensor.cpp @@ -30,7 +30,7 @@ namespace android { GravitySensor::GravitySensor(sensor_t const* list, size_t count) : mSensorDevice(SensorDevice::getInstance()), mEnabled(false), mAccTime(0), - mLowPass(M_SQRT1_2, 1), + mLowPass(M_SQRT1_2, 1.5f), mX(mLowPass), mY(mLowPass), mZ(mLowPass) { diff --git a/services/sensorservice/GravitySensor.h b/services/sensorservice/GravitySensor.h index f9850b75f..ff3bea77e 100644 --- a/services/sensorservice/GravitySensor.h +++ b/services/sensorservice/GravitySensor.h @@ -37,7 +37,7 @@ class GravitySensor : public SensorInterface { double mAccTime; SecondOrderLowPassFilter mLowPass; - BiquadFilter mX, mY, mZ; + CascadedBiquadFilter mX, mY, mZ; public: GravitySensor(sensor_t const* list, size_t count); diff --git a/services/sensorservice/SecondOrderLowPassFilter.cpp b/services/sensorservice/SecondOrderLowPassFilter.cpp index e13e1362d..eeb6d1e84 100644 --- a/services/sensorservice/SecondOrderLowPassFilter.cpp +++ b/services/sensorservice/SecondOrderLowPassFilter.cpp @@ -66,5 +66,24 @@ float BiquadFilter::operator()(float x) return y; } +// --------------------------------------------------------------------------- + +CascadedBiquadFilter::CascadedBiquadFilter(const SecondOrderLowPassFilter& s) + : mA(s), mB(s) +{ +} + +float CascadedBiquadFilter::init(float x) +{ + mA.init(x); + mB.init(x); + return x; +} + +float CascadedBiquadFilter::operator()(float x) +{ + return mB(mA(x)); +} + // --------------------------------------------------------------------------- }; // namespace android diff --git a/services/sensorservice/SecondOrderLowPassFilter.h b/services/sensorservice/SecondOrderLowPassFilter.h index 998ca35b9..85698ca63 100644 --- a/services/sensorservice/SecondOrderLowPassFilter.h +++ b/services/sensorservice/SecondOrderLowPassFilter.h @@ -54,6 +54,18 @@ public: float operator()(float in); }; +/* + * Two cascaded biquad IIR filters + * (4-poles IIR) + */ +class CascadedBiquadFilter { + BiquadFilter mA; + BiquadFilter mB; +public: + CascadedBiquadFilter(const SecondOrderLowPassFilter& s); + float init(float in); + float operator()(float in); +}; // --------------------------------------------------------------------------- }; // namespace android From 50b66767f6c5635430483393e17d15969dfe2f05 Mon Sep 17 00:00:00 2001 From: Mathias Agopian Date: Mon, 29 Nov 2010 17:26:51 -0800 Subject: [PATCH 18/51] fix [3237242] sensormanager sensor active count gets out of sync whether a physical sensor needed to be active or not was managed by a simpe reference counter; unfortunatelly nothing prevented it to get out of sync if a sensor was disabled more than once. sensorservice already maintainted a list of all the "clients" connected to a physical sensor; we now use that list to determine if a sensor should be enabled. This can never be "out-of-sync" since this is the only data structure linking a sensor to a user of that sensor. also removed the isEnabled() method, which was never used and implemented wrongly (since it didn't take into account that a sensor could be disabled for a client but not of another). Change-Id: I789affb877728ca957e99f7ba749def37c4db1c7 --- services/sensorservice/GravitySensor.cpp | 8 +------ services/sensorservice/GravitySensor.h | 2 -- .../LinearAccelerationSensor.cpp | 4 ---- .../sensorservice/LinearAccelerationSensor.h | 1 - .../sensorservice/RotationVectorSensor.cpp | 18 +++++---------- services/sensorservice/RotationVectorSensor.h | 2 -- services/sensorservice/SensorDevice.cpp | 22 ++++++++++--------- services/sensorservice/SensorDevice.h | 3 +-- services/sensorservice/SensorInterface.cpp | 13 +++-------- services/sensorservice/SensorInterface.h | 3 --- 10 files changed, 22 insertions(+), 54 deletions(-) diff --git a/services/sensorservice/GravitySensor.cpp b/services/sensorservice/GravitySensor.cpp index da72f9c4b..5c6aa99e9 100644 --- a/services/sensorservice/GravitySensor.cpp +++ b/services/sensorservice/GravitySensor.cpp @@ -29,7 +29,7 @@ namespace android { GravitySensor::GravitySensor(sensor_t const* list, size_t count) : mSensorDevice(SensorDevice::getInstance()), - mEnabled(false), mAccTime(0), + mAccTime(0), mLowPass(M_SQRT1_2, 1.5f), mX(mLowPass), mY(mLowPass), mZ(mLowPass) @@ -71,15 +71,9 @@ bool GravitySensor::process(sensors_event_t* outEvent, } return false; } - -bool GravitySensor::isEnabled() const { - return mEnabled; -} - status_t GravitySensor::activate(void* ident, bool enabled) { status_t err = mSensorDevice.activate(this, mAccelerometer.getHandle(), enabled); if (err == NO_ERROR) { - mEnabled = enabled; if (enabled) { mAccTime = 0; } diff --git a/services/sensorservice/GravitySensor.h b/services/sensorservice/GravitySensor.h index ff3bea77e..decfbb891 100644 --- a/services/sensorservice/GravitySensor.h +++ b/services/sensorservice/GravitySensor.h @@ -33,7 +33,6 @@ namespace android { class GravitySensor : public SensorInterface { SensorDevice& mSensorDevice; Sensor mAccelerometer; - bool mEnabled; double mAccTime; SecondOrderLowPassFilter mLowPass; @@ -43,7 +42,6 @@ public: GravitySensor(sensor_t const* list, size_t count); virtual bool process(sensors_event_t* outEvent, const sensors_event_t& event); - virtual bool isEnabled() const; virtual status_t activate(void* ident, bool enabled); virtual status_t setDelay(void* ident, int handle, int64_t ns); virtual Sensor getSensor() const; diff --git a/services/sensorservice/LinearAccelerationSensor.cpp b/services/sensorservice/LinearAccelerationSensor.cpp index 2dc12dc50..9425a9209 100644 --- a/services/sensorservice/LinearAccelerationSensor.cpp +++ b/services/sensorservice/LinearAccelerationSensor.cpp @@ -53,10 +53,6 @@ bool LinearAccelerationSensor::process(sensors_event_t* outEvent, return result; } -bool LinearAccelerationSensor::isEnabled() const { - return mGravitySensor.isEnabled(); -} - status_t LinearAccelerationSensor::activate(void* ident, bool enabled) { return mGravitySensor.activate(ident, enabled); } diff --git a/services/sensorservice/LinearAccelerationSensor.h b/services/sensorservice/LinearAccelerationSensor.h index ee918ce1c..c577086ab 100644 --- a/services/sensorservice/LinearAccelerationSensor.h +++ b/services/sensorservice/LinearAccelerationSensor.h @@ -40,7 +40,6 @@ class LinearAccelerationSensor : public SensorInterface { const sensors_event_t& event); public: LinearAccelerationSensor(sensor_t const* list, size_t count); - virtual bool isEnabled() const; virtual status_t activate(void* ident, bool enabled); virtual status_t setDelay(void* ident, int handle, int64_t ns); virtual Sensor getSensor() const; diff --git a/services/sensorservice/RotationVectorSensor.cpp b/services/sensorservice/RotationVectorSensor.cpp index 50cd6beac..418e7f8d0 100644 --- a/services/sensorservice/RotationVectorSensor.cpp +++ b/services/sensorservice/RotationVectorSensor.cpp @@ -34,7 +34,6 @@ static inline T clamp(T v) { RotationVectorSensor::RotationVectorSensor(sensor_t const* list, size_t count) : mSensorDevice(SensorDevice::getInstance()), - mEnabled(false), mALowPass(M_SQRT1_2, 5.0f), mAX(mALowPass), mAY(mALowPass), mAZ(mALowPass), mMLowPass(M_SQRT1_2, 2.5f), @@ -133,19 +132,12 @@ bool RotationVectorSensor::process(sensors_event_t* outEvent, return false; } -bool RotationVectorSensor::isEnabled() const { - return mEnabled; -} - status_t RotationVectorSensor::activate(void* ident, bool enabled) { - if (mEnabled != enabled) { - mSensorDevice.activate(this, mAcc.getHandle(), enabled); - mSensorDevice.activate(this, mMag.getHandle(), enabled); - mEnabled = enabled; - if (enabled) { - mMagTime = 0; - mAccTime = 0; - } + mSensorDevice.activate(this, mAcc.getHandle(), enabled); + mSensorDevice.activate(this, mMag.getHandle(), enabled); + if (enabled) { + mMagTime = 0; + mAccTime = 0; } return NO_ERROR; } diff --git a/services/sensorservice/RotationVectorSensor.h b/services/sensorservice/RotationVectorSensor.h index e7f28c93a..b7c951274 100644 --- a/services/sensorservice/RotationVectorSensor.h +++ b/services/sensorservice/RotationVectorSensor.h @@ -34,7 +34,6 @@ class RotationVectorSensor : public SensorInterface { SensorDevice& mSensorDevice; Sensor mAcc; Sensor mMag; - bool mEnabled; float mMagData[3]; double mAccTime; double mMagTime; @@ -47,7 +46,6 @@ public: RotationVectorSensor(sensor_t const* list, size_t count); virtual bool process(sensors_event_t* outEvent, const sensors_event_t& event); - virtual bool isEnabled() const; virtual status_t activate(void* ident, bool enabled); virtual status_t setDelay(void* ident, int handle, int64_t ns); virtual Sensor getSensor() const; diff --git a/services/sensorservice/SensorDevice.cpp b/services/sensorservice/SensorDevice.cpp index 73f85ba35..f192913fd 100644 --- a/services/sensorservice/SensorDevice.cpp +++ b/services/sensorservice/SensorDevice.cpp @@ -137,9 +137,8 @@ void SensorDevice::dump(String8& result, char* buffer, size_t SIZE) Mutex::Autolock _l(mLock); for (size_t i=0 ; i= 0) { + if (info.rates.size() == 0) { + actuateHardware = true; + } + } else { + // sensor wasn't enabled for this ident + } } + if (actuateHardware) { err = mSensorDevice->activate(mSensorDevice, handle, enabled); if (enabled) { diff --git a/services/sensorservice/SensorDevice.h b/services/sensorservice/SensorDevice.h index 63ecbcdde..c19b2ced4 100644 --- a/services/sensorservice/SensorDevice.h +++ b/services/sensorservice/SensorDevice.h @@ -40,8 +40,7 @@ class SensorDevice : public Singleton { Mutex mLock; // protect mActivationCount[].rates // fixed-size array after construction struct Info { - Info() : count(0) { } - int32_t count; + Info() { } KeyedVector rates; }; DefaultKeyedVector mActivationCount; diff --git a/services/sensorservice/SensorInterface.cpp b/services/sensorservice/SensorInterface.cpp index 93d23d9a4..be8eaff73 100644 --- a/services/sensorservice/SensorInterface.cpp +++ b/services/sensorservice/SensorInterface.cpp @@ -32,7 +32,7 @@ SensorInterface::~SensorInterface() HardwareSensor::HardwareSensor(const sensor_t& sensor) : mSensorDevice(SensorDevice::getInstance()), - mSensor(&sensor), mEnabled(false) + mSensor(&sensor) { LOGI("%s", sensor.name); } @@ -46,15 +46,8 @@ bool HardwareSensor::process(sensors_event_t* outEvent, return true; } -bool HardwareSensor::isEnabled() const { - return mEnabled; -} - -status_t HardwareSensor::activate(void* ident,bool enabled) { - status_t err = mSensorDevice.activate(ident, mSensor.getHandle(), enabled); - if (err == NO_ERROR) - mEnabled = enabled; - return err; +status_t HardwareSensor::activate(void* ident, bool enabled) { + return mSensorDevice.activate(ident, mSensor.getHandle(), enabled); } status_t HardwareSensor::setDelay(void* ident, int handle, int64_t ns) { diff --git a/services/sensorservice/SensorInterface.h b/services/sensorservice/SensorInterface.h index eebd5638f..084f2f5cc 100644 --- a/services/sensorservice/SensorInterface.h +++ b/services/sensorservice/SensorInterface.h @@ -38,7 +38,6 @@ public: virtual bool process(sensors_event_t* outEvent, const sensors_event_t& event) = 0; - virtual bool isEnabled() const = 0; virtual status_t activate(void* ident, bool enabled) = 0; virtual status_t setDelay(void* ident, int handle, int64_t ns) = 0; virtual Sensor getSensor() const = 0; @@ -51,7 +50,6 @@ class HardwareSensor : public SensorInterface { SensorDevice& mSensorDevice; Sensor mSensor; - bool mEnabled; public: HardwareSensor(const sensor_t& sensor); @@ -61,7 +59,6 @@ public: virtual bool process(sensors_event_t* outEvent, const sensors_event_t& event); - virtual bool isEnabled() const; virtual status_t activate(void* ident, bool enabled); virtual status_t setDelay(void* ident, int handle, int64_t ns); virtual Sensor getSensor() const; From 2cf098846c83bab82272f107ba110f7c47df1763 Mon Sep 17 00:00:00 2001 From: Mathias Agopian Date: Wed, 19 Jan 2011 14:26:25 -0800 Subject: [PATCH 19/51] fix [3369027] Sensor.TYPE_ROTATION_VECTOR is unstable and returns NaNs when running slowly The cut-off frequency of the lowpass filter was too high for the sampling rate used by DELAY_NORMAL. Now we use the same filters used for the gravity vector (cascaded biquad at 1.5 Hz) Change-Id: I319dc4f449a3abd553d61b196a9ddcf7782f912d --- services/sensorservice/RotationVectorSensor.cpp | 4 ++-- services/sensorservice/RotationVectorSensor.h | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/services/sensorservice/RotationVectorSensor.cpp b/services/sensorservice/RotationVectorSensor.cpp index 418e7f8d0..3abfc12bc 100644 --- a/services/sensorservice/RotationVectorSensor.cpp +++ b/services/sensorservice/RotationVectorSensor.cpp @@ -34,9 +34,9 @@ static inline T clamp(T v) { RotationVectorSensor::RotationVectorSensor(sensor_t const* list, size_t count) : mSensorDevice(SensorDevice::getInstance()), - mALowPass(M_SQRT1_2, 5.0f), + mALowPass(M_SQRT1_2, 1.5f), mAX(mALowPass), mAY(mALowPass), mAZ(mALowPass), - mMLowPass(M_SQRT1_2, 2.5f), + mMLowPass(M_SQRT1_2, 1.5f), mMX(mMLowPass), mMY(mMLowPass), mMZ(mMLowPass) { for (size_t i=0 ; i Date: Thu, 3 Feb 2011 14:52:47 -0800 Subject: [PATCH 20/51] fix [3421350] Killing a game that uses the accelerometer renders the device unable to sleep when an app dies, make sure to disable all sensors that process is connected to, regardless of wether this was the LAST connection to this sensor. Change-Id: I9c72b1792eee03815304674d5c2f25b5270e4748 --- services/sensorservice/SensorService.cpp | 13 ++++++++----- services/sensorservice/SensorService.h | 2 +- 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp index ea5e5ccd8..697e87920 100644 --- a/services/sensorservice/SensorService.cpp +++ b/services/sensorservice/SensorService.cpp @@ -293,18 +293,21 @@ sp SensorService::createSensorEventConnection() return result; } -void SensorService::cleanupConnection(const wp& connection) +void SensorService::cleanupConnection(SensorEventConnection* c) { Mutex::Autolock _l(mLock); + const wp connection(c); size_t size = mActiveSensors.size(); for (size_t i=0 ; iremoveConnection(connection)) { - int handle = mActiveSensors.keyAt(i); + int handle = mActiveSensors.keyAt(i); + if (c->hasSensor(handle)) { SensorInterface* sensor = mSensorMap.valueFor( handle ); if (sensor) { - sensor->activate(connection.unsafe_get(), false); + sensor->activate(c, false); } + } + SensorRecord* rec = mActiveSensors.valueAt(i); + if (rec && rec->removeConnection(connection)) { mActiveSensors.removeItemsAt(i, 1); mActiveVirtualSensors.removeItem(handle); delete rec; diff --git a/services/sensorservice/SensorService.h b/services/sensorservice/SensorService.h index 540c7e2a5..21f12bdbc 100644 --- a/services/sensorservice/SensorService.h +++ b/services/sensorservice/SensorService.h @@ -129,7 +129,7 @@ class SensorService : public: static char const* getServiceName() { return "sensorservice"; } - void cleanupConnection(const wp& connection); + void cleanupConnection(SensorEventConnection* connection); status_t enable(const sp& connection, int handle); status_t disable(const sp& connection, int handle); status_t setEventRate(const sp& connection, int handle, nsecs_t ns); From 3f2f8916112126fb1a93e4a4b4e2d4e01570aeee Mon Sep 17 00:00:00 2001 From: Mathias Agopian Date: Thu, 10 Mar 2011 15:23:28 -0800 Subject: [PATCH 21/51] fix [4025681] continuous sensors should not try to send an event as soon as they're activated Make sure to send an event down only for sensors that report a value only on data change. Other sensors, will naturally send an event when the next event is available. Bug: 4025681 Change-Id: I6d444deda388b6bc9a33e3371e09d390f1566ec5 --- services/sensorservice/SensorService.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp index 697e87920..ce1ab3df2 100644 --- a/services/sensorservice/SensorService.cpp +++ b/services/sensorservice/SensorService.cpp @@ -340,11 +340,14 @@ status_t SensorService::enable(const sp& connection, if (rec->addConnection(connection)) { // this sensor is already activated, but we are adding a // connection that uses it. Immediately send down the last - // known value of the requested sensor. - sensors_event_t scratch; - sensors_event_t& event(mLastEventSeen.editValueFor(handle)); - if (event.version == sizeof(sensors_event_t)) { - connection->sendEvents(&event, 1); + // known value of the requested sensor if it's not a + // "continuous" sensor. + if (sensor->getSensor().getMinDelay() == 0) { + sensors_event_t scratch; + sensors_event_t& event(mLastEventSeen.editValueFor(handle)); + if (event.version == sizeof(sensors_event_t)) { + connection->sendEvents(&event, 1); + } } } } From b1c14906ba87420d3a02e311ea65df1f03f7a3f9 Mon Sep 17 00:00:00 2001 From: Iliyan Malchev Date: Mon, 14 Mar 2011 14:02:13 -0700 Subject: [PATCH 22/51] frameworks/base: remove LOCAL_PRELINK_MODULE Change-Id: I54dd62ebef47e7690afa5a858f3cad941b135481 Signed-off-by: Iliyan Malchev --- services/sensorservice/Android.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/services/sensorservice/Android.mk b/services/sensorservice/Android.mk index 7e17fdd58..c50e4a16b 100644 --- a/services/sensorservice/Android.mk +++ b/services/sensorservice/Android.mk @@ -27,7 +27,7 @@ LOCAL_SHARED_LIBRARIES := \ libui \ libgui -LOCAL_PRELINK_MODULE := false + LOCAL_MODULE:= libsensorservice From e04a63b3053270d64890f156869e7cf75c436fbb Mon Sep 17 00:00:00 2001 From: Mathias Agopian Date: Thu, 19 May 2011 16:21:32 -0700 Subject: [PATCH 23/51] Fix a crasher in SensorService SensorService main thread wasn't java-enabled. however, in some situations we end-up calling into the BatteryService from that thread which causes a crash. Change-Id: Iffba90e4c4b743dba84d62f1342001a9db31916d --- services/sensorservice/SensorService.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp index ce1ab3df2..c5e69ff61 100644 --- a/services/sensorservice/SensorService.cpp +++ b/services/sensorservice/SensorService.cpp @@ -44,8 +44,7 @@ namespace android { // --------------------------------------------------------------------------- SensorService::SensorService() - : Thread(false), - mDump("android.permission.DUMP"), + : mDump("android.permission.DUMP"), mInitCheck(NO_INIT) { } From a1b7db95b6ccf5be9d8dfaac1b8f45494813edc0 Mon Sep 17 00:00:00 2001 From: Mathias Agopian Date: Fri, 27 May 2011 16:23:58 -0700 Subject: [PATCH 24/51] Fix a few issues with sensors reference-counting --- services/sensorservice/SensorDevice.cpp | 22 ++++++++++++++++++++-- services/sensorservice/SensorService.cpp | 10 ++++++++++ services/sensorservice/SensorService.h | 2 ++ 3 files changed, 32 insertions(+), 2 deletions(-) diff --git a/services/sensorservice/SensorDevice.cpp b/services/sensorservice/SensorDevice.cpp index f192913fd..b3c8ef59e 100644 --- a/services/sensorservice/SensorDevice.cpp +++ b/services/sensorservice/SensorDevice.cpp @@ -29,6 +29,7 @@ #include #include "SensorDevice.h" +#include "SensorService.h" namespace android { // --------------------------------------------------------------------------- @@ -166,17 +167,32 @@ status_t SensorDevice::activate(void* ident, int handle, int enabled) bool actuateHardware = false; Info& info( mActivationCount.editValueFor(handle) ); + + + LOGD_IF(DEBUG_CONNECTIONS, + "SensorDevice::activate: ident=%p, handle=0x%08x, enabled=%d, count=%d", + ident, handle, enabled, info.rates.size()); + if (enabled) { Mutex::Autolock _l(mLock); + LOGD_IF(DEBUG_CONNECTIONS, "... index=%ld", + info.rates.indexOfKey(ident)); + if (info.rates.indexOfKey(ident) < 0) { info.rates.add(ident, DEFAULT_EVENTS_PERIOD); - actuateHardware = true; + if (info.rates.size() == 1) { + actuateHardware = true; + } } else { // sensor was already activated for this ident } } else { Mutex::Autolock _l(mLock); - if (info.rates.removeItem(ident) >= 0) { + LOGD_IF(DEBUG_CONNECTIONS, "... index=%ld", + info.rates.indexOfKey(ident)); + + ssize_t idx = info.rates.removeItem(ident); + if (idx >= 0) { if (info.rates.size() == 0) { actuateHardware = true; } @@ -186,6 +202,8 @@ status_t SensorDevice::activate(void* ident, int handle, int enabled) } if (actuateHardware) { + LOGD_IF(DEBUG_CONNECTIONS, "\t>>> actuating h/w"); + err = mSensorDevice->activate(mSensorDevice, handle, enabled); if (enabled) { LOGE_IF(err, "Error activating sensor %d (%s)", handle, strerror(-err)); diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp index c5e69ff61..f1db2f555 100644 --- a/services/sensorservice/SensorService.cpp +++ b/services/sensorservice/SensorService.cpp @@ -297,16 +297,25 @@ void SensorService::cleanupConnection(SensorEventConnection* c) Mutex::Autolock _l(mLock); const wp connection(c); size_t size = mActiveSensors.size(); + LOGD_IF(DEBUG_CONNECTIONS, "%d active sensors", size); for (size_t i=0 ; ihasSensor(handle)) { + LOGD_IF(DEBUG_CONNECTIONS, "%i: disabling handle=0x%08x", i, handle); SensorInterface* sensor = mSensorMap.valueFor( handle ); + LOGE_IF(!sensor, "mSensorMap[handle=0x%08x] is null!", handle); if (sensor) { sensor->activate(c, false); } } SensorRecord* rec = mActiveSensors.valueAt(i); + LOGE_IF(!rec, "mActiveSensors[%d] is null (handle=0x%08x)!", i, handle); + LOGD_IF(DEBUG_CONNECTIONS, + "removing connection %p for sensor[%d].handle=0x%08x", + c, i, handle); + if (rec && rec->removeConnection(connection)) { + LOGD_IF(DEBUG_CONNECTIONS, "... and it was the last connection"); mActiveSensors.removeItemsAt(i, 1); mActiveVirtualSensors.removeItem(handle); delete rec; @@ -446,6 +455,7 @@ SensorService::SensorEventConnection::SensorEventConnection( SensorService::SensorEventConnection::~SensorEventConnection() { + LOGD_IF(DEBUG_CONNECTIONS, "~SensorEventConnection(%p)", this); mService->cleanupConnection(this); } diff --git a/services/sensorservice/SensorService.h b/services/sensorservice/SensorService.h index 21f12bdbc..77a7e34e8 100644 --- a/services/sensorservice/SensorService.h +++ b/services/sensorservice/SensorService.h @@ -38,6 +38,8 @@ // --------------------------------------------------------------------------- +#define DEBUG_CONNECTIONS false + struct sensors_poll_device_t; struct sensors_module_t; From 984826cc158193e61e3a00359ef4f6699c7d748a Mon Sep 17 00:00:00 2001 From: Mathias Agopian Date: Tue, 17 May 2011 22:54:42 -0700 Subject: [PATCH 25/51] 9-axis sensor fusion with Kalman filter Add support for 9-axis gravity and linear-acceleration sensors virtual orientation sensor using 9-axis fusion Change-Id: I6717539373fce781c10e97b6fa59f68a831a592f --- services/sensorservice/Android.mk | 17 +- .../sensorservice/CorrectedGyroSensor.cpp | 86 ++++ services/sensorservice/CorrectedGyroSensor.h | 52 +++ services/sensorservice/Fusion.cpp | 431 ++++++++++++++++++ services/sensorservice/Fusion.h | 86 ++++ services/sensorservice/GravitySensor.cpp | 75 ++- services/sensorservice/GravitySensor.h | 7 +- .../LinearAccelerationSensor.cpp | 25 +- .../sensorservice/LinearAccelerationSensor.h | 6 +- services/sensorservice/OrientationSensor.cpp | 89 ++++ services/sensorservice/OrientationSensor.h | 51 +++ .../sensorservice/RotationVectorSensor.cpp | 137 ++---- services/sensorservice/RotationVectorSensor.h | 15 +- .../SecondOrderLowPassFilter.cpp | 28 +- .../sensorservice/SecondOrderLowPassFilter.h | 20 +- services/sensorservice/SensorDevice.cpp | 3 + services/sensorservice/SensorFusion.cpp | 180 ++++++++ services/sensorservice/SensorFusion.h | 84 ++++ services/sensorservice/SensorInterface.h | 2 - services/sensorservice/SensorService.cpp | 46 +- services/sensorservice/mat.h | 370 +++++++++++++++ services/sensorservice/traits.h | 118 +++++ services/sensorservice/vec.h | 420 +++++++++++++++++ 23 files changed, 2158 insertions(+), 190 deletions(-) create mode 100644 services/sensorservice/CorrectedGyroSensor.cpp create mode 100644 services/sensorservice/CorrectedGyroSensor.h create mode 100644 services/sensorservice/Fusion.cpp create mode 100644 services/sensorservice/Fusion.h create mode 100644 services/sensorservice/OrientationSensor.cpp create mode 100644 services/sensorservice/OrientationSensor.h create mode 100644 services/sensorservice/SensorFusion.cpp create mode 100644 services/sensorservice/SensorFusion.h create mode 100644 services/sensorservice/mat.h create mode 100644 services/sensorservice/traits.h create mode 100644 services/sensorservice/vec.h diff --git a/services/sensorservice/Android.mk b/services/sensorservice/Android.mk index c50e4a16b..57a3b1569 100644 --- a/services/sensorservice/Android.mk +++ b/services/sensorservice/Android.mk @@ -2,13 +2,18 @@ LOCAL_PATH:= $(call my-dir) include $(CLEAR_VARS) LOCAL_SRC_FILES:= \ - GravitySensor.cpp \ - LinearAccelerationSensor.cpp \ - RotationVectorSensor.cpp \ - SensorService.cpp \ - SensorInterface.cpp \ + CorrectedGyroSensor.cpp \ + Fusion.cpp \ + GravitySensor.cpp \ + LinearAccelerationSensor.cpp \ + OrientationSensor.cpp \ + RotationVectorSensor.cpp \ + SecondOrderLowPassFilter.cpp \ SensorDevice.cpp \ - SecondOrderLowPassFilter.cpp + SensorFusion.cpp \ + SensorInterface.cpp \ + SensorService.cpp \ + LOCAL_CFLAGS:= -DLOG_TAG=\"SensorService\" diff --git a/services/sensorservice/CorrectedGyroSensor.cpp b/services/sensorservice/CorrectedGyroSensor.cpp new file mode 100644 index 000000000..9b75b7024 --- /dev/null +++ b/services/sensorservice/CorrectedGyroSensor.cpp @@ -0,0 +1,86 @@ +/* + * Copyright (C) 2011 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include + +#include + +#include + +#include "CorrectedGyroSensor.h" +#include "SensorDevice.h" +#include "SensorFusion.h" + +namespace android { +// --------------------------------------------------------------------------- + +CorrectedGyroSensor::CorrectedGyroSensor(sensor_t const* list, size_t count) + : mSensorDevice(SensorDevice::getInstance()), + mSensorFusion(SensorFusion::getInstance()) +{ + for (size_t i=0 ; idata[0] -= bias.x; + outEvent->data[1] -= bias.y; + outEvent->data[2] -= bias.z; + outEvent->sensor = '_cgy'; + return true; + } + return false; +} + +status_t CorrectedGyroSensor::activate(void* ident, bool enabled) { + mSensorDevice.activate(this, mGyro.getHandle(), enabled); + return mSensorFusion.activate(this, enabled); +} + +status_t CorrectedGyroSensor::setDelay(void* ident, int handle, int64_t ns) { + mSensorDevice.setDelay(this, mGyro.getHandle(), ns); + return mSensorFusion.setDelay(this, ns); +} + +Sensor CorrectedGyroSensor::getSensor() const { + sensor_t hwSensor; + hwSensor.name = "Corrected Gyroscope Sensor"; + hwSensor.vendor = "Google Inc."; + hwSensor.version = 1; + hwSensor.handle = '_cgy'; + hwSensor.type = SENSOR_TYPE_GYROSCOPE; + hwSensor.maxRange = mGyro.getMaxValue(); + hwSensor.resolution = mGyro.getResolution(); + hwSensor.power = mSensorFusion.getPowerUsage(); + hwSensor.minDelay = mGyro.getMinDelay(); + Sensor sensor(&hwSensor); + return sensor; +} + +// --------------------------------------------------------------------------- +}; // namespace android + diff --git a/services/sensorservice/CorrectedGyroSensor.h b/services/sensorservice/CorrectedGyroSensor.h new file mode 100644 index 000000000..3c49c0801 --- /dev/null +++ b/services/sensorservice/CorrectedGyroSensor.h @@ -0,0 +1,52 @@ +/* + * Copyright (C) 2011 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ANDROID_CORRECTED_GYRO_SENSOR_H +#define ANDROID_CORRECTED_GYRO_SENSOR_H + +#include +#include + +#include + +#include "SensorInterface.h" + +// --------------------------------------------------------------------------- +namespace android { +// --------------------------------------------------------------------------- + +class SensorDevice; +class SensorFusion; + +class CorrectedGyroSensor : public SensorInterface { + SensorDevice& mSensorDevice; + SensorFusion& mSensorFusion; + Sensor mGyro; + +public: + CorrectedGyroSensor(sensor_t const* list, size_t count); + virtual bool process(sensors_event_t* outEvent, + const sensors_event_t& event); + virtual status_t activate(void* ident, bool enabled); + virtual status_t setDelay(void* ident, int handle, int64_t ns); + virtual Sensor getSensor() const; + virtual bool isVirtual() const { return true; } +}; + +// --------------------------------------------------------------------------- +}; // namespace android + +#endif // ANDROID_CORRECTED_GYRO_SENSOR_H diff --git a/services/sensorservice/Fusion.cpp b/services/sensorservice/Fusion.cpp new file mode 100644 index 000000000..56ac9f931 --- /dev/null +++ b/services/sensorservice/Fusion.cpp @@ -0,0 +1,431 @@ +/* + * Copyright (C) 2011 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include + +#include + +#include "Fusion.h" + +namespace android { + +// ----------------------------------------------------------------------- + +template +static inline TYPE sqr(TYPE x) { + return x*x; +} + +template +static inline T clamp(T v) { + return v < 0 ? 0 : v; +} + +template +static mat scaleCovariance( + const mat& A, + const mat& P) { + // A*P*transpose(A); + mat APAt; + for (size_t r=0 ; r +static mat crossMatrix(const vec& p, OTHER_TYPE diag) { + mat r; + r[0][0] = diag; + r[1][1] = diag; + r[2][2] = diag; + r[0][1] = p.z; + r[1][0] =-p.z; + r[0][2] =-p.y; + r[2][0] = p.y; + r[1][2] = p.x; + r[2][1] =-p.x; + return r; +} + +template +static mat MRPsToMatrix(const vec& p) { + mat res(1); + const mat px(crossMatrix(p, 0)); + const TYPE ptp(dot_product(p,p)); + const TYPE t = 4/sqr(1+ptp); + res -= t * (1-ptp) * px; + res += t * 2 * sqr(px); + return res; +} + +template +vec matrixToMRPs(const mat& R) { + // matrix to MRPs + vec q; + const float Hx = R[0].x; + const float My = R[1].y; + const float Az = R[2].z; + const float w = 1 / (1 + sqrtf( clamp( Hx + My + Az + 1) * 0.25f )); + q.x = sqrtf( clamp( Hx - My - Az + 1) * 0.25f ) * w; + q.y = sqrtf( clamp(-Hx + My - Az + 1) * 0.25f ) * w; + q.z = sqrtf( clamp(-Hx - My + Az + 1) * 0.25f ) * w; + q.x = copysignf(q.x, R[2].y - R[1].z); + q.y = copysignf(q.y, R[0].z - R[2].x); + q.z = copysignf(q.z, R[1].x - R[0].y); + return q; +} + +template +class Covariance { + mat mSumXX; + vec mSumX; + size_t mN; +public: + Covariance() : mSumXX(0.0f), mSumX(0.0f), mN(0) { } + void update(const vec& x) { + mSumXX += x*transpose(x); + mSumX += x; + mN++; + } + mat operator()() const { + const float N = 1.0f / mN; + return mSumXX*N - (mSumX*transpose(mSumX))*(N*N); + } + void reset() { + mN = 0; + mSumXX = 0; + mSumX = 0; + } + size_t getCount() const { + return mN; + } +}; + +// ----------------------------------------------------------------------- + +Fusion::Fusion() { + // process noise covariance matrix + const float w1 = gyroSTDEV; + const float w2 = biasSTDEV; + Q[0] = w1*w1; + Q[1] = w2*w2; + + Ba.x = 0; + Ba.y = 0; + Ba.z = 1; + + Bm.x = 0; + Bm.y = 1; + Bm.z = 0; + + init(); +} + +void Fusion::init() { + // initial estimate: E{ x(t0) } + x = 0; + + // initial covariance: Var{ x(t0) } + P = 0; + + mInitState = 0; + mCount[0] = 0; + mCount[1] = 0; + mCount[2] = 0; + mData = 0; +} + +bool Fusion::hasEstimate() const { + return (mInitState == (MAG|ACC|GYRO)); +} + +bool Fusion::checkInitComplete(int what, const vec3_t& d) { + if (mInitState == (MAG|ACC|GYRO)) + return true; + + if (what == ACC) { + mData[0] += d * (1/length(d)); + mCount[0]++; + mInitState |= ACC; + } else if (what == MAG) { + mData[1] += d * (1/length(d)); + mCount[1]++; + mInitState |= MAG; + } else if (what == GYRO) { + mData[2] += d; + mCount[2]++; + if (mCount[2] == 64) { + // 64 samples is good enough to estimate the gyro drift and + // doesn't take too much time. + mInitState |= GYRO; + } + } + + if (mInitState == (MAG|ACC|GYRO)) { + // Average all the values we collected so far + mData[0] *= 1.0f/mCount[0]; + mData[1] *= 1.0f/mCount[1]; + mData[2] *= 1.0f/mCount[2]; + + // calculate the MRPs from the data collection, this gives us + // a rough estimate of our initial state + mat33_t R; + vec3_t up(mData[0]); + vec3_t east(cross_product(mData[1], up)); + east *= 1/length(east); + vec3_t north(cross_product(up, east)); + R << east << north << up; + x[0] = matrixToMRPs(R); + + // NOTE: we could try to use the average of the gyro data + // to estimate the initial bias, but this only works if + // the device is not moving. For now, we don't use that value + // and start with a bias of 0. + x[1] = 0; + + // initial covariance + P = 0; + } + + return false; +} + +void Fusion::handleGyro(const vec3_t& w, float dT) { + const vec3_t wdT(w * dT); // rad/s * s -> rad + if (!checkInitComplete(GYRO, wdT)) + return; + + predict(wdT); +} + +status_t Fusion::handleAcc(const vec3_t& a) { + if (length(a) < 0.981f) + return BAD_VALUE; + + if (!checkInitComplete(ACC, a)) + return BAD_VALUE; + + // ignore acceleration data if we're close to free-fall + const float l = 1/length(a); + update(a*l, Ba, accSTDEV*l); + return NO_ERROR; +} + +status_t Fusion::handleMag(const vec3_t& m) { + // the geomagnetic-field should be between 30uT and 60uT + // reject obviously wrong magnetic-fields + if (length(m) > 100) + return BAD_VALUE; + + if (!checkInitComplete(MAG, m)) + return BAD_VALUE; + + const vec3_t up( getRotationMatrix() * Ba ); + const vec3_t east( cross_product(m, up) ); + vec3_t north( cross_product(up, east) ); + + const float l = 1 / length(north); + north *= l; + +#if 0 + // in practice the magnetic-field sensor is so wrong + // that there is no point trying to use it to constantly + // correct the gyro. instead, we use the mag-sensor only when + // the device points north (just to give us a reference). + // We're hoping that it'll actually point north, if it doesn't + // we'll be offset, but at least the instantaneous posture + // of the device will be correct. + + const float cos_30 = 0.8660254f; + if (dot_product(north, Bm) < cos_30) + return BAD_VALUE; +#endif + + update(north, Bm, magSTDEV*l); + return NO_ERROR; +} + +bool Fusion::checkState(const vec3_t& v) { + if (isnanf(length(v))) { + LOGW("9-axis fusion diverged. reseting state."); + P = 0; + x[1] = 0; + mInitState = 0; + mCount[0] = 0; + mCount[1] = 0; + mCount[2] = 0; + mData = 0; + return false; + } + return true; +} + +vec3_t Fusion::getAttitude() const { + return x[0]; +} + +vec3_t Fusion::getBias() const { + return x[1]; +} + +mat33_t Fusion::getRotationMatrix() const { + return MRPsToMatrix(x[0]); +} + +mat33_t Fusion::getF(const vec3_t& p) { + const float p0 = p.x; + const float p1 = p.y; + const float p2 = p.z; + + // f(p, w) + const float p0p1 = p0*p1; + const float p0p2 = p0*p2; + const float p1p2 = p1*p2; + const float p0p0 = p0*p0; + const float p1p1 = p1*p1; + const float p2p2 = p2*p2; + const float pp = 0.5f * (1 - (p0p0 + p1p1 + p2p2)); + + mat33_t F; + F[0][0] = 0.5f*(p0p0 + pp); + F[0][1] = 0.5f*(p0p1 + p2); + F[0][2] = 0.5f*(p0p2 - p1); + F[1][0] = 0.5f*(p0p1 - p2); + F[1][1] = 0.5f*(p1p1 + pp); + F[1][2] = 0.5f*(p1p2 + p0); + F[2][0] = 0.5f*(p0p2 + p1); + F[2][1] = 0.5f*(p1p2 - p0); + F[2][2] = 0.5f*(p2p2 + pp); + return F; +} + +mat33_t Fusion::getdFdp(const vec3_t& p, const vec3_t& we) { + + // dF = | A = df/dp -F | + // | 0 0 | + + mat33_t A; + A[0][0] = A[1][1] = A[2][2] = 0.5f * (p.x*we.x + p.y*we.y + p.z*we.z); + A[0][1] = 0.5f * (p.y*we.x - p.x*we.y - we.z); + A[0][2] = 0.5f * (p.z*we.x - p.x*we.z + we.y); + A[1][2] = 0.5f * (p.z*we.y - p.y*we.z - we.x); + A[1][0] = -A[0][1]; + A[2][0] = -A[0][2]; + A[2][1] = -A[1][2]; + return A; +} + +void Fusion::predict(const vec3_t& w) { + // f(p, w) + vec3_t& p(x[0]); + + // There is a discontinuity at 2.pi, to avoid it we need to switch to + // the shadow of p when pT.p gets too big. + const float ptp(dot_product(p,p)); + if (ptp >= 2.0f) { + p = -p * (1/ptp); + } + + const mat33_t F(getF(p)); + + // compute w with the bias correction: + // w_estimated = w - b_estimated + const vec3_t& b(x[1]); + const vec3_t we(w - b); + + // prediction + const vec3_t dX(F*we); + + if (!checkState(dX)) + return; + + p += dX; + + const mat33_t A(getdFdp(p, we)); + + // G = | G0 0 | = | -F 0 | + // | 0 1 | | 0 1 | + + // P += A*P + P*At + F*Q*Ft + const mat33_t AP(A*transpose(P[0][0])); + const mat33_t PAt(P[0][0]*transpose(A)); + const mat33_t FPSt(F*transpose(P[1][0])); + const mat33_t PSFt(P[1][0]*transpose(F)); + const mat33_t FQFt(scaleCovariance(F, Q[0])); + P[0][0] += AP + PAt - FPSt - PSFt + FQFt; + P[1][0] += A*P[1][0] - F*P[1][1]; + P[1][1] += Q[1]; +} + +void Fusion::update(const vec3_t& z, const vec3_t& Bi, float sigma) { + const vec3_t p(x[0]); + // measured vector in body space: h(p) = A(p)*Bi + const mat33_t A(MRPsToMatrix(p)); + const vec3_t Bb(A*Bi); + + // Sensitivity matrix H = dh(p)/dp + // H = [ L 0 ] + const float ptp(dot_product(p,p)); + const mat33_t px(crossMatrix(p, 0.5f*(ptp-1))); + const mat33_t ppt(p*transpose(p)); + const mat33_t L((8 / sqr(1+ptp))*crossMatrix(Bb, 0)*(ppt-px)); + + // update... + const mat33_t R(sigma*sigma); + const mat33_t S(scaleCovariance(L, P[0][0]) + R); + const mat33_t Si(invert(S)); + const mat33_t LtSi(transpose(L)*Si); + + vec K; + K[0] = P[0][0] * LtSi; + K[1] = transpose(P[1][0])*LtSi; + + const vec3_t e(z - Bb); + const vec3_t K0e(K[0]*e); + const vec3_t K1e(K[1]*e); + + if (!checkState(K0e)) + return; + + if (!checkState(K1e)) + return; + + x[0] += K0e; + x[1] += K1e; + + // P -= K*H*P; + const mat33_t K0L(K[0] * L); + const mat33_t K1L(K[1] * L); + P[0][0] -= K0L*P[0][0]; + P[1][1] -= K1L*P[1][0]; + P[1][0] -= K0L*P[1][0]; +} + +// ----------------------------------------------------------------------- + +}; // namespace android + diff --git a/services/sensorservice/Fusion.h b/services/sensorservice/Fusion.h new file mode 100644 index 000000000..571a41527 --- /dev/null +++ b/services/sensorservice/Fusion.h @@ -0,0 +1,86 @@ +/* + * Copyright (C) 2011 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ANDROID_FUSION_H +#define ANDROID_FUSION_H + +#include + +#include "vec.h" +#include "mat.h" + +namespace android { + +class Fusion { + /* + * the state vector is made of two sub-vector containing respectively: + * - modified Rodrigues parameters + * - the estimated gyro bias + */ + vec x; + + /* + * the predicated covariance matrix is made of 4 3x3 sub-matrices and it + * semi-definite positive. + * + * P = | P00 P10 | = | P00 P10 | + * | P01 P11 | | P10t Q1 | + * + * Since P01 = transpose(P10), the code below never calculates or + * stores P01. P11 is always equal to Q1, so we don't store it either. + */ + mat P; + + /* + * the process noise covariance matrix is made of 2 3x3 sub-matrices + * Q0 encodes the attitude's noise + * Q1 encodes the bias' noise + */ + vec Q; + + static const float gyroSTDEV = 1.0e-5; // rad/s (measured 1.2e-5) + static const float accSTDEV = 0.05f; // m/s^2 (measured 0.08 / CDD 0.05) + static const float magSTDEV = 0.5f; // uT (measured 0.7 / CDD 0.5) + static const float biasSTDEV = 2e-9; // rad/s^2 (guessed) + +public: + Fusion(); + void init(); + void handleGyro(const vec3_t& w, float dT); + status_t handleAcc(const vec3_t& a); + status_t handleMag(const vec3_t& m); + vec3_t getAttitude() const; + vec3_t getBias() const; + mat33_t getRotationMatrix() const; + bool hasEstimate() const; + +private: + vec3_t Ba, Bm; + uint32_t mInitState; + vec mData; + size_t mCount[3]; + enum { ACC=0x1, MAG=0x2, GYRO=0x4 }; + bool checkInitComplete(int, const vec3_t&); + bool checkState(const vec3_t& v); + void predict(const vec3_t& w); + void update(const vec3_t& z, const vec3_t& Bi, float sigma); + static mat33_t getF(const vec3_t& p); + static mat33_t getdFdp(const vec3_t& p, const vec3_t& we); +}; + +}; // namespace android + +#endif // ANDROID_FUSION_H diff --git a/services/sensorservice/GravitySensor.cpp b/services/sensorservice/GravitySensor.cpp index 5c6aa99e9..541fad26f 100644 --- a/services/sensorservice/GravitySensor.cpp +++ b/services/sensorservice/GravitySensor.cpp @@ -23,16 +23,18 @@ #include #include "GravitySensor.h" +#include "SensorDevice.h" +#include "SensorFusion.h" namespace android { // --------------------------------------------------------------------------- GravitySensor::GravitySensor(sensor_t const* list, size_t count) : mSensorDevice(SensorDevice::getInstance()), + mSensorFusion(SensorFusion::getInstance()), mAccTime(0), mLowPass(M_SQRT1_2, 1.5f), mX(mLowPass), mY(mLowPass), mZ(mLowPass) - { for (size_t i=0 ; idata[0] = x; - outEvent->data[1] = y; - outEvent->data[2] = z; + outEvent->data[0] = g.x; + outEvent->data[1] = g.y; + outEvent->data[2] = g.z; outEvent->sensor = '_grv'; outEvent->type = SENSOR_TYPE_GRAVITY; return true; } return false; } + status_t GravitySensor::activate(void* ident, bool enabled) { - status_t err = mSensorDevice.activate(this, mAccelerometer.getHandle(), enabled); - if (err == NO_ERROR) { - if (enabled) { - mAccTime = 0; + status_t err; + if (mSensorFusion.hasGyro()) { + err = mSensorFusion.activate(this, enabled); + } else { + err = mSensorDevice.activate(this, mAccelerometer.getHandle(), enabled); + if (err == NO_ERROR) { + if (enabled) { + mAccTime = 0; + } } } return err; @@ -83,20 +102,26 @@ status_t GravitySensor::activate(void* ident, bool enabled) { status_t GravitySensor::setDelay(void* ident, int handle, int64_t ns) { - return mSensorDevice.setDelay(this, mAccelerometer.getHandle(), ns); + if (mSensorFusion.hasGyro()) { + return mSensorFusion.setDelay(this, ns); + } else { + return mSensorDevice.setDelay(this, mAccelerometer.getHandle(), ns); + } } Sensor GravitySensor::getSensor() const { sensor_t hwSensor; hwSensor.name = "Gravity Sensor"; hwSensor.vendor = "Google Inc."; - hwSensor.version = 1; + hwSensor.version = mSensorFusion.hasGyro() ? 3 : 2; hwSensor.handle = '_grv'; hwSensor.type = SENSOR_TYPE_GRAVITY; - hwSensor.maxRange = mAccelerometer.getMaxValue(); + hwSensor.maxRange = GRAVITY_EARTH * 2; hwSensor.resolution = mAccelerometer.getResolution(); - hwSensor.power = mAccelerometer.getPowerUsage(); - hwSensor.minDelay = mAccelerometer.getMinDelay(); + hwSensor.power = mSensorFusion.hasGyro() ? + mSensorFusion.getPowerUsage() : mAccelerometer.getPowerUsage(); + hwSensor.minDelay = mSensorFusion.hasGyro() ? + mSensorFusion.getMinDelay() : mAccelerometer.getMinDelay(); Sensor sensor(&hwSensor); return sensor; } diff --git a/services/sensorservice/GravitySensor.h b/services/sensorservice/GravitySensor.h index decfbb891..0ca3a3c0e 100644 --- a/services/sensorservice/GravitySensor.h +++ b/services/sensorservice/GravitySensor.h @@ -22,7 +22,6 @@ #include -#include "SensorDevice.h" #include "SensorInterface.h" #include "SecondOrderLowPassFilter.h" @@ -30,13 +29,17 @@ namespace android { // --------------------------------------------------------------------------- +class SensorDevice; +class SensorFusion; + class GravitySensor : public SensorInterface { SensorDevice& mSensorDevice; + SensorFusion& mSensorFusion; Sensor mAccelerometer; double mAccTime; SecondOrderLowPassFilter mLowPass; - CascadedBiquadFilter mX, mY, mZ; + CascadedBiquadFilter mX, mY, mZ; public: GravitySensor(sensor_t const* list, size_t count); diff --git a/services/sensorservice/LinearAccelerationSensor.cpp b/services/sensorservice/LinearAccelerationSensor.cpp index 9425a9209..f0054f2b3 100644 --- a/services/sensorservice/LinearAccelerationSensor.cpp +++ b/services/sensorservice/LinearAccelerationSensor.cpp @@ -23,6 +23,8 @@ #include #include "LinearAccelerationSensor.h" +#include "SensorDevice.h" +#include "SensorFusion.h" namespace android { // --------------------------------------------------------------------------- @@ -31,34 +33,29 @@ LinearAccelerationSensor::LinearAccelerationSensor(sensor_t const* list, size_t : mSensorDevice(SensorDevice::getInstance()), mGravitySensor(list, count) { - mData[0] = mData[1] = mData[2] = 0; } bool LinearAccelerationSensor::process(sensors_event_t* outEvent, const sensors_event_t& event) { bool result = mGravitySensor.process(outEvent, event); - if (result) { - if (event.type == SENSOR_TYPE_ACCELEROMETER) { - mData[0] = event.acceleration.x; - mData[1] = event.acceleration.y; - mData[2] = event.acceleration.z; - } - outEvent->data[0] = mData[0] - outEvent->data[0]; - outEvent->data[1] = mData[1] - outEvent->data[1]; - outEvent->data[2] = mData[2] - outEvent->data[2]; + if (result && event.type == SENSOR_TYPE_ACCELEROMETER) { + outEvent->data[0] = event.acceleration.x - outEvent->data[0]; + outEvent->data[1] = event.acceleration.y - outEvent->data[1]; + outEvent->data[2] = event.acceleration.z - outEvent->data[2]; outEvent->sensor = '_lin'; outEvent->type = SENSOR_TYPE_LINEAR_ACCELERATION; + return true; } - return result; + return false; } status_t LinearAccelerationSensor::activate(void* ident, bool enabled) { - return mGravitySensor.activate(ident, enabled); + return mGravitySensor.activate(this, enabled); } status_t LinearAccelerationSensor::setDelay(void* ident, int handle, int64_t ns) { - return mGravitySensor.setDelay(ident, handle, ns); + return mGravitySensor.setDelay(this, handle, ns); } Sensor LinearAccelerationSensor::getSensor() const { @@ -66,7 +63,7 @@ Sensor LinearAccelerationSensor::getSensor() const { sensor_t hwSensor; hwSensor.name = "Linear Acceleration Sensor"; hwSensor.vendor = "Google Inc."; - hwSensor.version = 1; + hwSensor.version = gsensor.getVersion(); hwSensor.handle = '_lin'; hwSensor.type = SENSOR_TYPE_LINEAR_ACCELERATION; hwSensor.maxRange = gsensor.getMaxValue(); diff --git a/services/sensorservice/LinearAccelerationSensor.h b/services/sensorservice/LinearAccelerationSensor.h index c577086ab..5deb24f37 100644 --- a/services/sensorservice/LinearAccelerationSensor.h +++ b/services/sensorservice/LinearAccelerationSensor.h @@ -22,19 +22,19 @@ #include -#include "SensorDevice.h" #include "SensorInterface.h" #include "GravitySensor.h" // --------------------------------------------------------------------------- - namespace android { // --------------------------------------------------------------------------- +class SensorDevice; +class SensorFusion; + class LinearAccelerationSensor : public SensorInterface { SensorDevice& mSensorDevice; GravitySensor mGravitySensor; - float mData[3]; virtual bool process(sensors_event_t* outEvent, const sensors_event_t& event); diff --git a/services/sensorservice/OrientationSensor.cpp b/services/sensorservice/OrientationSensor.cpp new file mode 100644 index 000000000..c9e50803f --- /dev/null +++ b/services/sensorservice/OrientationSensor.cpp @@ -0,0 +1,89 @@ +/* + * Copyright (C) 2011 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include + +#include + +#include + +#include "OrientationSensor.h" +#include "SensorDevice.h" +#include "SensorFusion.h" + +namespace android { +// --------------------------------------------------------------------------- + +OrientationSensor::OrientationSensor() + : mSensorDevice(SensorDevice::getInstance()), + mSensorFusion(SensorFusion::getInstance()) +{ +} + +bool OrientationSensor::process(sensors_event_t* outEvent, + const sensors_event_t& event) +{ + if (event.type == SENSOR_TYPE_ACCELEROMETER) { + if (mSensorFusion.hasEstimate()) { + vec3_t g; + const float rad2deg = 180 / M_PI; + const mat33_t R(mSensorFusion.getRotationMatrix()); + g[0] = atan2f(-R[1][0], R[0][0]) * rad2deg; + g[1] = atan2f(-R[2][1], R[2][2]) * rad2deg; + g[2] = asinf ( R[2][0]) * rad2deg; + if (g[0] < 0) + g[0] += 360; + + *outEvent = event; + outEvent->data[0] = g.x; + outEvent->data[1] = g.y; + outEvent->data[2] = g.z; + outEvent->sensor = '_ypr'; + outEvent->type = SENSOR_TYPE_ORIENTATION; + return true; + } + } + return false; +} + +status_t OrientationSensor::activate(void* ident, bool enabled) { + return mSensorFusion.activate(this, enabled); +} + +status_t OrientationSensor::setDelay(void* ident, int handle, int64_t ns) { + return mSensorFusion.setDelay(this, ns); +} + +Sensor OrientationSensor::getSensor() const { + sensor_t hwSensor; + hwSensor.name = "Orientation Sensor"; + hwSensor.vendor = "Google Inc."; + hwSensor.version = 1; + hwSensor.handle = '_ypr'; + hwSensor.type = SENSOR_TYPE_ORIENTATION; + hwSensor.maxRange = 360.0f; + hwSensor.resolution = 1.0f/256.0f; // FIXME: real value here + hwSensor.power = mSensorFusion.getPowerUsage(); + hwSensor.minDelay = mSensorFusion.getMinDelay(); + Sensor sensor(&hwSensor); + return sensor; +} + +// --------------------------------------------------------------------------- +}; // namespace android + diff --git a/services/sensorservice/OrientationSensor.h b/services/sensorservice/OrientationSensor.h new file mode 100644 index 000000000..855949dea --- /dev/null +++ b/services/sensorservice/OrientationSensor.h @@ -0,0 +1,51 @@ +/* + * Copyright (C) 2011 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ANDROID_ORIENTATION_SENSOR_H +#define ANDROID_ORIENTATION_SENSOR_H + +#include +#include + +#include + +#include "SensorInterface.h" + +// --------------------------------------------------------------------------- +namespace android { +// --------------------------------------------------------------------------- + +class SensorDevice; +class SensorFusion; + +class OrientationSensor : public SensorInterface { + SensorDevice& mSensorDevice; + SensorFusion& mSensorFusion; + +public: + OrientationSensor(); + virtual bool process(sensors_event_t* outEvent, + const sensors_event_t& event); + virtual status_t activate(void* ident, bool enabled); + virtual status_t setDelay(void* ident, int handle, int64_t ns); + virtual Sensor getSensor() const; + virtual bool isVirtual() const { return true; } +}; + +// --------------------------------------------------------------------------- +}; // namespace android + +#endif // ANDROID_ORIENTATION_SENSOR_H diff --git a/services/sensorservice/RotationVectorSensor.cpp b/services/sensorservice/RotationVectorSensor.cpp index 3abfc12bc..cba89c9fc 100644 --- a/services/sensorservice/RotationVectorSensor.cpp +++ b/services/sensorservice/RotationVectorSensor.cpp @@ -32,134 +32,67 @@ static inline T clamp(T v) { return v < 0 ? 0 : v; } -RotationVectorSensor::RotationVectorSensor(sensor_t const* list, size_t count) +RotationVectorSensor::RotationVectorSensor() : mSensorDevice(SensorDevice::getInstance()), - mALowPass(M_SQRT1_2, 1.5f), - mAX(mALowPass), mAY(mALowPass), mAZ(mALowPass), - mMLowPass(M_SQRT1_2, 1.5f), - mMX(mMLowPass), mMY(mMLowPass), mMZ(mMLowPass) + mSensorFusion(SensorFusion::getInstance()) { - for (size_t i=0 ; idata[0] = qx; + outEvent->data[1] = qy; + outEvent->data[2] = qz; + outEvent->data[3] = qw; + outEvent->sensor = '_rov'; + outEvent->type = SENSOR_TYPE_ROTATION_VECTOR; + return true; } - mAccTime = now; - const float Ex = mMagData[0]; - const float Ey = mMagData[1]; - const float Ez = mMagData[2]; - float Hx = Ey*Az - Ez*Ay; - float Hy = Ez*Ax - Ex*Az; - float Hz = Ex*Ay - Ey*Ax; - const float normH = sqrtf(Hx*Hx + Hy*Hy + Hz*Hz); - if (normH < 0.1f) { - // device is close to free fall (or in space?), or close to - // magnetic north pole. Typical values are > 100. - return false; - } - const float invH = 1.0f / normH; - const float invA = 1.0f / sqrtf(Ax*Ax + Ay*Ay + Az*Az); - Hx *= invH; - Hy *= invH; - Hz *= invH; - Ax *= invA; - Ay *= invA; - Az *= invA; - const float Mx = Ay*Hz - Az*Hy; - const float My = Az*Hx - Ax*Hz; - const float Mz = Ax*Hy - Ay*Hx; - - // matrix to rotation vector (normalized quaternion) - float qw = sqrtf( clamp( Hx + My + Az + 1) * 0.25f ); - float qx = sqrtf( clamp( Hx - My - Az + 1) * 0.25f ); - float qy = sqrtf( clamp(-Hx + My - Az + 1) * 0.25f ); - float qz = sqrtf( clamp(-Hx - My + Az + 1) * 0.25f ); - qx = copysignf(qx, Ay - Mz); - qy = copysignf(qy, Hz - Ax); - qz = copysignf(qz, Mx - Hy); - - // this quaternion is guaranteed to be normalized, by construction - // of the rotation matrix. - - *outEvent = event; - outEvent->data[0] = qx; - outEvent->data[1] = qy; - outEvent->data[2] = qz; - outEvent->data[3] = qw; - outEvent->sensor = '_rov'; - outEvent->type = SENSOR_TYPE_ROTATION_VECTOR; - return true; } return false; } status_t RotationVectorSensor::activate(void* ident, bool enabled) { - mSensorDevice.activate(this, mAcc.getHandle(), enabled); - mSensorDevice.activate(this, mMag.getHandle(), enabled); - if (enabled) { - mMagTime = 0; - mAccTime = 0; - } - return NO_ERROR; + return mSensorFusion.activate(this, enabled); } -status_t RotationVectorSensor::setDelay(void* ident, int handle, int64_t ns) -{ - mSensorDevice.setDelay(this, mAcc.getHandle(), ns); - mSensorDevice.setDelay(this, mMag.getHandle(), ns); - return NO_ERROR; +status_t RotationVectorSensor::setDelay(void* ident, int handle, int64_t ns) { + return mSensorFusion.setDelay(this, ns); } Sensor RotationVectorSensor::getSensor() const { sensor_t hwSensor; hwSensor.name = "Rotation Vector Sensor"; hwSensor.vendor = "Google Inc."; - hwSensor.version = 1; + hwSensor.version = mSensorFusion.hasGyro() ? 3 : 2; hwSensor.handle = '_rov'; hwSensor.type = SENSOR_TYPE_ROTATION_VECTOR; hwSensor.maxRange = 1; hwSensor.resolution = 1.0f / (1<<24); - hwSensor.power = mAcc.getPowerUsage() + mMag.getPowerUsage(); - hwSensor.minDelay = mAcc.getMinDelay(); + hwSensor.power = mSensorFusion.getPowerUsage(); + hwSensor.minDelay = mSensorFusion.getMinDelay(); Sensor sensor(&hwSensor); return sensor; } diff --git a/services/sensorservice/RotationVectorSensor.h b/services/sensorservice/RotationVectorSensor.h index 17699f8a4..ac76487e4 100644 --- a/services/sensorservice/RotationVectorSensor.h +++ b/services/sensorservice/RotationVectorSensor.h @@ -26,24 +26,19 @@ #include "SensorInterface.h" #include "SecondOrderLowPassFilter.h" +#include "Fusion.h" +#include "SensorFusion.h" + // --------------------------------------------------------------------------- namespace android { // --------------------------------------------------------------------------- class RotationVectorSensor : public SensorInterface { SensorDevice& mSensorDevice; - Sensor mAcc; - Sensor mMag; - float mMagData[3]; - double mAccTime; - double mMagTime; - SecondOrderLowPassFilter mALowPass; - CascadedBiquadFilter mAX, mAY, mAZ; - SecondOrderLowPassFilter mMLowPass; - CascadedBiquadFilter mMX, mMY, mMZ; + SensorFusion& mSensorFusion; public: - RotationVectorSensor(sensor_t const* list, size_t count); + RotationVectorSensor(); virtual bool process(sensors_event_t* outEvent, const sensors_event_t& event); virtual status_t activate(void* ident, bool enabled); diff --git a/services/sensorservice/SecondOrderLowPassFilter.cpp b/services/sensorservice/SecondOrderLowPassFilter.cpp index eeb6d1e84..c76dd4cff 100644 --- a/services/sensorservice/SecondOrderLowPassFilter.cpp +++ b/services/sensorservice/SecondOrderLowPassFilter.cpp @@ -21,6 +21,7 @@ #include #include "SecondOrderLowPassFilter.h" +#include "vec.h" // --------------------------------------------------------------------------- @@ -44,21 +45,24 @@ void SecondOrderLowPassFilter::setSamplingPeriod(float dT) // --------------------------------------------------------------------------- -BiquadFilter::BiquadFilter(const SecondOrderLowPassFilter& s) +template +BiquadFilter::BiquadFilter(const SecondOrderLowPassFilter& s) : s(s) { } -float BiquadFilter::init(float x) +template +T BiquadFilter::init(const T& x) { x1 = x2 = x; y1 = y2 = x; return x; } -float BiquadFilter::operator()(float x) +template +T BiquadFilter::operator()(const T& x) { - float y = (x + x2)*s.a0 + x1*s.a1 - y1*s.b1 - y2*s.b2; + T y = (x + x2)*s.a0 + x1*s.a1 - y1*s.b1 - y2*s.b2; x2 = x1; y2 = y1; x1 = x; @@ -68,22 +72,32 @@ float BiquadFilter::operator()(float x) // --------------------------------------------------------------------------- -CascadedBiquadFilter::CascadedBiquadFilter(const SecondOrderLowPassFilter& s) +template +CascadedBiquadFilter::CascadedBiquadFilter(const SecondOrderLowPassFilter& s) : mA(s), mB(s) { } -float CascadedBiquadFilter::init(float x) +template +T CascadedBiquadFilter::init(const T& x) { mA.init(x); mB.init(x); return x; } -float CascadedBiquadFilter::operator()(float x) +template +T CascadedBiquadFilter::operator()(const T& x) { return mB(mA(x)); } +// --------------------------------------------------------------------------- + +template class BiquadFilter; +template class CascadedBiquadFilter; +template class BiquadFilter; +template class CascadedBiquadFilter; + // --------------------------------------------------------------------------- }; // namespace android diff --git a/services/sensorservice/SecondOrderLowPassFilter.h b/services/sensorservice/SecondOrderLowPassFilter.h index 85698ca63..0cc2446cc 100644 --- a/services/sensorservice/SecondOrderLowPassFilter.h +++ b/services/sensorservice/SecondOrderLowPassFilter.h @@ -25,12 +25,14 @@ namespace android { // --------------------------------------------------------------------------- +template class BiquadFilter; /* * State of a 2nd order low-pass IIR filter */ class SecondOrderLowPassFilter { + template friend class BiquadFilter; float iQ, fc; float K, iD; @@ -44,27 +46,29 @@ public: /* * Implements a Biquad IIR filter */ +template class BiquadFilter { - float x1, x2; - float y1, y2; + T x1, x2; + T y1, y2; const SecondOrderLowPassFilter& s; public: BiquadFilter(const SecondOrderLowPassFilter& s); - float init(float in); - float operator()(float in); + T init(const T& in); + T operator()(const T& in); }; /* * Two cascaded biquad IIR filters * (4-poles IIR) */ +template class CascadedBiquadFilter { - BiquadFilter mA; - BiquadFilter mB; + BiquadFilter mA; + BiquadFilter mB; public: CascadedBiquadFilter(const SecondOrderLowPassFilter& s); - float init(float in); - float operator()(float in); + T init(const T& in); + T operator()(const T& in); }; // --------------------------------------------------------------------------- diff --git a/services/sensorservice/SensorDevice.cpp b/services/sensorservice/SensorDevice.cpp index b3c8ef59e..38d498c06 100644 --- a/services/sensorservice/SensorDevice.cpp +++ b/services/sensorservice/SensorDevice.cpp @@ -251,6 +251,9 @@ status_t SensorDevice::setDelay(void* ident, int handle, int64_t ns) } } } + + //LOGD("setDelay: ident=%p, handle=%d, ns=%lld", ident, handle, ns); + return mSensorDevice->setDelay(mSensorDevice, handle, ns); } diff --git a/services/sensorservice/SensorFusion.cpp b/services/sensorservice/SensorFusion.cpp new file mode 100644 index 000000000..d4226ec11 --- /dev/null +++ b/services/sensorservice/SensorFusion.cpp @@ -0,0 +1,180 @@ +/* + * Copyright (C) 2011 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "SensorDevice.h" +#include "SensorFusion.h" +#include "SensorService.h" + +namespace android { +// --------------------------------------------------------------------------- + +ANDROID_SINGLETON_STATIC_INSTANCE(SensorFusion) + +SensorFusion::SensorFusion() + : mSensorDevice(SensorDevice::getInstance()), + mEnabled(false), mHasGyro(false), mGyroTime(0), mRotationMatrix(1), + mLowPass(M_SQRT1_2, 1.0f), mAccData(mLowPass), + mFilteredMag(0.0f), mFilteredAcc(0.0f) +{ + sensor_t const* list; + size_t count = mSensorDevice.getSensorList(&list); + for (size_t i=0 ; i5 && l<100) { + mFilteredMag = mag * (1/l); + } + } + } else if (event.type == SENSOR_TYPE_ACCELEROMETER) { + const vec3_t acc(event.data); + if (mHasGyro) { + mFusion.handleAcc(acc); + mRotationMatrix = mFusion.getRotationMatrix(); + } else { + const float l(length(acc)); + if (l > 0.981f) { + // remove the linear-acceleration components + mFilteredAcc = mAccData(acc * (1/l)); + } + if (length(mFilteredAcc)>0 && length(mFilteredMag)>0) { + vec3_t up(mFilteredAcc); + vec3_t east(cross_product(mFilteredMag, up)); + east *= 1/length(east); + vec3_t north(cross_product(up, east)); + mRotationMatrix << east << north << up; + } + } + } +} + +template inline T min(T a, T b) { return a inline T max(T a, T b) { return a>b ? a : b; } + +status_t SensorFusion::activate(void* ident, bool enabled) { + + LOGD_IF(DEBUG_CONNECTIONS, + "SensorFusion::activate(ident=%p, enabled=%d)", + ident, enabled); + + const ssize_t idx = mClients.indexOf(ident); + if (enabled) { + if (idx < 0) { + mClients.add(ident); + } + } else { + if (idx >= 0) { + mClients.removeItemsAt(idx); + } + } + + mSensorDevice.activate(ident, mAcc.getHandle(), enabled); + mSensorDevice.activate(ident, mMag.getHandle(), enabled); + if (mHasGyro) { + mSensorDevice.activate(ident, mGyro.getHandle(), enabled); + } + + const bool newState = mClients.size() != 0; + if (newState != mEnabled) { + mEnabled = newState; + if (newState) { + mFusion.init(); + } + } + return NO_ERROR; +} + +status_t SensorFusion::setDelay(void* ident, int64_t ns) { + if (mHasGyro) { + mSensorDevice.setDelay(ident, mAcc.getHandle(), ns); + mSensorDevice.setDelay(ident, mMag.getHandle(), ms2ns(20)); + mSensorDevice.setDelay(ident, mGyro.getHandle(), mTargetDelayNs); + } else { + const static double NS2S = 1.0 / 1000000000.0; + mSensorDevice.setDelay(ident, mAcc.getHandle(), ns); + mSensorDevice.setDelay(ident, mMag.getHandle(), max(ns, mMag.getMinDelayNs())); + mLowPass.setSamplingPeriod(ns*NS2S); + } + return NO_ERROR; +} + + +float SensorFusion::getPowerUsage() const { + float power = mAcc.getPowerUsage() + mMag.getPowerUsage(); + if (mHasGyro) { + power += mGyro.getPowerUsage(); + } + return power; +} + +int32_t SensorFusion::getMinDelay() const { + return mAcc.getMinDelay(); +} + +void SensorFusion::dump(String8& result, char* buffer, size_t SIZE) { + const Fusion& fusion(mFusion); + snprintf(buffer, SIZE, "Fusion (%s) %s (%d clients), gyro-rate=%7.2fHz, " + "MRPS=< %g, %g, %g > (%g), " + "BIAS=< %g, %g, %g >\n", + mHasGyro ? "9-axis" : "6-axis", + mEnabled ? "enabled" : "disabled", + mClients.size(), + mGyroRate, + fusion.getAttitude().x, + fusion.getAttitude().y, + fusion.getAttitude().z, + dot_product(fusion.getAttitude(), fusion.getAttitude()), + fusion.getBias().x, + fusion.getBias().y, + fusion.getBias().z); + result.append(buffer); +} + +// --------------------------------------------------------------------------- +}; // namespace android diff --git a/services/sensorservice/SensorFusion.h b/services/sensorservice/SensorFusion.h new file mode 100644 index 000000000..c7eab1262 --- /dev/null +++ b/services/sensorservice/SensorFusion.h @@ -0,0 +1,84 @@ +/* + * Copyright (C) 2011 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ANDROID_SENSOR_FUSION_H +#define ANDROID_SENSOR_FUSION_H + +#include +#include + +#include +#include +#include + +#include + +#include "Fusion.h" +#include "SecondOrderLowPassFilter.h" + +// --------------------------------------------------------------------------- + +namespace android { +// --------------------------------------------------------------------------- + +class SensorDevice; + +class SensorFusion : public Singleton { + friend class Singleton; + + SensorDevice& mSensorDevice; + Sensor mAcc; + Sensor mMag; + Sensor mGyro; + Fusion mFusion; + bool mEnabled; + bool mHasGyro; + float mGyroRate; + nsecs_t mTargetDelayNs; + nsecs_t mGyroTime; + mat33_t mRotationMatrix; + SecondOrderLowPassFilter mLowPass; + BiquadFilter mAccData; + vec3_t mFilteredMag; + vec3_t mFilteredAcc; + SortedVector mClients; + + SensorFusion(); + +public: + void process(const sensors_event_t& event); + + bool isEnabled() const { return mEnabled; } + bool hasGyro() const { return mHasGyro; } + bool hasEstimate() const { return !mHasGyro || mFusion.hasEstimate(); } + mat33_t getRotationMatrix() const { return mRotationMatrix; } + vec3_t getGyroBias() const { return mFusion.getBias(); } + float getEstimatedRate() const { return mGyroRate; } + + status_t activate(void* ident, bool enabled); + status_t setDelay(void* ident, int64_t ns); + + float getPowerUsage() const; + int32_t getMinDelay() const; + + void dump(String8& result, char* buffer, size_t SIZE); +}; + + +// --------------------------------------------------------------------------- +}; // namespace android + +#endif // ANDROID_SENSOR_FUSION_H diff --git a/services/sensorservice/SensorInterface.h b/services/sensorservice/SensorInterface.h index 084f2f5cc..fb357d769 100644 --- a/services/sensorservice/SensorInterface.h +++ b/services/sensorservice/SensorInterface.h @@ -20,8 +20,6 @@ #include #include -#include - #include #include "SensorDevice.h" diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp index f1db2f555..5b86d10d0 100644 --- a/services/sensorservice/SensorService.cpp +++ b/services/sensorservice/SensorService.cpp @@ -35,10 +35,13 @@ #include -#include "SensorService.h" +#include "CorrectedGyroSensor.h" #include "GravitySensor.h" #include "LinearAccelerationSensor.h" +#include "OrientationSensor.h" #include "RotationVectorSensor.h" +#include "SensorFusion.h" +#include "SensorService.h" namespace android { // --------------------------------------------------------------------------- @@ -74,14 +77,26 @@ void SensorService::onFirstRef() } } - if (virtualSensorsNeeds & (1<& args) for (size_t i=0 ; i\n", + snprintf(buffer, SIZE, + "%-48s| %-32s | 0x%08x | maxRate=%7.2fHz | " + "last=<%5.1f,%5.1f,%5.1f>\n", s.getName().string(), s.getVendor().string(), s.getHandle(), @@ -141,6 +158,7 @@ status_t SensorService::dump(int fd, const Vector& args) e.data[0], e.data[1], e.data[2]); result.append(buffer); } + SensorFusion::getInstance().dump(result, buffer, SIZE); SensorDevice::getInstance().dump(result, buffer, SIZE); snprintf(buffer, SIZE, "%d active connections\n", @@ -183,13 +201,19 @@ bool SensorService::threadLoop() // handle virtual sensors if (count && vcount) { + sensors_event_t const * const event = buffer; const DefaultKeyedVector virtualSensors( getActiveVirtualSensors()); const size_t activeVirtualSensorCount = virtualSensors.size(); if (activeVirtualSensorCount) { size_t k = 0; + SensorFusion& fusion(SensorFusion::getInstance()); + if (fusion.isEnabled()) { + for (size_t i=0 ; iprocess(&out, event[i])) { diff --git a/services/sensorservice/mat.h b/services/sensorservice/mat.h new file mode 100644 index 000000000..1302ca327 --- /dev/null +++ b/services/sensorservice/mat.h @@ -0,0 +1,370 @@ +/* + * Copyright (C) 2011 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ANDROID_MAT_H +#define ANDROID_MAT_H + +#include "vec.h" +#include "traits.h" + +// ----------------------------------------------------------------------- + +namespace android { + +template +class mat; + +namespace helpers { + +template +mat& doAssign( + mat& lhs, + typename TypeTraits::ParameterType rhs) { + for (size_t i=0 ; i +mat PURE doMul( + const mat& lhs, + const mat& rhs) +{ + mat res; + for (size_t c=0 ; c +vec PURE doMul( + const mat& lhs, + const vec& rhs) +{ + vec res; + for (size_t r=0 ; r +mat PURE doMul( + const vec& lhs, + const mat& rhs) +{ + mat res; + for (size_t c=0 ; c +mat PURE doMul( + const mat& rhs, + typename TypeTraits::ParameterType v) +{ + mat res; + for (size_t c=0 ; c +mat PURE doMul( + typename TypeTraits::ParameterType v, + const mat& rhs) +{ + mat res; + for (size_t c=0 ; c +class mat : public vec< vec, C > { + typedef typename TypeTraits::ParameterType pTYPE; + typedef vec< vec, C > base; +public: + // STL-like interface. + typedef TYPE value_type; + typedef TYPE& reference; + typedef TYPE const& const_reference; + typedef size_t size_type; + size_type size() const { return R*C; } + enum { ROWS = R, COLS = C }; + + + // ----------------------------------------------------------------------- + // default constructors + + mat() { } + mat(const mat& rhs) : base(rhs) { } + mat(const base& rhs) : base(rhs) { } + + // ----------------------------------------------------------------------- + // conversion constructors + + // sets the diagonal to the value, off-diagonal to zero + mat(pTYPE rhs) { + helpers::doAssign(*this, rhs); + } + + // ----------------------------------------------------------------------- + // Assignment + + mat& operator=(const mat& rhs) { + base::operator=(rhs); + return *this; + } + + mat& operator=(const base& rhs) { + base::operator=(rhs); + return *this; + } + + mat& operator=(pTYPE rhs) { + return helpers::doAssign(*this, rhs); + } + + // ----------------------------------------------------------------------- + // non-member function declaration and definition + + friend inline mat PURE operator + (const mat& lhs, const mat& rhs) { + return helpers::doAdd( + static_cast(lhs), + static_cast(rhs)); + } + friend inline mat PURE operator - (const mat& lhs, const mat& rhs) { + return helpers::doSub( + static_cast(lhs), + static_cast(rhs)); + } + + // matrix*matrix + template + friend mat PURE operator * ( + const mat& lhs, + const mat& rhs) { + return helpers::doMul(lhs, rhs); + } + + // matrix*vector + friend vec PURE operator * ( + const mat& lhs, const vec& rhs) { + return helpers::doMul(lhs, rhs); + } + + // vector*matrix + friend mat PURE operator * ( + const vec& lhs, const mat& rhs) { + return helpers::doMul(lhs, rhs); + } + + // matrix*scalar + friend inline mat PURE operator * (const mat& lhs, pTYPE v) { + return helpers::doMul(lhs, v); + } + + // scalar*matrix + friend inline mat PURE operator * (pTYPE v, const mat& rhs) { + return helpers::doMul(v, rhs); + } + + // ----------------------------------------------------------------------- + // streaming operator to set the columns of the matrix: + // example: + // mat33_t m; + // m << v0 << v1 << v2; + + // column_builder<> stores the matrix and knows which column to set + template + struct column_builder { + mat& matrix; + column_builder(mat& matrix) : matrix(matrix) { } + }; + + // operator << is not a method of column_builder<> so we can + // overload it for unauthorized values (partial specialization + // not allowed in class-scope). + // we just set the column and return the next column_builder<> + template + friend column_builder operator << ( + const column_builder& lhs, + const vec& rhs) { + lhs.matrix[PREV_COLUMN+1] = rhs; + return column_builder(lhs.matrix); + } + + // we return void here so we get a compile-time error if the + // user tries to set too many columns + friend void operator << ( + const column_builder& lhs, + const vec& rhs) { + lhs.matrix[C-1] = rhs; + } + + // this is where the process starts. we set the first columns and + // return the next column_builder<> + column_builder<0> operator << (const vec& rhs) { + (*this)[0] = rhs; + return column_builder<0>(*this); + } +}; + +// Specialize column matrix so they're exactly equivalent to a vector +template +class mat : public vec { + typedef vec base; +public: + // STL-like interface. + typedef TYPE value_type; + typedef TYPE& reference; + typedef TYPE const& const_reference; + typedef size_t size_type; + size_type size() const { return R; } + enum { ROWS = R, COLS = 1 }; + + mat() { } + mat(const base& rhs) : base(rhs) { } + mat(const mat& rhs) : base(rhs) { } + mat(const TYPE& rhs) { helpers::doAssign(*this, rhs); } + mat& operator=(const mat& rhs) { base::operator=(rhs); return *this; } + mat& operator=(const base& rhs) { base::operator=(rhs); return *this; } + mat& operator=(const TYPE& rhs) { return helpers::doAssign(*this, rhs); } + // we only have one column, so ignore the index + const base& operator[](size_t) const { return *this; } + base& operator[](size_t) { return *this; } + void operator << (const vec& rhs) { base::operator[](0) = rhs; } +}; + +// ----------------------------------------------------------------------- +// matrix functions + +// transpose. this handles matrices of matrices +inline int PURE transpose(int v) { return v; } +inline float PURE transpose(float v) { return v; } +inline double PURE transpose(double v) { return v; } + +// Transpose a matrix +template +mat PURE transpose(const mat& m) { + mat r; + for (size_t i=0 ; i class VEC, + typename TYPE, + size_t SIZE +> +mat PURE transpose(const VEC& v) { + mat r; + for (size_t i=0 ; i +mat PURE invert(const mat& src) { + T t; + size_t swap; + mat tmp(src); + mat inverse(1); + + for (size_t i=0 ; i fabs(tmp[i][i])) { + swap = j; + } + } + + if (swap != i) { + /* swap rows. */ + for (size_t k=0 ; k mat22_t; +typedef mat mat33_t; +typedef mat mat44_t; + +// ----------------------------------------------------------------------- + +}; // namespace android + +#endif /* ANDROID_MAT_H */ diff --git a/services/sensorservice/traits.h b/services/sensorservice/traits.h new file mode 100644 index 000000000..da4c599a7 --- /dev/null +++ b/services/sensorservice/traits.h @@ -0,0 +1,118 @@ +/* + * Copyright (C) 2011 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ANDROID_TRAITS_H +#define ANDROID_TRAITS_H + +// ----------------------------------------------------------------------- +// Typelists + +namespace android { + +// end-of-list marker +class NullType {}; + +// type-list node +template +struct TypeList { + typedef T Head; + typedef U Tail; +}; + +// helpers to build typelists +#define TYPELIST_1(T1) TypeList +#define TYPELIST_2(T1, T2) TypeList +#define TYPELIST_3(T1, T2, T3) TypeList +#define TYPELIST_4(T1, T2, T3, T4) TypeList + +// typelists algorithms +namespace TL { +template struct IndexOf; + +template +struct IndexOf { + enum { value = -1 }; +}; + +template +struct IndexOf, T> { + enum { value = 0 }; +}; + +template +struct IndexOf, T> { +private: + enum { temp = IndexOf::value }; +public: + enum { value = temp == -1 ? -1 : 1 + temp }; +}; + +}; // namespace TL + +// type selection based on a boolean +template +struct Select { + typedef T Result; +}; +template +struct Select { + typedef U Result; +}; + +// ----------------------------------------------------------------------- +// Type traits + +template +class TypeTraits { + typedef TYPELIST_4( + unsigned char, unsigned short, + unsigned int, unsigned long int) UnsignedInts; + + typedef TYPELIST_4( + signed char, signed short, + signed int, signed long int) SignedInts; + + typedef TYPELIST_1( + bool) OtherInts; + + typedef TYPELIST_3( + float, double, long double) Floats; + + template struct PointerTraits { + enum { result = false }; + typedef NullType PointeeType; + }; + template struct PointerTraits { + enum { result = true }; + typedef U PointeeType; + }; + +public: + enum { isStdUnsignedInt = TL::IndexOf::value >= 0 }; + enum { isStdSignedInt = TL::IndexOf::value >= 0 }; + enum { isStdIntegral = TL::IndexOf::value >= 0 || isStdUnsignedInt || isStdSignedInt }; + enum { isStdFloat = TL::IndexOf::value >= 0 }; + enum { isPointer = PointerTraits::result }; + enum { isStdArith = isStdIntegral || isStdFloat }; + + // best parameter type for given type + typedef typename Select::Result ParameterType; +}; + +// ----------------------------------------------------------------------- +}; // namespace android + +#endif /* ANDROID_TRAITS_H */ diff --git a/services/sensorservice/vec.h b/services/sensorservice/vec.h new file mode 100644 index 000000000..736ff37b9 --- /dev/null +++ b/services/sensorservice/vec.h @@ -0,0 +1,420 @@ +/* + * Copyright (C) 2011 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ANDROID_VEC_H +#define ANDROID_VEC_H + +#include + +#include +#include + +#include "traits.h" + +// ----------------------------------------------------------------------- + +#define PURE __attribute__((pure)) + +namespace android { + +// ----------------------------------------------------------------------- +// non-inline helpers + +template +class vec; + +template +class vbase; + +namespace helpers { + +template inline T min(T a, T b) { return a inline T max(T a, T b) { return a>b ? a : b; } + +template < template class VEC, + typename TYPE, size_t SIZE, size_t S> +vec& doAssign( + vec& lhs, const VEC& rhs) { + const size_t minSize = min(SIZE, S); + const size_t maxSize = max(SIZE, S); + for (size_t i=0 ; i class VLHS, + template class VRHS, + typename TYPE, + size_t SIZE +> +VLHS PURE doAdd( + const VLHS& lhs, + const VRHS& rhs) { + VLHS r; + for (size_t i=0 ; i class VLHS, + template class VRHS, + typename TYPE, + size_t SIZE +> +VLHS PURE doSub( + const VLHS& lhs, + const VRHS& rhs) { + VLHS r; + for (size_t i=0 ; i class VEC, + typename TYPE, + size_t SIZE +> +VEC PURE doMulScalar( + const VEC& lhs, + typename TypeTraits::ParameterType rhs) { + VEC r; + for (size_t i=0 ; i class VEC, + typename TYPE, + size_t SIZE +> +VEC PURE doScalarMul( + typename TypeTraits::ParameterType lhs, + const VEC& rhs) { + VEC r; + for (size_t i=0 ; i. Without this, an extra conversion to vec<> would be needed. +// +// example: +// vec4_t a; +// vec3_t b; +// vec3_t c = a.xyz + b; +// +// "a.xyz + b" is a mixed-operation between a vbase<> and a vec<>, requiring +// a conversion of vbase<> to vec<>. The template gunk below avoids this, +// by allowing the addition on these different vector types directly +// + +template < + template class VLHS, + template class VRHS, + typename TYPE, + size_t SIZE +> +inline VLHS PURE operator + ( + const VLHS& lhs, + const VRHS& rhs) { + return helpers::doAdd(lhs, rhs); +} + +template < + template class VLHS, + template class VRHS, + typename TYPE, + size_t SIZE +> +inline VLHS PURE operator - ( + const VLHS& lhs, + const VRHS& rhs) { + return helpers::doSub(lhs, rhs); +} + +template < + template class VEC, + typename TYPE, + size_t SIZE +> +inline VEC PURE operator * ( + const VEC& lhs, + typename TypeTraits::ParameterType rhs) { + return helpers::doMulScalar(lhs, rhs); +} + +template < + template class VEC, + typename TYPE, + size_t SIZE +> +inline VEC PURE operator * ( + typename TypeTraits::ParameterType lhs, + const VEC& rhs) { + return helpers::doScalarMul(lhs, rhs); +} + + +template < + template class VLHS, + template class VRHS, + typename TYPE, + size_t SIZE +> +TYPE PURE dot_product( + const VLHS& lhs, + const VRHS& rhs) { + TYPE r(0); + for (size_t i=0 ; i class V, + typename TYPE, + size_t SIZE +> +TYPE PURE length(const V& v) { + return sqrt(dot_product(v, v)); +} + +template < + template class VLHS, + template class VRHS, + typename TYPE +> +VLHS PURE cross_product( + const VLHS& u, + const VRHS& v) { + VLHS r; + r.x = u.y*v.z - u.z*v.y; + r.y = u.z*v.x - u.x*v.z; + r.z = u.x*v.y - u.y*v.x; + return r; +} + + +template +vec PURE operator - (const vec& lhs) { + vec r; + for (size_t i=0 ; i +struct vbase { + TYPE v[SIZE]; + inline const TYPE& operator[](size_t i) const { return v[i]; } + inline TYPE& operator[](size_t i) { return v[i]; } +}; +template<> struct vbase { + union { + float v[2]; + struct { float x, y; }; + struct { float s, t; }; + }; + inline const float& operator[](size_t i) const { return v[i]; } + inline float& operator[](size_t i) { return v[i]; } +}; +template<> struct vbase { + union { + float v[3]; + struct { float x, y, z; }; + struct { float s, t, r; }; + vbase xy; + vbase st; + }; + inline const float& operator[](size_t i) const { return v[i]; } + inline float& operator[](size_t i) { return v[i]; } +}; +template<> struct vbase { + union { + float v[4]; + struct { float x, y, z, w; }; + struct { float s, t, r, q; }; + vbase xyz; + vbase str; + vbase xy; + vbase st; + }; + inline const float& operator[](size_t i) const { return v[i]; } + inline float& operator[](size_t i) { return v[i]; } +}; + +// ----------------------------------------------------------------------- + +template +class vec : public vbase +{ + typedef typename TypeTraits::ParameterType pTYPE; + typedef vbase base; + +public: + // STL-like interface. + typedef TYPE value_type; + typedef TYPE& reference; + typedef TYPE const& const_reference; + typedef size_t size_type; + + typedef TYPE* iterator; + typedef TYPE const* const_iterator; + iterator begin() { return base::v; } + iterator end() { return base::v + SIZE; } + const_iterator begin() const { return base::v; } + const_iterator end() const { return base::v + SIZE; } + size_type size() const { return SIZE; } + + // ----------------------------------------------------------------------- + // default constructors + + vec() { } + vec(const vec& rhs) : base(rhs) { } + vec(const base& rhs) : base(rhs) { } + + // ----------------------------------------------------------------------- + // conversion constructors + + vec(pTYPE rhs) { + for (size_t i=0 ; i class VEC, size_t S> + explicit vec(const VEC& rhs) { + helpers::doAssign(*this, rhs); + } + + explicit vec(TYPE const* array) { + for (size_t i=0 ; i class VEC, size_t S> + vec& operator = (const VEC& rhs) { + return helpers::doAssign(*this, rhs); + } + + // ----------------------------------------------------------------------- + // operation-assignment + + vec& operator += (const vec& rhs); + vec& operator -= (const vec& rhs); + vec& operator *= (pTYPE rhs); + + // ----------------------------------------------------------------------- + // non-member function declaration and definition + // NOTE: we declare the non-member function as friend inside the class + // so that they are known to the compiler when the class is instantiated. + // This helps the compiler doing template argument deduction when the + // passed types are not identical. Essentially this helps with + // type conversion so that you can multiply a vec by an scalar int + // (for instance). + + friend inline vec PURE operator + (const vec& lhs, const vec& rhs) { + return helpers::doAdd(lhs, rhs); + } + friend inline vec PURE operator - (const vec& lhs, const vec& rhs) { + return helpers::doSub(lhs, rhs); + } + friend inline vec PURE operator * (const vec& lhs, pTYPE v) { + return helpers::doMulScalar(lhs, v); + } + friend inline vec PURE operator * (pTYPE v, const vec& rhs) { + return helpers::doScalarMul(v, rhs); + } + friend inline TYPE PURE dot_product(const vec& lhs, const vec& rhs) { + return android::dot_product(lhs, rhs); + } +}; + +// ----------------------------------------------------------------------- + +template +vec& vec::operator += (const vec& rhs) { + vec& lhs(*this); + for (size_t i=0 ; i +vec& vec::operator -= (const vec& rhs) { + vec& lhs(*this); + for (size_t i=0 ; i +vec& vec::operator *= (vec::pTYPE rhs) { + vec& lhs(*this); + for (size_t i=0 ; i vec2_t; +typedef vec vec3_t; +typedef vec vec4_t; + +// ----------------------------------------------------------------------- + +}; // namespace android + +#endif /* ANDROID_VEC_H */ From 3301542828febc768e1df42892cfac4992c35474 Mon Sep 17 00:00:00 2001 From: Mathias Agopian Date: Fri, 27 May 2011 18:18:13 -0700 Subject: [PATCH 26/51] use quaternions instead of MRPs also use correct time propagation equation disable the fused sensors when gyro is not present since they were unusable in practice. Change-Id: Iad797425784e67dc6c5690e97c71c583418cc5b5 --- services/sensorservice/Android.mk | 1 - .../sensorservice/CorrectedGyroSensor.cpp | 2 +- services/sensorservice/Fusion.cpp | 273 ++++++------------ services/sensorservice/Fusion.h | 35 ++- services/sensorservice/GravitySensor.cpp | 67 +---- services/sensorservice/GravitySensor.h | 5 - services/sensorservice/OrientationSensor.cpp | 7 +- .../sensorservice/RotationVectorSensor.cpp | 83 ++++-- services/sensorservice/RotationVectorSensor.h | 15 +- .../SecondOrderLowPassFilter.cpp | 103 ------- .../sensorservice/SecondOrderLowPassFilter.h | 77 ----- services/sensorservice/SensorFusion.cpp | 82 ++---- services/sensorservice/SensorFusion.h | 14 +- services/sensorservice/SensorService.cpp | 43 ++- services/sensorservice/quat.h | 98 +++++++ services/sensorservice/vec.h | 9 + 16 files changed, 366 insertions(+), 548 deletions(-) delete mode 100644 services/sensorservice/SecondOrderLowPassFilter.cpp delete mode 100644 services/sensorservice/SecondOrderLowPassFilter.h create mode 100644 services/sensorservice/quat.h diff --git a/services/sensorservice/Android.mk b/services/sensorservice/Android.mk index 57a3b1569..ba3e6e5bf 100644 --- a/services/sensorservice/Android.mk +++ b/services/sensorservice/Android.mk @@ -8,7 +8,6 @@ LOCAL_SRC_FILES:= \ LinearAccelerationSensor.cpp \ OrientationSensor.cpp \ RotationVectorSensor.cpp \ - SecondOrderLowPassFilter.cpp \ SensorDevice.cpp \ SensorFusion.cpp \ SensorInterface.cpp \ diff --git a/services/sensorservice/CorrectedGyroSensor.cpp b/services/sensorservice/CorrectedGyroSensor.cpp index 9b75b7024..1857443e8 100644 --- a/services/sensorservice/CorrectedGyroSensor.cpp +++ b/services/sensorservice/CorrectedGyroSensor.cpp @@ -45,7 +45,7 @@ bool CorrectedGyroSensor::process(sensors_event_t* outEvent, const sensors_event_t& event) { if (event.type == SENSOR_TYPE_GYROSCOPE) { - const vec3_t bias(mSensorFusion.getGyroBias() * mSensorFusion.getEstimatedRate()); + const vec3_t bias(mSensorFusion.getGyroBias()); *outEvent = event; outEvent->data[0] -= bias.x; outEvent->data[1] -= bias.y; diff --git a/services/sensorservice/Fusion.cpp b/services/sensorservice/Fusion.cpp index 56ac9f931..b5f97e035 100644 --- a/services/sensorservice/Fusion.cpp +++ b/services/sensorservice/Fusion.cpp @@ -24,15 +24,14 @@ namespace android { // ----------------------------------------------------------------------- -template -static inline TYPE sqr(TYPE x) { - return x*x; -} +static const float gyroSTDEV = 3.16e-4; // rad/s^3/2 +static const float accSTDEV = 0.05f; // m/s^2 (measured 0.08 / CDD 0.05) +static const float magSTDEV = 0.5f; // uT (measured 0.7 / CDD 0.5) +static const float biasSTDEV = 3.16e-5; // rad/s^1/2 (guessed) -template -static inline T clamp(T v) { - return v < 0 ? 0 : v; -} +static const float FREE_FALL_THRESHOLD = 0.981f; + +// ----------------------------------------------------------------------- template static mat scaleCovariance( @@ -71,33 +70,6 @@ static mat crossMatrix(const vec& p, OTHER_TYPE diag) { return r; } -template -static mat MRPsToMatrix(const vec& p) { - mat res(1); - const mat px(crossMatrix(p, 0)); - const TYPE ptp(dot_product(p,p)); - const TYPE t = 4/sqr(1+ptp); - res -= t * (1-ptp) * px; - res += t * 2 * sqr(px); - return res; -} - -template -vec matrixToMRPs(const mat& R) { - // matrix to MRPs - vec q; - const float Hx = R[0].x; - const float My = R[1].y; - const float Az = R[2].z; - const float w = 1 / (1 + sqrtf( clamp( Hx + My + Az + 1) * 0.25f )); - q.x = sqrtf( clamp( Hx - My - Az + 1) * 0.25f ) * w; - q.y = sqrtf( clamp(-Hx + My - Az + 1) * 0.25f ) * w; - q.z = sqrtf( clamp(-Hx - My + Az + 1) * 0.25f ) * w; - q.x = copysignf(q.x, R[2].y - R[1].z); - q.y = copysignf(q.y, R[0].z - R[2].x); - q.z = copysignf(q.z, R[1].x - R[0].y); - return q; -} template class Covariance { @@ -128,11 +100,8 @@ public: // ----------------------------------------------------------------------- Fusion::Fusion() { - // process noise covariance matrix - const float w1 = gyroSTDEV; - const float w2 = biasSTDEV; - Q[0] = w1*w1; - Q[1] = w2*w2; + Phi[0][1] = 0; + Phi[1][1] = 1; Ba.x = 0; Ba.y = 0; @@ -146,25 +115,46 @@ Fusion::Fusion() { } void Fusion::init() { - // initial estimate: E{ x(t0) } - x = 0; - - // initial covariance: Var{ x(t0) } - P = 0; - mInitState = 0; + mGyroRate = 0; mCount[0] = 0; mCount[1] = 0; mCount[2] = 0; mData = 0; } +void Fusion::initFusion(const vec4_t& q, float dT) +{ + // initial estimate: E{ x(t0) } + x0 = q; + x1 = 0; + + // process noise covariance matrix + // G = | -1 0 | + // | 0 1 | + + const float v = gyroSTDEV; + const float u = biasSTDEV; + const float q00 = v*v*dT + 0.33333f*(dT*dT*dT)*u*u; + const float q10 = 0.5f*(dT*dT) *u*u; + const float q01 = q10; + const float q11 = u*u*dT; + GQGt[0][0] = q00; + GQGt[1][0] = -q10; + GQGt[0][1] = -q01; + GQGt[1][1] = q11; + + + // initial covariance: Var{ x(t0) } + P = 0; +} + bool Fusion::hasEstimate() const { return (mInitState == (MAG|ACC|GYRO)); } -bool Fusion::checkInitComplete(int what, const vec3_t& d) { - if (mInitState == (MAG|ACC|GYRO)) +bool Fusion::checkInitComplete(int what, const vec3_t& d, float dT) { + if (hasEstimate()) return true; if (what == ACC) { @@ -176,7 +166,8 @@ bool Fusion::checkInitComplete(int what, const vec3_t& d) { mCount[1]++; mInitState |= MAG; } else if (what == GYRO) { - mData[2] += d; + mGyroRate = dT; + mData[2] += d*dT; mCount[2]++; if (mCount[2] == 64) { // 64 samples is good enough to estimate the gyro drift and @@ -199,37 +190,29 @@ bool Fusion::checkInitComplete(int what, const vec3_t& d) { east *= 1/length(east); vec3_t north(cross_product(up, east)); R << east << north << up; - x[0] = matrixToMRPs(R); + const vec4_t q = matrixToQuat(R); - // NOTE: we could try to use the average of the gyro data - // to estimate the initial bias, but this only works if - // the device is not moving. For now, we don't use that value - // and start with a bias of 0. - x[1] = 0; - - // initial covariance - P = 0; + initFusion(q, mGyroRate); } return false; } void Fusion::handleGyro(const vec3_t& w, float dT) { - const vec3_t wdT(w * dT); // rad/s * s -> rad - if (!checkInitComplete(GYRO, wdT)) + if (!checkInitComplete(GYRO, w, dT)) return; - predict(wdT); + predict(w, dT); } status_t Fusion::handleAcc(const vec3_t& a) { - if (length(a) < 0.981f) + // ignore acceleration data if we're close to free-fall + if (length(a) < FREE_FALL_THRESHOLD) return BAD_VALUE; if (!checkInitComplete(ACC, a)) return BAD_VALUE; - // ignore acceleration data if we're close to free-fall const float l = 1/length(a); update(a*l, Ba, accSTDEV*l); return NO_ERROR; @@ -251,20 +234,6 @@ status_t Fusion::handleMag(const vec3_t& m) { const float l = 1 / length(north); north *= l; -#if 0 - // in practice the magnetic-field sensor is so wrong - // that there is no point trying to use it to constantly - // correct the gyro. instead, we use the mag-sensor only when - // the device points north (just to give us a reference). - // We're hoping that it'll actually point north, if it doesn't - // we'll be offset, but at least the instantaneous posture - // of the device will be correct. - - const float cos_30 = 0.8660254f; - if (dot_product(north, Bm) < cos_30) - return BAD_VALUE; -#endif - update(north, Bm, magSTDEV*l); return NO_ERROR; } @@ -273,7 +242,7 @@ bool Fusion::checkState(const vec3_t& v) { if (isnanf(length(v))) { LOGW("9-axis fusion diverged. reseting state."); P = 0; - x[1] = 0; + x1 = 0; mInitState = 0; mCount[0] = 0; mCount[1] = 0; @@ -284,145 +253,89 @@ bool Fusion::checkState(const vec3_t& v) { return true; } -vec3_t Fusion::getAttitude() const { - return x[0]; +vec4_t Fusion::getAttitude() const { + return x0; } vec3_t Fusion::getBias() const { - return x[1]; + return x1; } mat33_t Fusion::getRotationMatrix() const { - return MRPsToMatrix(x[0]); + return quatToMatrix(x0); } -mat33_t Fusion::getF(const vec3_t& p) { - const float p0 = p.x; - const float p1 = p.y; - const float p2 = p.z; - - // f(p, w) - const float p0p1 = p0*p1; - const float p0p2 = p0*p2; - const float p1p2 = p1*p2; - const float p0p0 = p0*p0; - const float p1p1 = p1*p1; - const float p2p2 = p2*p2; - const float pp = 0.5f * (1 - (p0p0 + p1p1 + p2p2)); - - mat33_t F; - F[0][0] = 0.5f*(p0p0 + pp); - F[0][1] = 0.5f*(p0p1 + p2); - F[0][2] = 0.5f*(p0p2 - p1); - F[1][0] = 0.5f*(p0p1 - p2); - F[1][1] = 0.5f*(p1p1 + pp); - F[1][2] = 0.5f*(p1p2 + p0); - F[2][0] = 0.5f*(p0p2 + p1); - F[2][1] = 0.5f*(p1p2 - p0); - F[2][2] = 0.5f*(p2p2 + pp); +mat34_t Fusion::getF(const vec4_t& q) { + mat34_t F; + F[0].x = q.w; F[1].x =-q.z; F[2].x = q.y; + F[0].y = q.z; F[1].y = q.w; F[2].y =-q.x; + F[0].z =-q.y; F[1].z = q.x; F[2].z = q.w; + F[0].w =-q.x; F[1].w =-q.y; F[2].w =-q.z; return F; } -mat33_t Fusion::getdFdp(const vec3_t& p, const vec3_t& we) { +void Fusion::predict(const vec3_t& w, float dT) { + const vec4_t q = x0; + const vec3_t b = x1; + const vec3_t we = w - b; + const vec4_t dq = getF(q)*((0.5f*dT)*we); + x0 = normalize_quat(q + dq); - // dF = | A = df/dp -F | - // | 0 0 | + // P(k+1) = F*P(k)*Ft + G*Q*Gt - mat33_t A; - A[0][0] = A[1][1] = A[2][2] = 0.5f * (p.x*we.x + p.y*we.y + p.z*we.z); - A[0][1] = 0.5f * (p.y*we.x - p.x*we.y - we.z); - A[0][2] = 0.5f * (p.z*we.x - p.x*we.z + we.y); - A[1][2] = 0.5f * (p.z*we.y - p.y*we.z - we.x); - A[1][0] = -A[0][1]; - A[2][0] = -A[0][2]; - A[2][1] = -A[1][2]; - return A; -} + // Phi = | Phi00 Phi10 | + // | 0 1 | + const mat33_t I33(1); + const mat33_t I33dT(dT); + const mat33_t wx(crossMatrix(we, 0)); + const mat33_t wx2(wx*wx); + const float lwedT = length(we)*dT; + const float ilwe = 1/length(we); + const float k0 = (1-cosf(lwedT))*(ilwe*ilwe); + const float k1 = sinf(lwedT); -void Fusion::predict(const vec3_t& w) { - // f(p, w) - vec3_t& p(x[0]); + Phi[0][0] = I33 - wx*(k1*ilwe) + wx2*k0; + Phi[1][0] = wx*k0 - I33dT - wx2*(ilwe*ilwe*ilwe)*(lwedT-k1); - // There is a discontinuity at 2.pi, to avoid it we need to switch to - // the shadow of p when pT.p gets too big. - const float ptp(dot_product(p,p)); - if (ptp >= 2.0f) { - p = -p * (1/ptp); - } - - const mat33_t F(getF(p)); - - // compute w with the bias correction: - // w_estimated = w - b_estimated - const vec3_t& b(x[1]); - const vec3_t we(w - b); - - // prediction - const vec3_t dX(F*we); - - if (!checkState(dX)) - return; - - p += dX; - - const mat33_t A(getdFdp(p, we)); - - // G = | G0 0 | = | -F 0 | - // | 0 1 | | 0 1 | - - // P += A*P + P*At + F*Q*Ft - const mat33_t AP(A*transpose(P[0][0])); - const mat33_t PAt(P[0][0]*transpose(A)); - const mat33_t FPSt(F*transpose(P[1][0])); - const mat33_t PSFt(P[1][0]*transpose(F)); - const mat33_t FQFt(scaleCovariance(F, Q[0])); - P[0][0] += AP + PAt - FPSt - PSFt + FQFt; - P[1][0] += A*P[1][0] - F*P[1][1]; - P[1][1] += Q[1]; + P = Phi*P*transpose(Phi) + GQGt; } void Fusion::update(const vec3_t& z, const vec3_t& Bi, float sigma) { - const vec3_t p(x[0]); + vec4_t q(x0); // measured vector in body space: h(p) = A(p)*Bi - const mat33_t A(MRPsToMatrix(p)); + const mat33_t A(quatToMatrix(q)); const vec3_t Bb(A*Bi); // Sensitivity matrix H = dh(p)/dp // H = [ L 0 ] - const float ptp(dot_product(p,p)); - const mat33_t px(crossMatrix(p, 0.5f*(ptp-1))); - const mat33_t ppt(p*transpose(p)); - const mat33_t L((8 / sqr(1+ptp))*crossMatrix(Bb, 0)*(ppt-px)); + const mat33_t L(crossMatrix(Bb, 0)); - // update... + // gain... + // K = P*Ht / [H*P*Ht + R] + vec K; const mat33_t R(sigma*sigma); const mat33_t S(scaleCovariance(L, P[0][0]) + R); const mat33_t Si(invert(S)); const mat33_t LtSi(transpose(L)*Si); - - vec K; K[0] = P[0][0] * LtSi; K[1] = transpose(P[1][0])*LtSi; - const vec3_t e(z - Bb); - const vec3_t K0e(K[0]*e); - const vec3_t K1e(K[1]*e); - - if (!checkState(K0e)) - return; - - if (!checkState(K1e)) - return; - - x[0] += K0e; - x[1] += K1e; - + // update... // P -= K*H*P; const mat33_t K0L(K[0] * L); const mat33_t K1L(K[1] * L); P[0][0] -= K0L*P[0][0]; P[1][1] -= K1L*P[1][0]; P[1][0] -= K0L*P[1][0]; + P[0][1] = transpose(P[1][0]); + + const vec3_t e(z - Bb); + const vec3_t dq(K[0]*e); + const vec3_t db(K[1]*e); + + q += getF(q)*(0.5f*dq); + x0 = normalize_quat(q); + x1 += db; } // ----------------------------------------------------------------------- diff --git a/services/sensorservice/Fusion.h b/services/sensorservice/Fusion.h index 571a41527..556944bbe 100644 --- a/services/sensorservice/Fusion.h +++ b/services/sensorservice/Fusion.h @@ -19,42 +19,39 @@ #include -#include "vec.h" +#include "quat.h" #include "mat.h" +#include "vec.h" namespace android { +typedef mat mat34_t; + class Fusion { /* * the state vector is made of two sub-vector containing respectively: * - modified Rodrigues parameters * - the estimated gyro bias */ - vec x; + quat_t x0; + vec3_t x1; /* * the predicated covariance matrix is made of 4 3x3 sub-matrices and it * semi-definite positive. * * P = | P00 P10 | = | P00 P10 | - * | P01 P11 | | P10t Q1 | + * | P01 P11 | | P10t P11 | * * Since P01 = transpose(P10), the code below never calculates or - * stores P01. P11 is always equal to Q1, so we don't store it either. + * stores P01. */ mat P; /* - * the process noise covariance matrix is made of 2 3x3 sub-matrices - * Q0 encodes the attitude's noise - * Q1 encodes the bias' noise + * the process noise covariance matrix */ - vec Q; - - static const float gyroSTDEV = 1.0e-5; // rad/s (measured 1.2e-5) - static const float accSTDEV = 0.05f; // m/s^2 (measured 0.08 / CDD 0.05) - static const float magSTDEV = 0.5f; // uT (measured 0.7 / CDD 0.5) - static const float biasSTDEV = 2e-9; // rad/s^2 (guessed) + mat GQGt; public: Fusion(); @@ -62,23 +59,25 @@ public: void handleGyro(const vec3_t& w, float dT); status_t handleAcc(const vec3_t& a); status_t handleMag(const vec3_t& m); - vec3_t getAttitude() const; + vec4_t getAttitude() const; vec3_t getBias() const; mat33_t getRotationMatrix() const; bool hasEstimate() const; private: + mat Phi; vec3_t Ba, Bm; uint32_t mInitState; + float mGyroRate; vec mData; size_t mCount[3]; enum { ACC=0x1, MAG=0x2, GYRO=0x4 }; - bool checkInitComplete(int, const vec3_t&); + bool checkInitComplete(int, const vec3_t& w, float d = 0); + void initFusion(const vec4_t& q0, float dT); bool checkState(const vec3_t& v); - void predict(const vec3_t& w); + void predict(const vec3_t& w, float dT); void update(const vec3_t& z, const vec3_t& Bi, float sigma); - static mat33_t getF(const vec3_t& p); - static mat33_t getdFdp(const vec3_t& p, const vec3_t& we); + static mat34_t getF(const vec4_t& p); }; }; // namespace android diff --git a/services/sensorservice/GravitySensor.cpp b/services/sensorservice/GravitySensor.cpp index 541fad26f..c57715f0d 100644 --- a/services/sensorservice/GravitySensor.cpp +++ b/services/sensorservice/GravitySensor.cpp @@ -31,10 +31,7 @@ namespace android { GravitySensor::GravitySensor(sensor_t const* list, size_t count) : mSensorDevice(SensorDevice::getInstance()), - mSensorFusion(SensorFusion::getInstance()), - mAccTime(0), - mLowPass(M_SQRT1_2, 1.5f), - mX(mLowPass), mY(mLowPass), mZ(mLowPass) + mSensorFusion(SensorFusion::getInstance()) { for (size_t i=0 ; idata[0] = g.x; outEvent->data[1] = g.y; @@ -86,42 +67,24 @@ bool GravitySensor::process(sensors_event_t* outEvent, } status_t GravitySensor::activate(void* ident, bool enabled) { - status_t err; - if (mSensorFusion.hasGyro()) { - err = mSensorFusion.activate(this, enabled); - } else { - err = mSensorDevice.activate(this, mAccelerometer.getHandle(), enabled); - if (err == NO_ERROR) { - if (enabled) { - mAccTime = 0; - } - } - } - return err; + return mSensorFusion.activate(this, enabled); } -status_t GravitySensor::setDelay(void* ident, int handle, int64_t ns) -{ - if (mSensorFusion.hasGyro()) { - return mSensorFusion.setDelay(this, ns); - } else { - return mSensorDevice.setDelay(this, mAccelerometer.getHandle(), ns); - } +status_t GravitySensor::setDelay(void* ident, int handle, int64_t ns) { + return mSensorFusion.setDelay(this, ns); } Sensor GravitySensor::getSensor() const { sensor_t hwSensor; hwSensor.name = "Gravity Sensor"; hwSensor.vendor = "Google Inc."; - hwSensor.version = mSensorFusion.hasGyro() ? 3 : 2; + hwSensor.version = 3; hwSensor.handle = '_grv'; hwSensor.type = SENSOR_TYPE_GRAVITY; hwSensor.maxRange = GRAVITY_EARTH * 2; hwSensor.resolution = mAccelerometer.getResolution(); - hwSensor.power = mSensorFusion.hasGyro() ? - mSensorFusion.getPowerUsage() : mAccelerometer.getPowerUsage(); - hwSensor.minDelay = mSensorFusion.hasGyro() ? - mSensorFusion.getMinDelay() : mAccelerometer.getMinDelay(); + hwSensor.power = mSensorFusion.getPowerUsage(); + hwSensor.minDelay = mSensorFusion.getMinDelay(); Sensor sensor(&hwSensor); return sensor; } diff --git a/services/sensorservice/GravitySensor.h b/services/sensorservice/GravitySensor.h index 0ca3a3c0e..ac177c465 100644 --- a/services/sensorservice/GravitySensor.h +++ b/services/sensorservice/GravitySensor.h @@ -23,7 +23,6 @@ #include #include "SensorInterface.h" -#include "SecondOrderLowPassFilter.h" // --------------------------------------------------------------------------- namespace android { @@ -36,10 +35,6 @@ class GravitySensor : public SensorInterface { SensorDevice& mSensorDevice; SensorFusion& mSensorFusion; Sensor mAccelerometer; - double mAccTime; - - SecondOrderLowPassFilter mLowPass; - CascadedBiquadFilter mX, mY, mZ; public: GravitySensor(sensor_t const* list, size_t count); diff --git a/services/sensorservice/OrientationSensor.cpp b/services/sensorservice/OrientationSensor.cpp index c9e50803f..037adaa2e 100644 --- a/services/sensorservice/OrientationSensor.cpp +++ b/services/sensorservice/OrientationSensor.cpp @@ -50,9 +50,10 @@ bool OrientationSensor::process(sensors_event_t* outEvent, g[0] += 360; *outEvent = event; - outEvent->data[0] = g.x; - outEvent->data[1] = g.y; - outEvent->data[2] = g.z; + outEvent->orientation.azimuth = g.x; + outEvent->orientation.pitch = g.y; + outEvent->orientation.roll = g.z; + outEvent->orientation.status = SENSOR_STATUS_ACCURACY_HIGH; outEvent->sensor = '_ypr'; outEvent->type = SENSOR_TYPE_ORIENTATION; return true; diff --git a/services/sensorservice/RotationVectorSensor.cpp b/services/sensorservice/RotationVectorSensor.cpp index cba89c9fc..5ea95683f 100644 --- a/services/sensorservice/RotationVectorSensor.cpp +++ b/services/sensorservice/RotationVectorSensor.cpp @@ -27,11 +27,6 @@ namespace android { // --------------------------------------------------------------------------- -template -static inline T clamp(T v) { - return v < 0 ? 0 : v; -} - RotationVectorSensor::RotationVectorSensor() : mSensorDevice(SensorDevice::getInstance()), mSensorFusion(SensorFusion::getInstance()) @@ -43,29 +38,12 @@ bool RotationVectorSensor::process(sensors_event_t* outEvent, { if (event.type == SENSOR_TYPE_ACCELEROMETER) { if (mSensorFusion.hasEstimate()) { - const mat33_t R(mSensorFusion.getRotationMatrix()); - - // matrix to rotation vector (normalized quaternion) - const float Hx = R[0].x; - const float My = R[1].y; - const float Az = R[2].z; - - float qw = sqrtf( clamp( Hx + My + Az + 1) * 0.25f ); - float qx = sqrtf( clamp( Hx - My - Az + 1) * 0.25f ); - float qy = sqrtf( clamp(-Hx + My - Az + 1) * 0.25f ); - float qz = sqrtf( clamp(-Hx - My + Az + 1) * 0.25f ); - qx = copysignf(qx, R[2].y - R[1].z); - qy = copysignf(qy, R[0].z - R[2].x); - qz = copysignf(qz, R[1].x - R[0].y); - - // this quaternion is guaranteed to be normalized, by construction - // of the rotation matrix. - + const vec4_t q(mSensorFusion.getAttitude()); *outEvent = event; - outEvent->data[0] = qx; - outEvent->data[1] = qy; - outEvent->data[2] = qz; - outEvent->data[3] = qw; + outEvent->data[0] = q.x; + outEvent->data[1] = q.y; + outEvent->data[2] = q.z; + outEvent->data[3] = q.w; outEvent->sensor = '_rov'; outEvent->type = SENSOR_TYPE_ROTATION_VECTOR; return true; @@ -86,7 +64,7 @@ Sensor RotationVectorSensor::getSensor() const { sensor_t hwSensor; hwSensor.name = "Rotation Vector Sensor"; hwSensor.vendor = "Google Inc."; - hwSensor.version = mSensorFusion.hasGyro() ? 3 : 2; + hwSensor.version = 3; hwSensor.handle = '_rov'; hwSensor.type = SENSOR_TYPE_ROTATION_VECTOR; hwSensor.maxRange = 1; @@ -97,6 +75,55 @@ Sensor RotationVectorSensor::getSensor() const { return sensor; } +// --------------------------------------------------------------------------- + +GyroDriftSensor::GyroDriftSensor() + : mSensorDevice(SensorDevice::getInstance()), + mSensorFusion(SensorFusion::getInstance()) +{ +} + +bool GyroDriftSensor::process(sensors_event_t* outEvent, + const sensors_event_t& event) +{ + if (event.type == SENSOR_TYPE_ACCELEROMETER) { + if (mSensorFusion.hasEstimate()) { + const vec3_t b(mSensorFusion.getGyroBias()); + *outEvent = event; + outEvent->data[0] = b.x; + outEvent->data[1] = b.y; + outEvent->data[2] = b.z; + outEvent->sensor = '_gbs'; + outEvent->type = SENSOR_TYPE_ACCELEROMETER; + return true; + } + } + return false; +} + +status_t GyroDriftSensor::activate(void* ident, bool enabled) { + return mSensorFusion.activate(this, enabled); +} + +status_t GyroDriftSensor::setDelay(void* ident, int handle, int64_t ns) { + return mSensorFusion.setDelay(this, ns); +} + +Sensor GyroDriftSensor::getSensor() const { + sensor_t hwSensor; + hwSensor.name = "Gyroscope Bias (debug)"; + hwSensor.vendor = "Google Inc."; + hwSensor.version = 1; + hwSensor.handle = '_gbs'; + hwSensor.type = SENSOR_TYPE_ACCELEROMETER; + hwSensor.maxRange = 1; + hwSensor.resolution = 1.0f / (1<<24); + hwSensor.power = mSensorFusion.getPowerUsage(); + hwSensor.minDelay = mSensorFusion.getMinDelay(); + Sensor sensor(&hwSensor); + return sensor; +} + // --------------------------------------------------------------------------- }; // namespace android diff --git a/services/sensorservice/RotationVectorSensor.h b/services/sensorservice/RotationVectorSensor.h index ac76487e4..bb97fe189 100644 --- a/services/sensorservice/RotationVectorSensor.h +++ b/services/sensorservice/RotationVectorSensor.h @@ -24,7 +24,6 @@ #include "SensorDevice.h" #include "SensorInterface.h" -#include "SecondOrderLowPassFilter.h" #include "Fusion.h" #include "SensorFusion.h" @@ -47,6 +46,20 @@ public: virtual bool isVirtual() const { return true; } }; +class GyroDriftSensor : public SensorInterface { + SensorDevice& mSensorDevice; + SensorFusion& mSensorFusion; + +public: + GyroDriftSensor(); + virtual bool process(sensors_event_t* outEvent, + const sensors_event_t& event); + virtual status_t activate(void* ident, bool enabled); + virtual status_t setDelay(void* ident, int handle, int64_t ns); + virtual Sensor getSensor() const; + virtual bool isVirtual() const { return true; } +}; + // --------------------------------------------------------------------------- }; // namespace android diff --git a/services/sensorservice/SecondOrderLowPassFilter.cpp b/services/sensorservice/SecondOrderLowPassFilter.cpp deleted file mode 100644 index c76dd4cff..000000000 --- a/services/sensorservice/SecondOrderLowPassFilter.cpp +++ /dev/null @@ -1,103 +0,0 @@ -/* - * Copyright (C) 2010 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include -#include -#include - -#include - -#include "SecondOrderLowPassFilter.h" -#include "vec.h" - -// --------------------------------------------------------------------------- - -namespace android { -// --------------------------------------------------------------------------- - -SecondOrderLowPassFilter::SecondOrderLowPassFilter(float Q, float fc) - : iQ(1.0f / Q), fc(fc) -{ -} - -void SecondOrderLowPassFilter::setSamplingPeriod(float dT) -{ - K = tanf(float(M_PI) * fc * dT); - iD = 1.0f / (K*K + K*iQ + 1); - a0 = K*K*iD; - a1 = 2.0f * a0; - b1 = 2.0f*(K*K - 1)*iD; - b2 = (K*K - K*iQ + 1)*iD; -} - -// --------------------------------------------------------------------------- - -template -BiquadFilter::BiquadFilter(const SecondOrderLowPassFilter& s) - : s(s) -{ -} - -template -T BiquadFilter::init(const T& x) -{ - x1 = x2 = x; - y1 = y2 = x; - return x; -} - -template -T BiquadFilter::operator()(const T& x) -{ - T y = (x + x2)*s.a0 + x1*s.a1 - y1*s.b1 - y2*s.b2; - x2 = x1; - y2 = y1; - x1 = x; - y1 = y; - return y; -} - -// --------------------------------------------------------------------------- - -template -CascadedBiquadFilter::CascadedBiquadFilter(const SecondOrderLowPassFilter& s) - : mA(s), mB(s) -{ -} - -template -T CascadedBiquadFilter::init(const T& x) -{ - mA.init(x); - mB.init(x); - return x; -} - -template -T CascadedBiquadFilter::operator()(const T& x) -{ - return mB(mA(x)); -} - -// --------------------------------------------------------------------------- - -template class BiquadFilter; -template class CascadedBiquadFilter; -template class BiquadFilter; -template class CascadedBiquadFilter; - -// --------------------------------------------------------------------------- -}; // namespace android diff --git a/services/sensorservice/SecondOrderLowPassFilter.h b/services/sensorservice/SecondOrderLowPassFilter.h deleted file mode 100644 index 0cc2446cc..000000000 --- a/services/sensorservice/SecondOrderLowPassFilter.h +++ /dev/null @@ -1,77 +0,0 @@ -/* - * Copyright (C) 2010 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef ANDROID_SECOND_ORDER_LOW_PASS_FILTER_H -#define ANDROID_SECOND_ORDER_LOW_PASS_FILTER_H - -#include -#include - -// --------------------------------------------------------------------------- - -namespace android { -// --------------------------------------------------------------------------- - -template -class BiquadFilter; - -/* - * State of a 2nd order low-pass IIR filter - */ -class SecondOrderLowPassFilter { - template - friend class BiquadFilter; - float iQ, fc; - float K, iD; - float a0, a1; - float b1, b2; -public: - SecondOrderLowPassFilter(float Q, float fc); - void setSamplingPeriod(float dT); -}; - -/* - * Implements a Biquad IIR filter - */ -template -class BiquadFilter { - T x1, x2; - T y1, y2; - const SecondOrderLowPassFilter& s; -public: - BiquadFilter(const SecondOrderLowPassFilter& s); - T init(const T& in); - T operator()(const T& in); -}; - -/* - * Two cascaded biquad IIR filters - * (4-poles IIR) - */ -template -class CascadedBiquadFilter { - BiquadFilter mA; - BiquadFilter mB; -public: - CascadedBiquadFilter(const SecondOrderLowPassFilter& s); - T init(const T& in); - T operator()(const T& in); -}; - -// --------------------------------------------------------------------------- -}; // namespace android - -#endif // ANDROID_SECOND_ORDER_LOW_PASS_FILTER_H diff --git a/services/sensorservice/SensorFusion.cpp b/services/sensorservice/SensorFusion.cpp index d4226ec11..4ec0c8cbd 100644 --- a/services/sensorservice/SensorFusion.cpp +++ b/services/sensorservice/SensorFusion.cpp @@ -25,9 +25,7 @@ ANDROID_SINGLETON_STATIC_INSTANCE(SensorFusion) SensorFusion::SensorFusion() : mSensorDevice(SensorDevice::getInstance()), - mEnabled(false), mHasGyro(false), mGyroTime(0), mRotationMatrix(1), - mLowPass(M_SQRT1_2, 1.0f), mAccData(mLowPass), - mFilteredMag(0.0f), mFilteredAcc(0.0f) + mEnabled(false), mGyroTime(0) { sensor_t const* list; size_t count = mSensorDevice.getSensorList(&list); @@ -42,55 +40,32 @@ SensorFusion::SensorFusion() mGyro = Sensor(list + i); // 200 Hz for gyro events is a good compromise between precision // and power/cpu usage. - mTargetDelayNs = 1000000000LL/200; - mGyroRate = 1000000000.0f / mTargetDelayNs; - mHasGyro = true; + mGyroRate = 200; + mTargetDelayNs = 1000000000LL/mGyroRate; } } mFusion.init(); - mAccData.init(vec3_t(0.0f)); } void SensorFusion::process(const sensors_event_t& event) { - - if (event.type == SENSOR_TYPE_GYROSCOPE && mHasGyro) { + if (event.type == SENSOR_TYPE_GYROSCOPE) { if (mGyroTime != 0) { const float dT = (event.timestamp - mGyroTime) / 1000000000.0f; const float freq = 1 / dT; - const float alpha = 2 / (2 + dT); // 2s time-constant - mGyroRate = mGyroRate*alpha + freq*(1 - alpha); + if (freq >= 100 && freq<1000) { // filter values obviously wrong + const float alpha = 1 / (1 + dT); // 1s time-constant + mGyroRate = freq + (mGyroRate - freq)*alpha; + } } mGyroTime = event.timestamp; mFusion.handleGyro(vec3_t(event.data), 1.0f/mGyroRate); } else if (event.type == SENSOR_TYPE_MAGNETIC_FIELD) { const vec3_t mag(event.data); - if (mHasGyro) { - mFusion.handleMag(mag); - } else { - const float l(length(mag)); - if (l>5 && l<100) { - mFilteredMag = mag * (1/l); - } - } + mFusion.handleMag(mag); } else if (event.type == SENSOR_TYPE_ACCELEROMETER) { const vec3_t acc(event.data); - if (mHasGyro) { - mFusion.handleAcc(acc); - mRotationMatrix = mFusion.getRotationMatrix(); - } else { - const float l(length(acc)); - if (l > 0.981f) { - // remove the linear-acceleration components - mFilteredAcc = mAccData(acc * (1/l)); - } - if (length(mFilteredAcc)>0 && length(mFilteredMag)>0) { - vec3_t up(mFilteredAcc); - vec3_t east(cross_product(mFilteredMag, up)); - east *= 1/length(east); - vec3_t north(cross_product(up, east)); - mRotationMatrix << east << north << up; - } - } + mFusion.handleAcc(acc); + mAttitude = mFusion.getAttitude(); } } @@ -116,40 +91,31 @@ status_t SensorFusion::activate(void* ident, bool enabled) { mSensorDevice.activate(ident, mAcc.getHandle(), enabled); mSensorDevice.activate(ident, mMag.getHandle(), enabled); - if (mHasGyro) { - mSensorDevice.activate(ident, mGyro.getHandle(), enabled); - } + mSensorDevice.activate(ident, mGyro.getHandle(), enabled); const bool newState = mClients.size() != 0; if (newState != mEnabled) { mEnabled = newState; if (newState) { mFusion.init(); + mGyroTime = 0; } } return NO_ERROR; } status_t SensorFusion::setDelay(void* ident, int64_t ns) { - if (mHasGyro) { - mSensorDevice.setDelay(ident, mAcc.getHandle(), ns); - mSensorDevice.setDelay(ident, mMag.getHandle(), ms2ns(20)); - mSensorDevice.setDelay(ident, mGyro.getHandle(), mTargetDelayNs); - } else { - const static double NS2S = 1.0 / 1000000000.0; - mSensorDevice.setDelay(ident, mAcc.getHandle(), ns); - mSensorDevice.setDelay(ident, mMag.getHandle(), max(ns, mMag.getMinDelayNs())); - mLowPass.setSamplingPeriod(ns*NS2S); - } + mSensorDevice.setDelay(ident, mAcc.getHandle(), ns); + mSensorDevice.setDelay(ident, mMag.getHandle(), ms2ns(20)); + mSensorDevice.setDelay(ident, mGyro.getHandle(), mTargetDelayNs); return NO_ERROR; } float SensorFusion::getPowerUsage() const { - float power = mAcc.getPowerUsage() + mMag.getPowerUsage(); - if (mHasGyro) { - power += mGyro.getPowerUsage(); - } + float power = mAcc.getPowerUsage() + + mMag.getPowerUsage() + + mGyro.getPowerUsage(); return power; } @@ -159,17 +125,17 @@ int32_t SensorFusion::getMinDelay() const { void SensorFusion::dump(String8& result, char* buffer, size_t SIZE) { const Fusion& fusion(mFusion); - snprintf(buffer, SIZE, "Fusion (%s) %s (%d clients), gyro-rate=%7.2fHz, " - "MRPS=< %g, %g, %g > (%g), " - "BIAS=< %g, %g, %g >\n", - mHasGyro ? "9-axis" : "6-axis", + snprintf(buffer, SIZE, "9-axis fusion %s (%d clients), gyro-rate=%7.2fHz, " + "q=< %g, %g, %g, %g > (%g), " + "b=< %g, %g, %g >\n", mEnabled ? "enabled" : "disabled", mClients.size(), mGyroRate, fusion.getAttitude().x, fusion.getAttitude().y, fusion.getAttitude().z, - dot_product(fusion.getAttitude(), fusion.getAttitude()), + fusion.getAttitude().w, + length(fusion.getAttitude()), fusion.getBias().x, fusion.getBias().y, fusion.getBias().z); diff --git a/services/sensorservice/SensorFusion.h b/services/sensorservice/SensorFusion.h index c7eab1262..4c99bcb75 100644 --- a/services/sensorservice/SensorFusion.h +++ b/services/sensorservice/SensorFusion.h @@ -27,7 +27,6 @@ #include #include "Fusion.h" -#include "SecondOrderLowPassFilter.h" // --------------------------------------------------------------------------- @@ -45,15 +44,10 @@ class SensorFusion : public Singleton { Sensor mGyro; Fusion mFusion; bool mEnabled; - bool mHasGyro; float mGyroRate; nsecs_t mTargetDelayNs; nsecs_t mGyroTime; - mat33_t mRotationMatrix; - SecondOrderLowPassFilter mLowPass; - BiquadFilter mAccData; - vec3_t mFilteredMag; - vec3_t mFilteredAcc; + vec4_t mAttitude; SortedVector mClients; SensorFusion(); @@ -62,9 +56,9 @@ public: void process(const sensors_event_t& event); bool isEnabled() const { return mEnabled; } - bool hasGyro() const { return mHasGyro; } - bool hasEstimate() const { return !mHasGyro || mFusion.hasEstimate(); } - mat33_t getRotationMatrix() const { return mRotationMatrix; } + bool hasEstimate() const { return mFusion.hasEstimate(); } + mat33_t getRotationMatrix() const { return mFusion.getRotationMatrix(); } + vec4_t getAttitude() const { return mAttitude; } vec3_t getGyroBias() const { return mFusion.getBias(); } float getEstimatedRate() const { return mGyroRate; } diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp index 5b86d10d0..d1b10f799 100644 --- a/services/sensorservice/SensorService.cpp +++ b/services/sensorservice/SensorService.cpp @@ -18,6 +18,8 @@ #include #include +#include + #include #include #include @@ -46,6 +48,16 @@ namespace android { // --------------------------------------------------------------------------- +/* + * Notes: + * + * - what about a gyro-corrected magnetic-field sensor? + * - option to "hide" the HAL sensors + * - run mag sensor from time to time to force calibration + * - gravity sensor length is wrong (=> drift in linear-acc sensor) + * + */ + SensorService::SensorService() : mDump("android.permission.DUMP"), mInitCheck(NO_INIT) @@ -59,6 +71,7 @@ void SensorService::onFirstRef() SensorDevice& dev(SensorDevice::getInstance()); if (dev.initCheck() == NO_ERROR) { + bool hasGyro = false; uint32_t virtualSensorsNeeds = (1< + +#include "vec.h" +#include "mat.h" + +// ----------------------------------------------------------------------- +namespace android { +// ----------------------------------------------------------------------- + +template +mat quatToMatrix(const vec& q) { + mat R; + TYPE q0(q.w); + TYPE q1(q.x); + TYPE q2(q.y); + TYPE q3(q.z); + TYPE sq_q1 = 2 * q1 * q1; + TYPE sq_q2 = 2 * q2 * q2; + TYPE sq_q3 = 2 * q3 * q3; + TYPE q1_q2 = 2 * q1 * q2; + TYPE q3_q0 = 2 * q3 * q0; + TYPE q1_q3 = 2 * q1 * q3; + TYPE q2_q0 = 2 * q2 * q0; + TYPE q2_q3 = 2 * q2 * q3; + TYPE q1_q0 = 2 * q1 * q0; + R[0][0] = 1 - sq_q2 - sq_q3; + R[0][1] = q1_q2 - q3_q0; + R[0][2] = q1_q3 + q2_q0; + R[1][0] = q1_q2 + q3_q0; + R[1][1] = 1 - sq_q1 - sq_q3; + R[1][2] = q2_q3 - q1_q0; + R[2][0] = q1_q3 - q2_q0; + R[2][1] = q2_q3 + q1_q0; + R[2][2] = 1 - sq_q1 - sq_q2; + return R; +} + +template +vec matrixToQuat(const mat& R) { + // matrix to quaternion + + struct { + inline TYPE operator()(TYPE v) { + return v < 0 ? 0 : v; + } + } clamp; + + vec q; + const float Hx = R[0].x; + const float My = R[1].y; + const float Az = R[2].z; + q.x = sqrtf( clamp( Hx - My - Az + 1) * 0.25f ); + q.y = sqrtf( clamp(-Hx + My - Az + 1) * 0.25f ); + q.z = sqrtf( clamp(-Hx - My + Az + 1) * 0.25f ); + q.w = sqrtf( clamp( Hx + My + Az + 1) * 0.25f ); + q.x = copysignf(q.x, R[2].y - R[1].z); + q.y = copysignf(q.y, R[0].z - R[2].x); + q.z = copysignf(q.z, R[1].x - R[0].y); + // guaranteed to be unit-quaternion + return q; +} + +template +vec normalize_quat(const vec& q) { + vec r(q); + if (r.w < 0) { + r = -r; + } + return normalize(r); +} + +// ----------------------------------------------------------------------- + +typedef vec4_t quat_t; + +// ----------------------------------------------------------------------- +}; // namespace android + +#endif /* ANDROID_QUAT_H */ diff --git a/services/sensorservice/vec.h b/services/sensorservice/vec.h index 736ff37b9..f74ccc579 100644 --- a/services/sensorservice/vec.h +++ b/services/sensorservice/vec.h @@ -207,6 +207,15 @@ TYPE PURE length(const V& v) { return sqrt(dot_product(v, v)); } +template < + template class V, + typename TYPE, + size_t SIZE +> +V PURE normalize(const V& v) { + return v * (1/length(v)); +} + template < template class VLHS, template class VRHS, From 010e42230135815907e76e5d7e5f30edf9e1799d Mon Sep 17 00:00:00 2001 From: Mathias Agopian Date: Wed, 8 Jun 2011 20:06:50 -0700 Subject: [PATCH 27/51] improve orientation sensor with gyro data when we do our own sensor fusion, we also export an improved orientation sensor and hide the HAL sensor. The fused orientation sensor is much more precise, fast and smooth. Change-Id: I0ea843b47ad9d12f6b22cce51f8629852d423126 --- services/sensorservice/SensorService.cpp | 23 +++++++++++++++++++---- services/sensorservice/SensorService.h | 1 + 2 files changed, 20 insertions(+), 4 deletions(-) diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp index d1b10f799..0ae79294f 100644 --- a/services/sensorservice/SensorService.cpp +++ b/services/sensorservice/SensorService.cpp @@ -52,7 +52,6 @@ namespace android { * Notes: * * - what about a gyro-corrected magnetic-field sensor? - * - option to "hide" the HAL sensors * - run mag sensor from time to time to force calibration * - gravity sensor length is wrong (=> drift in linear-acc sensor) * @@ -71,6 +70,7 @@ void SensorService::onFirstRef() SensorDevice& dev(SensorDevice::getInstance()); if (dev.initCheck() == NO_ERROR) { + ssize_t orientationIndex = -1; bool hasGyro = false; uint32_t virtualSensorsNeeds = (1<= 0) { + mUserSensorList.removeItemsAt(orientationIndex); + } + } + run("SensorService", PRIORITY_URGENT_DISPLAY); mInitCheck = NO_ERROR; } @@ -315,9 +330,9 @@ SensorService::getActiveVirtualSensors() const } String8 SensorService::getSensorName(int handle) const { - size_t count = mSensorList.size(); + size_t count = mUserSensorList.size(); for (size_t i=0 ; i SensorService::getSensorList() { - return mSensorList; + return mUserSensorList; } sp SensorService::createSensorEventConnection() diff --git a/services/sensorservice/SensorService.h b/services/sensorservice/SensorService.h index 77a7e34e8..4d0f1d9d9 100644 --- a/services/sensorservice/SensorService.h +++ b/services/sensorservice/SensorService.h @@ -114,6 +114,7 @@ class SensorService : // constants Vector mSensorList; + Vector mUserSensorList; DefaultKeyedVector mSensorMap; Vector mVirtualSensorList; Permission mDump; From eaf2d0bfe37415ba1e42a97608823e8dbef53220 Mon Sep 17 00:00:00 2001 From: Mathias Agopian Date: Mon, 13 Jun 2011 16:00:49 -0700 Subject: [PATCH 28/51] cleanup Kalman filter parameters, add/fix comments/units Change-Id: Iedcae7164af8f7ea0e048ea7c72d0f35d16d739f --- services/sensorservice/Fusion.cpp | 55 +++++++++++++++++++++++-------- 1 file changed, 42 insertions(+), 13 deletions(-) diff --git a/services/sensorservice/Fusion.cpp b/services/sensorservice/Fusion.cpp index b5f97e035..d706af598 100644 --- a/services/sensorservice/Fusion.cpp +++ b/services/sensorservice/Fusion.cpp @@ -24,10 +24,28 @@ namespace android { // ----------------------------------------------------------------------- -static const float gyroSTDEV = 3.16e-4; // rad/s^3/2 +/* + * gyroVAR gives the measured variance of the gyro's output per + * Hz (or variance at 1 Hz). This is an "intrinsic" parameter of the gyro, + * which is independent of the sampling frequency. + * + * The variance of gyro's output at a given sampling period can be + * calculated as: + * variance(T) = gyroVAR / T + * + * The variance of the INTEGRATED OUTPUT at a given sampling period can be + * calculated as: + * variance_integrate_output(T) = gyroVAR * T + * + */ +static const float gyroVAR = 1e-7; // (rad/s)^2 / Hz +static const float biasVAR = 1e-8; // (rad/s)^2 / s (guessed) + +/* + * Standard deviations of accelerometer and magnetometer + */ static const float accSTDEV = 0.05f; // m/s^2 (measured 0.08 / CDD 0.05) static const float magSTDEV = 0.5f; // uT (measured 0.7 / CDD 0.5) -static const float biasSTDEV = 3.16e-5; // rad/s^1/2 (guessed) static const float FREE_FALL_THRESHOLD = 0.981f; @@ -129,23 +147,34 @@ void Fusion::initFusion(const vec4_t& q, float dT) x0 = q; x1 = 0; - // process noise covariance matrix - // G = | -1 0 | - // | 0 1 | + // process noise covariance matrix: G.Q.Gt, with + // + // G = | -1 0 | Q = | q00 q10 | + // | 0 1 | | q01 q11 | + // + // q00 = sv^2.dt + 1/3.su^2.dt^3 + // q10 = q01 = 1/2.su^2.dt^2 + // q11 = su^2.dt + // - const float v = gyroSTDEV; - const float u = biasSTDEV; - const float q00 = v*v*dT + 0.33333f*(dT*dT*dT)*u*u; - const float q10 = 0.5f*(dT*dT) *u*u; + // variance of integrated output at 1/dT Hz + // (random drift) + const float q00 = gyroVAR * dT; + + // variance of drift rate ramp + const float q11 = biasVAR * dT; + + const float u = q11 / dT; + const float q10 = 0.5f*u*dT*dT; const float q01 = q10; - const float q11 = u*u*dT; - GQGt[0][0] = q00; + + GQGt[0][0] = q00; // rad^2 GQGt[1][0] = -q10; GQGt[0][1] = -q01; - GQGt[1][1] = q11; - + GQGt[1][1] = q11; // (rad/s)^2 // initial covariance: Var{ x(t0) } + // TODO: initialize P correctly P = 0; } From 1cb13461a8cf62e3ba634e5965332f2d284f6d42 Mon Sep 17 00:00:00 2001 From: Mathias Agopian Date: Mon, 27 Jun 2011 16:05:52 -0700 Subject: [PATCH 29/51] PermissionCache caches permission checks This is intended to absorb the cost of the IPC to the permission controller. Cached permission checks cost about 3us, while full blown ones are two orders of magnitude slower. CAVEAT: PermissionCache can only handle system permissions safely for now, because the cache is not purged upon global permission changes. Change-Id: I8b8a5e71e191e3c01e8f792f253c379190eee62e --- services/sensorservice/SensorService.cpp | 8 +++++--- services/sensorservice/SensorService.h | 2 -- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp index 0ae79294f..64d214b54 100644 --- a/services/sensorservice/SensorService.cpp +++ b/services/sensorservice/SensorService.cpp @@ -31,6 +31,7 @@ #include #include +#include #include #include @@ -58,8 +59,7 @@ namespace android { */ SensorService::SensorService() - : mDump("android.permission.DUMP"), - mInitCheck(NO_INIT) + : mInitCheck(NO_INIT) { } @@ -166,12 +166,14 @@ SensorService::~SensorService() delete mSensorMap.valueAt(i); } +static const String16 sDump("android.permission.DUMP"); + status_t SensorService::dump(int fd, const Vector& args) { const size_t SIZE = 1024; char buffer[SIZE]; String8 result; - if (!mDump.checkCalling()) { + if (!PermissionCache::checkCallingPermission(sDump)) { snprintf(buffer, SIZE, "Permission Denial: " "can't dump SurfaceFlinger from pid=%d, uid=%d\n", IPCThreadState::self()->getCallingPid(), diff --git a/services/sensorservice/SensorService.h b/services/sensorservice/SensorService.h index 4d0f1d9d9..85f4ecbf9 100644 --- a/services/sensorservice/SensorService.h +++ b/services/sensorservice/SensorService.h @@ -27,7 +27,6 @@ #include #include -#include #include #include @@ -117,7 +116,6 @@ class SensorService : Vector mUserSensorList; DefaultKeyedVector mSensorMap; Vector mVirtualSensorList; - Permission mDump; status_t mInitCheck; // protected by mLock From 0be7a26220f786c068d6f5488d89ea4937c1df51 Mon Sep 17 00:00:00 2001 From: Jeff Brown Date: Mon, 11 Jul 2011 22:12:16 -0700 Subject: [PATCH 30/51] Remove the simulator target from all makefiles. Bug: 5010576 Change-Id: I04d722f258951a3078fe07899f5bbe8aac02a8e8 --- services/sensorservice/Android.mk | 7 ------- 1 file changed, 7 deletions(-) diff --git a/services/sensorservice/Android.mk b/services/sensorservice/Android.mk index ba3e6e5bf..6a302c061 100644 --- a/services/sensorservice/Android.mk +++ b/services/sensorservice/Android.mk @@ -16,13 +16,6 @@ LOCAL_SRC_FILES:= \ LOCAL_CFLAGS:= -DLOG_TAG=\"SensorService\" -# need "-lrt" on Linux simulator to pick up clock_gettime -ifeq ($(TARGET_SIMULATOR),true) - ifeq ($(HOST_OS),linux) - LOCAL_LDLIBS += -lrt -lpthread - endif -endif - LOCAL_SHARED_LIBRARIES := \ libcutils \ libhardware \ From 7b2b32f2e761a919deb6f82d978b379429f77b05 Mon Sep 17 00:00:00 2001 From: Mathias Agopian Date: Thu, 14 Jul 2011 16:39:46 -0700 Subject: [PATCH 31/51] sensorservice: be more robust when there are no sensor h/w Bug: 5030108 Change-Id: I45b85b3c492b9268cb0ae44d2e5fc8c708b6e66e --- services/sensorservice/SensorFusion.cpp | 32 ++++--- services/sensorservice/SensorService.cpp | 115 ++++++++++++----------- 2 files changed, 76 insertions(+), 71 deletions(-) diff --git a/services/sensorservice/SensorFusion.cpp b/services/sensorservice/SensorFusion.cpp index 4ec0c8cbd..518a1bb75 100644 --- a/services/sensorservice/SensorFusion.cpp +++ b/services/sensorservice/SensorFusion.cpp @@ -28,23 +28,25 @@ SensorFusion::SensorFusion() mEnabled(false), mGyroTime(0) { sensor_t const* list; - size_t count = mSensorDevice.getSensorList(&list); - for (size_t i=0 ; i 0) { + for (size_t i=0 ; i 0) { + ssize_t orientationIndex = -1; + bool hasGyro = false; + uint32_t virtualSensorsNeeds = + (1<= 0) { - mUserSensorList.removeItemsAt(orientationIndex); + // build the sensor list returned to users + mUserSensorList = mSensorList; + if (hasGyro && + (virtualSensorsNeeds & (1<= 0) { + mUserSensorList.removeItemsAt(orientationIndex); + } } - } - run("SensorService", PRIORITY_URGENT_DISPLAY); - mInitCheck = NO_ERROR; + run("SensorService", PRIORITY_URGENT_DISPLAY); + mInitCheck = NO_ERROR; + } } } From 16c3e4ae72e543f53a82f393dd287c11db8a7a80 Mon Sep 17 00:00:00 2001 From: Mathias Agopian Date: Wed, 20 Jul 2011 18:51:15 -0700 Subject: [PATCH 32/51] silence sensorservice when it's dropping events Change-Id: Ib05862e545aa780821aa605e45ab189f530494b7 --- services/sensorservice/SensorService.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp index e0dce1ff9..5b74fb809 100644 --- a/services/sensorservice/SensorService.cpp +++ b/services/sensorservice/SensorService.cpp @@ -586,12 +586,12 @@ status_t SensorService::SensorEventConnection::sendEvents( if (size == -EAGAIN) { // the destination doesn't accept events anymore, it's probably // full. For now, we just drop the events on the floor. - LOGW("dropping %d events on the floor", count); + //LOGW("dropping %d events on the floor", count); return size; } - LOGE_IF(size<0, "dropping %d events on the floor (%s)", - count, strerror(-size)); + //LOGE_IF(size<0, "dropping %d events on the floor (%s)", + // count, strerror(-size)); return size < 0 ? status_t(size) : status_t(NO_ERROR); } From 16bcf66afa333f8d3f3b835ed556e4fce8fa35bf Mon Sep 17 00:00:00 2001 From: Mathias Agopian Date: Mon, 15 Aug 2011 17:10:23 -0700 Subject: [PATCH 33/51] have the sensorsevice test print the delay of the first received event Change-Id: I89d63122574c3f8790f00512c76d59b463acf18f --- .../sensorservice/tests/sensorservicetest.cpp | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/services/sensorservice/tests/sensorservicetest.cpp b/services/sensorservice/tests/sensorservicetest.cpp index aea106224..54bce091f 100644 --- a/services/sensorservice/tests/sensorservicetest.cpp +++ b/services/sensorservice/tests/sensorservicetest.cpp @@ -22,6 +22,9 @@ using namespace android; +static nsecs_t sStartTime = 0; + + int receiver(int fd, int events, void* data) { sp q((SensorEventQueue*)data); @@ -32,7 +35,7 @@ int receiver(int fd, int events, void* data) while ((n = q->read(buffer, 8)) > 0) { for (int i=0 ; i\n", buffer[i].timestamp, buffer[i].acceleration.x, @@ -43,9 +46,11 @@ int receiver(int fd, int events, void* data) if (oldTimeStamp) { float t = float(buffer[i].timestamp - oldTimeStamp) / s2ns(1); printf("%f ms (%f Hz)\n", t*1000, 1.0/t); + } else { + float t = float(buffer[i].timestamp - sStartTime) / s2ns(1); + printf("first event: %f ms\n", t*1000); } oldTimeStamp = buffer[i].timestamp; - } } if (n<0 && n != -EAGAIN) { @@ -66,12 +71,15 @@ int main(int argc, char** argv) sp q = mgr.createEventQueue(); printf("queue=%p\n", q.get()); - Sensor const* accelerometer = mgr.getDefaultSensor(Sensor::TYPE_GYROSCOPE); + Sensor const* accelerometer = mgr.getDefaultSensor(Sensor::TYPE_ACCELEROMETER); printf("accelerometer=%p (%s)\n", accelerometer, accelerometer->getName().string()); + + sStartTime = systemTime(); + q->enableSensor(accelerometer); - q->setEventRate(accelerometer, ms2ns(10)); + q->setEventRate(accelerometer, ms2ns(200)); sp loop = new Looper(false); loop->addFd(q->getFd(), 0, ALOOPER_EVENT_INPUT, receiver, q.get()); From a01b4e237d57b74689576a3d486a2b2b903e74f4 Mon Sep 17 00:00:00 2001 From: Max Braun Date: Wed, 17 Aug 2011 18:22:52 -0700 Subject: [PATCH 34/51] Fix occasional fusion divergence by detecting it and resetting the fusion. Change-Id: I51186e12fb9b2316e3671e3908174f4495df89a0 --- services/sensorservice/Fusion.cpp | 27 ++++++++++++++++----------- services/sensorservice/Fusion.h | 4 ++-- services/sensorservice/mat.h | 23 +++++++++++++++++++++++ 3 files changed, 41 insertions(+), 13 deletions(-) diff --git a/services/sensorservice/Fusion.cpp b/services/sensorservice/Fusion.cpp index d706af598..ff4786bf0 100644 --- a/services/sensorservice/Fusion.cpp +++ b/services/sensorservice/Fusion.cpp @@ -48,6 +48,7 @@ static const float accSTDEV = 0.05f; // m/s^2 (measured 0.08 / CDD 0.05) static const float magSTDEV = 0.5f; // uT (measured 0.7 / CDD 0.5) static const float FREE_FALL_THRESHOLD = 0.981f; +static const float SYMMETRY_TOLERANCE = 1e-10f; // ----------------------------------------------------------------------- @@ -134,10 +135,13 @@ Fusion::Fusion() { void Fusion::init() { mInitState = 0; + mGyroRate = 0; + mCount[0] = 0; mCount[1] = 0; mCount[2] = 0; + mData = 0; } @@ -267,19 +271,16 @@ status_t Fusion::handleMag(const vec3_t& m) { return NO_ERROR; } -bool Fusion::checkState(const vec3_t& v) { - if (isnanf(length(v))) { - LOGW("9-axis fusion diverged. reseting state."); +void Fusion::checkState() { + // P needs to stay positive semidefinite or the fusion diverges. When we + // detect divergence, we reset the fusion. + // TODO(braun): Instead, find the reason for the divergence and fix it. + + if (!isPositiveSemidefinite(P[0][0], SYMMETRY_TOLERANCE) || + !isPositiveSemidefinite(P[1][1], SYMMETRY_TOLERANCE)) { + LOGW("Sensor fusion diverged; resetting state."); P = 0; - x1 = 0; - mInitState = 0; - mCount[0] = 0; - mCount[1] = 0; - mCount[2] = 0; - mData = 0; - return false; } - return true; } vec4_t Fusion::getAttitude() const { @@ -327,6 +328,8 @@ void Fusion::predict(const vec3_t& w, float dT) { Phi[1][0] = wx*k0 - I33dT - wx2*(ilwe*ilwe*ilwe)*(lwedT-k1); P = Phi*P*transpose(Phi) + GQGt; + + checkState(); } void Fusion::update(const vec3_t& z, const vec3_t& Bi, float sigma) { @@ -365,6 +368,8 @@ void Fusion::update(const vec3_t& z, const vec3_t& Bi, float sigma) { q += getF(q)*(0.5f*dq); x0 = normalize_quat(q); x1 += db; + + checkState(); } // ----------------------------------------------------------------------- diff --git a/services/sensorservice/Fusion.h b/services/sensorservice/Fusion.h index 556944bbe..7062999b7 100644 --- a/services/sensorservice/Fusion.h +++ b/services/sensorservice/Fusion.h @@ -37,7 +37,7 @@ class Fusion { vec3_t x1; /* - * the predicated covariance matrix is made of 4 3x3 sub-matrices and it + * the predicated covariance matrix is made of 4 3x3 sub-matrices and it is * semi-definite positive. * * P = | P00 P10 | = | P00 P10 | @@ -74,7 +74,7 @@ private: enum { ACC=0x1, MAG=0x2, GYRO=0x4 }; bool checkInitComplete(int, const vec3_t& w, float d = 0); void initFusion(const vec4_t& q0, float dT); - bool checkState(const vec3_t& v); + void checkState(); void predict(const vec3_t& w, float dT); void update(const vec3_t& z, const vec3_t& Bi, float sigma); static mat34_t getF(const vec4_t& p); diff --git a/services/sensorservice/mat.h b/services/sensorservice/mat.h index 1302ca327..a76fc91cb 100644 --- a/services/sensorservice/mat.h +++ b/services/sensorservice/mat.h @@ -295,6 +295,29 @@ mat PURE transpose(const mat& m) { return r; } +// Calculate the trace of a matrix +template static TYPE trace(const mat& m) { + TYPE t; + for (size_t i=0 ; i +static bool isPositiveSemidefinite(const mat& m, TYPE tolerance) { + for (size_t i=0 ; i tolerance) + return false; + + return true; +} + // Transpose a vector template < template class VEC, From 3e87d8dadefaf4b56bf15a15f1b53928d7a12cd2 Mon Sep 17 00:00:00 2001 From: Michael Johnson Date: Fri, 19 Aug 2011 11:47:08 -0700 Subject: [PATCH 35/51] More error checks to avoid div by zero. Change-Id: I18e5b72d02bf5420c14334d3a03f18fa40572d31 --- services/sensorservice/Fusion.cpp | 59 ++++++++++++++++++++++++++++--- services/sensorservice/vec.h | 9 +++++ 2 files changed, 64 insertions(+), 4 deletions(-) diff --git a/services/sensorservice/Fusion.cpp b/services/sensorservice/Fusion.cpp index ff4786bf0..e6ca2ccff 100644 --- a/services/sensorservice/Fusion.cpp +++ b/services/sensorservice/Fusion.cpp @@ -47,9 +47,41 @@ static const float biasVAR = 1e-8; // (rad/s)^2 / s (guessed) static const float accSTDEV = 0.05f; // m/s^2 (measured 0.08 / CDD 0.05) static const float magSTDEV = 0.5f; // uT (measured 0.7 / CDD 0.5) -static const float FREE_FALL_THRESHOLD = 0.981f; static const float SYMMETRY_TOLERANCE = 1e-10f; +/* + * Accelerometer updates will not be performed near free fall to avoid ill-conditioning and + * div by zeros. + * Threshhold: 10% of g, in m/s^2 + */ +static const float FREE_FALL_THRESHOLD = 0.981f; +static const float FREE_FALL_THRESHOLD_SQ = FREE_FALL_THRESHOLD*FREE_FALL_THRESHOLD; + +/* + * The geomagnetic-field should be between 30uT and 60uT. + * Fields strengths greater than this likely indicate a local magnetic disturbance which + * we do not want to update into the fused frame. + */ +static const float MAX_VALID_MAGNETIC_FIELD = 100; // uT +static const float MAX_VALID_MAGNETIC_FIELD_SQ = MAX_VALID_MAGNETIC_FIELD*MAX_VALID_MAGNETIC_FIELD; + +/* + * Values of the field smaller than this should be ignored in fusion to avoid ill-conditioning. + * This state can happen with anomalous local magnetic disturbances canceling the Earth field. + */ +static const float MIN_VALID_MAGNETIC_FIELD = 10; // uT +static const float MIN_VALID_MAGNETIC_FIELD_SQ = MIN_VALID_MAGNETIC_FIELD*MIN_VALID_MAGNETIC_FIELD; + +/* + * If the cross product of two vectors has magnitude squared less than this, we reject it as + * invalid due to alignment of the vectors. + * This threshold is used to check for the case where the magnetic field sample is parallel to + * the gravity field, which can happen in certain places due to magnetic field disturbances. + */ +static const float MIN_VALID_CROSS_PRODUCT_MAG = 1.0e-3; +static const float MIN_VALID_CROSS_PRODUCT_MAG_SQ = + MIN_VALID_CROSS_PRODUCT_MAG*MIN_VALID_CROSS_PRODUCT_MAG; + // ----------------------------------------------------------------------- template @@ -240,8 +272,10 @@ void Fusion::handleGyro(const vec3_t& w, float dT) { status_t Fusion::handleAcc(const vec3_t& a) { // ignore acceleration data if we're close to free-fall - if (length(a) < FREE_FALL_THRESHOLD) + if (length_squared(a) < FREE_FALL_THRESHOLD_SQ) { + LOGW("handleAcc: near free fall, not updating!"); return BAD_VALUE; + } if (!checkInitComplete(ACC, a)) return BAD_VALUE; @@ -253,15 +287,32 @@ status_t Fusion::handleAcc(const vec3_t& a) { status_t Fusion::handleMag(const vec3_t& m) { // the geomagnetic-field should be between 30uT and 60uT - // reject obviously wrong magnetic-fields - if (length(m) > 100) + // reject if too large to avoid spurious magnetic sources + const float magFieldSq = length_squared(m); + if (magFieldSq > MAX_VALID_MAGNETIC_FIELD_SQ) { + LOGW("handleMag: magnetic field too large, not updating!"); return BAD_VALUE; + } else if (magFieldSq < MIN_VALID_MAGNETIC_FIELD_SQ) { + // Also reject if too small since we will get ill-defined (zero mag) cross-products below + LOGW("handleMag: magnetic field too small, not updating!"); + return BAD_VALUE; + } if (!checkInitComplete(MAG, m)) return BAD_VALUE; + // Orthogonalize the magnetic field to the gravity field, mapping it into tangent to Earth. const vec3_t up( getRotationMatrix() * Ba ); const vec3_t east( cross_product(m, up) ); + + // If the m and up vectors align, the cross product magnitude will approach 0. + // Reject this case as well to avoid div by zero problems and ill-conditioning below. + if (length_squared(east) < MIN_VALID_CROSS_PRODUCT_MAG_SQ) { + LOGW("handleMag: magnetic field too aligned with up vector, not updating!"); + return BAD_VALUE; + } + + // If we have created an orthogonal magnetic field successfully, then pass it in as the update. vec3_t north( cross_product(up, east) ); const float l = 1 / length(north); diff --git a/services/sensorservice/vec.h b/services/sensorservice/vec.h index f74ccc579..24f30ff43 100644 --- a/services/sensorservice/vec.h +++ b/services/sensorservice/vec.h @@ -207,6 +207,15 @@ TYPE PURE length(const V& v) { return sqrt(dot_product(v, v)); } +template < + template class V, + typename TYPE, + size_t SIZE +> +TYPE PURE length_squared(const V& v) { + return dot_product(v, v); +} + template < template class V, typename TYPE, From a83f45c6c734084422f56733c25350625594bc00 Mon Sep 17 00:00:00 2001 From: Mathias Agopian Date: Wed, 24 Aug 2011 18:40:33 -0700 Subject: [PATCH 36/51] Fix a few style issues and remove LOG spam Change-Id: I6b6f75373f4ac28f98dea6a6f1c2567a6aa02243 --- services/sensorservice/Fusion.cpp | 50 +++++++++++++++++-------------- 1 file changed, 28 insertions(+), 22 deletions(-) diff --git a/services/sensorservice/Fusion.cpp b/services/sensorservice/Fusion.cpp index e6ca2ccff..0ab86c3e7 100644 --- a/services/sensorservice/Fusion.cpp +++ b/services/sensorservice/Fusion.cpp @@ -50,33 +50,38 @@ static const float magSTDEV = 0.5f; // uT (measured 0.7 / CDD 0.5) static const float SYMMETRY_TOLERANCE = 1e-10f; /* - * Accelerometer updates will not be performed near free fall to avoid ill-conditioning and - * div by zeros. + * Accelerometer updates will not be performed near free fall to avoid + * ill-conditioning and div by zeros. * Threshhold: 10% of g, in m/s^2 */ static const float FREE_FALL_THRESHOLD = 0.981f; -static const float FREE_FALL_THRESHOLD_SQ = FREE_FALL_THRESHOLD*FREE_FALL_THRESHOLD; +static const float FREE_FALL_THRESHOLD_SQ = + FREE_FALL_THRESHOLD*FREE_FALL_THRESHOLD; /* * The geomagnetic-field should be between 30uT and 60uT. - * Fields strengths greater than this likely indicate a local magnetic disturbance which - * we do not want to update into the fused frame. + * Fields strengths greater than this likely indicate a local magnetic + * disturbance which we do not want to update into the fused frame. */ static const float MAX_VALID_MAGNETIC_FIELD = 100; // uT -static const float MAX_VALID_MAGNETIC_FIELD_SQ = MAX_VALID_MAGNETIC_FIELD*MAX_VALID_MAGNETIC_FIELD; +static const float MAX_VALID_MAGNETIC_FIELD_SQ = + MAX_VALID_MAGNETIC_FIELD*MAX_VALID_MAGNETIC_FIELD; /* - * Values of the field smaller than this should be ignored in fusion to avoid ill-conditioning. - * This state can happen with anomalous local magnetic disturbances canceling the Earth field. + * Values of the field smaller than this should be ignored in fusion to avoid + * ill-conditioning. This state can happen with anomalous local magnetic + * disturbances canceling the Earth field. */ static const float MIN_VALID_MAGNETIC_FIELD = 10; // uT -static const float MIN_VALID_MAGNETIC_FIELD_SQ = MIN_VALID_MAGNETIC_FIELD*MIN_VALID_MAGNETIC_FIELD; +static const float MIN_VALID_MAGNETIC_FIELD_SQ = + MIN_VALID_MAGNETIC_FIELD*MIN_VALID_MAGNETIC_FIELD; /* - * If the cross product of two vectors has magnitude squared less than this, we reject it as - * invalid due to alignment of the vectors. - * This threshold is used to check for the case where the magnetic field sample is parallel to - * the gravity field, which can happen in certain places due to magnetic field disturbances. + * If the cross product of two vectors has magnitude squared less than this, + * we reject it as invalid due to alignment of the vectors. + * This threshold is used to check for the case where the magnetic field sample + * is parallel to the gravity field, which can happen in certain places due + * to magnetic field disturbances. */ static const float MIN_VALID_CROSS_PRODUCT_MAG = 1.0e-3; static const float MIN_VALID_CROSS_PRODUCT_MAG_SQ = @@ -273,7 +278,6 @@ void Fusion::handleGyro(const vec3_t& w, float dT) { status_t Fusion::handleAcc(const vec3_t& a) { // ignore acceleration data if we're close to free-fall if (length_squared(a) < FREE_FALL_THRESHOLD_SQ) { - LOGW("handleAcc: near free fall, not updating!"); return BAD_VALUE; } @@ -290,29 +294,31 @@ status_t Fusion::handleMag(const vec3_t& m) { // reject if too large to avoid spurious magnetic sources const float magFieldSq = length_squared(m); if (magFieldSq > MAX_VALID_MAGNETIC_FIELD_SQ) { - LOGW("handleMag: magnetic field too large, not updating!"); return BAD_VALUE; } else if (magFieldSq < MIN_VALID_MAGNETIC_FIELD_SQ) { - // Also reject if too small since we will get ill-defined (zero mag) cross-products below - LOGW("handleMag: magnetic field too small, not updating!"); + // Also reject if too small since we will get ill-defined (zero mag) + // cross-products below return BAD_VALUE; } if (!checkInitComplete(MAG, m)) return BAD_VALUE; - // Orthogonalize the magnetic field to the gravity field, mapping it into tangent to Earth. + // Orthogonalize the magnetic field to the gravity field, mapping it into + // tangent to Earth. const vec3_t up( getRotationMatrix() * Ba ); const vec3_t east( cross_product(m, up) ); - // If the m and up vectors align, the cross product magnitude will approach 0. - // Reject this case as well to avoid div by zero problems and ill-conditioning below. + // If the m and up vectors align, the cross product magnitude will + // approach 0. + // Reject this case as well to avoid div by zero problems and + // ill-conditioning below. if (length_squared(east) < MIN_VALID_CROSS_PRODUCT_MAG_SQ) { - LOGW("handleMag: magnetic field too aligned with up vector, not updating!"); return BAD_VALUE; } - // If we have created an orthogonal magnetic field successfully, then pass it in as the update. + // If we have created an orthogonal magnetic field successfully, + // then pass it in as the update. vec3_t north( cross_product(up, east) ); const float l = 1 / length(north); From b9e152637a99548fdbab95e8de6a9d70bf9a6a47 Mon Sep 17 00:00:00 2001 From: Mathias Agopian Date: Mon, 12 Sep 2011 22:16:01 -0700 Subject: [PATCH 37/51] improve sensorservice_test output Change-Id: I6248b6f1f001fedec1bddcddfcd2b381d9bb4bf4 --- .../sensorservice/tests/sensorservicetest.cpp | 25 +++++++++---------- 1 file changed, 12 insertions(+), 13 deletions(-) diff --git a/services/sensorservice/tests/sensorservicetest.cpp b/services/sensorservice/tests/sensorservicetest.cpp index 54bce091f..1025fa83f 100644 --- a/services/sensorservice/tests/sensorservicetest.cpp +++ b/services/sensorservice/tests/sensorservicetest.cpp @@ -35,22 +35,21 @@ int receiver(int fd, int events, void* data) while ((n = q->read(buffer, 8)) > 0) { for (int i=0 ; i\n", - buffer[i].timestamp, - buffer[i].acceleration.x, - buffer[i].acceleration.y, - buffer[i].acceleration.z); - } - + float t; if (oldTimeStamp) { - float t = float(buffer[i].timestamp - oldTimeStamp) / s2ns(1); - printf("%f ms (%f Hz)\n", t*1000, 1.0/t); + t = float(buffer[i].timestamp - oldTimeStamp) / s2ns(1); } else { - float t = float(buffer[i].timestamp - sStartTime) / s2ns(1); - printf("first event: %f ms\n", t*1000); + t = float(buffer[i].timestamp - sStartTime) / s2ns(1); } oldTimeStamp = buffer[i].timestamp; + + if (buffer[i].type == Sensor::TYPE_ACCELEROMETER) { + printf("%lld\t%8f\t%8f\t%8f\t%f\n", + buffer[i].timestamp, + buffer[i].data[0], buffer[i].data[1], buffer[i].data[2], + 1.0/t); + } + } } if (n<0 && n != -EAGAIN) { @@ -79,7 +78,7 @@ int main(int argc, char** argv) q->enableSensor(accelerometer); - q->setEventRate(accelerometer, ms2ns(200)); + q->setEventRate(accelerometer, ms2ns(10)); sp loop = new Looper(false); loop->addFd(q->getFd(), 0, ALOOPER_EVENT_INPUT, receiver, q.get()); From 667102f6b072582fe497599e0b760f9fc94ceffa Mon Sep 17 00:00:00 2001 From: Mathias Agopian Date: Wed, 14 Sep 2011 16:43:34 -0700 Subject: [PATCH 38/51] improve sensorservice dumpsys Change-Id: I8b53d5cab884c3aca16d95df5fbf288368d52e8b --- services/sensorservice/Fusion.cpp | 3 ++ services/sensorservice/SensorDevice.cpp | 70 +++++++++++++++---------- services/sensorservice/SensorDevice.h | 7 ++- 3 files changed, 51 insertions(+), 29 deletions(-) diff --git a/services/sensorservice/Fusion.cpp b/services/sensorservice/Fusion.cpp index 0ab86c3e7..d76f19c1a 100644 --- a/services/sensorservice/Fusion.cpp +++ b/services/sensorservice/Fusion.cpp @@ -167,6 +167,9 @@ Fusion::Fusion() { Bm.y = 1; Bm.z = 0; + x0 = 0; + x1 = 0; + init(); } diff --git a/services/sensorservice/SensorDevice.cpp b/services/sensorservice/SensorDevice.cpp index 38d498c06..d82a7e238 100644 --- a/services/sensorservice/SensorDevice.cpp +++ b/services/sensorservice/SensorDevice.cpp @@ -138,9 +138,18 @@ void SensorDevice::dump(String8& result, char* buffer, size_t SIZE) Mutex::Autolock _l(mLock); for (size_t i=0 ; isetDelay(mSensorDevice, handle, ns); } @@ -237,24 +238,39 @@ status_t SensorDevice::activate(void* ident, int handle, int enabled) status_t SensorDevice::setDelay(void* ident, int handle, int64_t ns) { if (!mSensorDevice) return NO_INIT; + Mutex::Autolock _l(mLock); Info& info( mActivationCount.editValueFor(handle) ); - { // scope for lock - Mutex::Autolock _l(mLock); - ssize_t index = info.rates.indexOfKey(ident); - if (index < 0) return BAD_INDEX; - info.rates.editValueAt(index) = ns; - ns = info.rates.valueAt(0); - for (size_t i=1 ; isetDelay(mSensorDevice, handle, ns); +} + +// --------------------------------------------------------------------------- + +status_t SensorDevice::Info::setDelayForIdent(void* ident, int64_t ns) +{ + ssize_t index = rates.indexOfKey(ident); + if (index < 0) { + LOGE("Info::setDelayForIdent(ident=%p, ns=%lld) failed (%s)", + ident, ns, strerror(-index)); + return BAD_INDEX; + } + rates.editValueAt(index) = ns; + return NO_ERROR; +} + +nsecs_t SensorDevice::Info::selectDelay() +{ + nsecs_t ns = rates.valueAt(0); + for (size_t i=1 ; isetDelay(mSensorDevice, handle, ns); + delay = ns; + return ns; } // --------------------------------------------------------------------------- diff --git a/services/sensorservice/SensorDevice.h b/services/sensorservice/SensorDevice.h index c19b2ced4..728b6cb86 100644 --- a/services/sensorservice/SensorDevice.h +++ b/services/sensorservice/SensorDevice.h @@ -37,11 +37,14 @@ class SensorDevice : public Singleton { friend class Singleton; struct sensors_poll_device_t* mSensorDevice; struct sensors_module_t* mSensorModule; - Mutex mLock; // protect mActivationCount[].rates + mutable Mutex mLock; // protect mActivationCount[].rates // fixed-size array after construction struct Info { - Info() { } + Info() : delay(0) { } KeyedVector rates; + nsecs_t delay; + status_t setDelayForIdent(void* ident, int64_t ns); + nsecs_t selectDelay(); }; DefaultKeyedVector mActivationCount; From b3989276d17f2b083bec67b695d1078fb86c6c53 Mon Sep 17 00:00:00 2001 From: Mathias Agopian Date: Thu, 20 Oct 2011 18:42:02 -0700 Subject: [PATCH 39/51] Rename SensorChannel to BitTube --- services/sensorservice/SensorService.cpp | 4 ++-- services/sensorservice/SensorService.h | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp index 5b74fb809..57cb53c8a 100644 --- a/services/sensorservice/SensorService.cpp +++ b/services/sensorservice/SensorService.cpp @@ -514,7 +514,7 @@ bool SensorService::SensorRecord::removeConnection( SensorService::SensorEventConnection::SensorEventConnection( const sp& service) - : mService(service), mChannel(new SensorChannel()) + : mService(service), mChannel(new BitTube()) { } @@ -596,7 +596,7 @@ status_t SensorService::SensorEventConnection::sendEvents( return size < 0 ? status_t(size) : status_t(NO_ERROR); } -sp SensorService::SensorEventConnection::getSensorChannel() const +sp SensorService::SensorEventConnection::getSensorChannel() const { return mChannel; } diff --git a/services/sensorservice/SensorService.h b/services/sensorservice/SensorService.h index 85f4ecbf9..e357f967c 100644 --- a/services/sensorservice/SensorService.h +++ b/services/sensorservice/SensorService.h @@ -29,7 +29,7 @@ #include #include -#include +#include #include #include @@ -71,12 +71,12 @@ class SensorService : class SensorEventConnection : public BnSensorEventConnection { virtual ~SensorEventConnection(); virtual void onFirstRef(); - virtual sp getSensorChannel() const; + virtual sp getSensorChannel() const; virtual status_t enableDisable(int handle, bool enabled); virtual status_t setEventRate(int handle, nsecs_t ns); sp const mService; - sp const mChannel; + sp const mChannel; mutable Mutex mConnectionLock; // protected by SensorService::mLock From ae09d65f5b35cb51da2e1386a6dd7a52085f1325 Mon Sep 17 00:00:00 2001 From: Mathias Agopian Date: Tue, 1 Nov 2011 17:37:49 -0700 Subject: [PATCH 40/51] fix an issue where SensorService could request an invalid sensor delay When the app requests "fastest", the java layer encodes this as a delay of 0. SensorService was passing this unchanged to the HAL. However the HAL is required to reject delays lower that the advertised lower delay. Change-Id: I92be77acd3af62ffeb49e4b31e24ddcd203510e2 --- services/sensorservice/SensorService.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp index 57cb53c8a..11adbe24f 100644 --- a/services/sensorservice/SensorService.cpp +++ b/services/sensorservice/SensorService.cpp @@ -471,14 +471,20 @@ status_t SensorService::setEventRate(const sp& connection if (mInitCheck != NO_ERROR) return mInitCheck; + SensorInterface* sensor = mSensorMap.valueFor(handle); + if (!sensor) + return BAD_VALUE; + if (ns < 0) return BAD_VALUE; + if (ns == 0) { + ns = sensor->getSensor().getMinDelayNs(); + } + if (ns < MINIMUM_EVENTS_PERIOD) ns = MINIMUM_EVENTS_PERIOD; - SensorInterface* sensor = mSensorMap.valueFor(handle); - if (!sensor) return BAD_VALUE; return sensor->setDelay(connection.get(), handle, ns); } From 62569ecf526c7c01cb68ea461c6bbd3cb26057d2 Mon Sep 17 00:00:00 2001 From: Mathias Agopian Date: Mon, 7 Nov 2011 21:21:47 -0800 Subject: [PATCH 41/51] SensorService now always clamps the requested rate Requested rate will be clamped to the minimum rate and then to 1ms. Previously we would return an error if a lower rate was asked. The SensorManager documentation wording allows this change. We do this to get more consistancy between all the sensor drivers / HALs Change-Id: I199f76486fb76ccbb11e7280460a03726c767e84 --- services/sensorservice/SensorService.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp index 11adbe24f..7d5a7617d 100644 --- a/services/sensorservice/SensorService.cpp +++ b/services/sensorservice/SensorService.cpp @@ -478,8 +478,9 @@ status_t SensorService::setEventRate(const sp& connection if (ns < 0) return BAD_VALUE; - if (ns == 0) { - ns = sensor->getSensor().getMinDelayNs(); + nsecs_t minDelayNs = sensor->getSensor().getMinDelayNs(); + if (ns < minDelayNs) { + ns = minDelayNs; } if (ns < MINIMUM_EVENTS_PERIOD) From 1a62301fc58cd2af18239b0415813461bf5fc41b Mon Sep 17 00:00:00 2001 From: Mathias Agopian Date: Wed, 9 Nov 2011 17:50:15 -0800 Subject: [PATCH 42/51] handle EINTR when calling sensor HAL's poll function some sensor HALs don't handle EINTR, make sure to catch it in the sensorservice. also if we ever encounter an error that we can't handle, we abort which will restart us (or the whole system process if we're running in it) Bug: 5511741 Change-Id: I7051882b06980f778736b53d6cd021a99b5ca8d2 --- services/sensorservice/SensorDevice.cpp | 6 +++++- services/sensorservice/SensorService.cpp | 3 ++- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/services/sensorservice/SensorDevice.cpp b/services/sensorservice/SensorDevice.cpp index d82a7e238..7575ebd19 100644 --- a/services/sensorservice/SensorDevice.cpp +++ b/services/sensorservice/SensorDevice.cpp @@ -166,7 +166,11 @@ status_t SensorDevice::initCheck() const { ssize_t SensorDevice::poll(sensors_event_t* buffer, size_t count) { if (!mSensorDevice) return NO_INIT; - return mSensorDevice->poll(mSensorDevice, buffer, count); + ssize_t c; + do { + c = mSensorDevice->poll(mSensorDevice, buffer, count); + } while (c == -EINTR); + return c; } status_t SensorDevice::activate(void* ident, int handle, int enabled) diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp index 7d5a7617d..f61a11a72 100644 --- a/services/sensorservice/SensorService.cpp +++ b/services/sensorservice/SensorService.cpp @@ -286,7 +286,8 @@ bool SensorService::threadLoop() } } while (count >= 0 || Thread::exitPending()); - LOGW("Exiting SensorService::threadLoop!"); + LOGW("Exiting SensorService::threadLoop => aborting..."); + abort(); return false; } From a551237de142549fb8a6608ee9d2fbf4b7ca2ebf Mon Sep 17 00:00:00 2001 From: Steve Block Date: Tue, 20 Dec 2011 16:23:08 +0000 Subject: [PATCH 43/51] Rename (IF_)LOGD(_IF) to (IF_)ALOGD(_IF) DO NOT MERGE See https://android-git.corp.google.com/g/156016 Bug: 5449033 Change-Id: I4c4e33bb9df3e39e11cd985e193e6fbab4635298 --- services/sensorservice/SensorDevice.cpp | 8 ++++---- services/sensorservice/SensorFusion.cpp | 2 +- services/sensorservice/SensorService.cpp | 14 +++++++------- 3 files changed, 12 insertions(+), 12 deletions(-) diff --git a/services/sensorservice/SensorDevice.cpp b/services/sensorservice/SensorDevice.cpp index 7575ebd19..8f2350630 100644 --- a/services/sensorservice/SensorDevice.cpp +++ b/services/sensorservice/SensorDevice.cpp @@ -182,13 +182,13 @@ status_t SensorDevice::activate(void* ident, int handle, int enabled) Info& info( mActivationCount.editValueFor(handle) ); - LOGD_IF(DEBUG_CONNECTIONS, + ALOGD_IF(DEBUG_CONNECTIONS, "SensorDevice::activate: ident=%p, handle=0x%08x, enabled=%d, count=%d", ident, handle, enabled, info.rates.size()); if (enabled) { Mutex::Autolock _l(mLock); - LOGD_IF(DEBUG_CONNECTIONS, "... index=%ld", + ALOGD_IF(DEBUG_CONNECTIONS, "... index=%ld", info.rates.indexOfKey(ident)); if (info.rates.indexOfKey(ident) < 0) { @@ -201,7 +201,7 @@ status_t SensorDevice::activate(void* ident, int handle, int enabled) } } else { Mutex::Autolock _l(mLock); - LOGD_IF(DEBUG_CONNECTIONS, "... index=%ld", + ALOGD_IF(DEBUG_CONNECTIONS, "... index=%ld", info.rates.indexOfKey(ident)); ssize_t idx = info.rates.removeItem(ident); @@ -215,7 +215,7 @@ status_t SensorDevice::activate(void* ident, int handle, int enabled) } if (actuateHardware) { - LOGD_IF(DEBUG_CONNECTIONS, "\t>>> actuating h/w"); + ALOGD_IF(DEBUG_CONNECTIONS, "\t>>> actuating h/w"); err = mSensorDevice->activate(mSensorDevice, handle, enabled); if (enabled) { diff --git a/services/sensorservice/SensorFusion.cpp b/services/sensorservice/SensorFusion.cpp index 518a1bb75..d23906dea 100644 --- a/services/sensorservice/SensorFusion.cpp +++ b/services/sensorservice/SensorFusion.cpp @@ -76,7 +76,7 @@ template inline T max(T a, T b) { return a>b ? a : b; } status_t SensorFusion::activate(void* ident, bool enabled) { - LOGD_IF(DEBUG_CONNECTIONS, + ALOGD_IF(DEBUG_CONNECTIONS, "SensorFusion::activate(ident=%p, enabled=%d)", ident, enabled); diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp index f61a11a72..3e4a2f59e 100644 --- a/services/sensorservice/SensorService.cpp +++ b/services/sensorservice/SensorService.cpp @@ -65,7 +65,7 @@ SensorService::SensorService() void SensorService::onFirstRef() { - LOGD("nuSensorService starting..."); + ALOGD("nuSensorService starting..."); SensorDevice& dev(SensorDevice::getInstance()); @@ -222,7 +222,7 @@ status_t SensorService::dump(int fd, const Vector& args) bool SensorService::threadLoop() { - LOGD("nuSensorService thread starting..."); + ALOGD("nuSensorService thread starting..."); const size_t numEventMax = 16 * (1 + mVirtualSensorList.size()); sensors_event_t buffer[numEventMax]; @@ -363,11 +363,11 @@ void SensorService::cleanupConnection(SensorEventConnection* c) Mutex::Autolock _l(mLock); const wp connection(c); size_t size = mActiveSensors.size(); - LOGD_IF(DEBUG_CONNECTIONS, "%d active sensors", size); + ALOGD_IF(DEBUG_CONNECTIONS, "%d active sensors", size); for (size_t i=0 ; ihasSensor(handle)) { - LOGD_IF(DEBUG_CONNECTIONS, "%i: disabling handle=0x%08x", i, handle); + ALOGD_IF(DEBUG_CONNECTIONS, "%i: disabling handle=0x%08x", i, handle); SensorInterface* sensor = mSensorMap.valueFor( handle ); LOGE_IF(!sensor, "mSensorMap[handle=0x%08x] is null!", handle); if (sensor) { @@ -376,12 +376,12 @@ void SensorService::cleanupConnection(SensorEventConnection* c) } SensorRecord* rec = mActiveSensors.valueAt(i); LOGE_IF(!rec, "mActiveSensors[%d] is null (handle=0x%08x)!", i, handle); - LOGD_IF(DEBUG_CONNECTIONS, + ALOGD_IF(DEBUG_CONNECTIONS, "removing connection %p for sensor[%d].handle=0x%08x", c, i, handle); if (rec && rec->removeConnection(connection)) { - LOGD_IF(DEBUG_CONNECTIONS, "... and it was the last connection"); + ALOGD_IF(DEBUG_CONNECTIONS, "... and it was the last connection"); mActiveSensors.removeItemsAt(i, 1); mActiveVirtualSensors.removeItem(handle); delete rec; @@ -528,7 +528,7 @@ SensorService::SensorEventConnection::SensorEventConnection( SensorService::SensorEventConnection::~SensorEventConnection() { - LOGD_IF(DEBUG_CONNECTIONS, "~SensorEventConnection(%p)", this); + ALOGD_IF(DEBUG_CONNECTIONS, "~SensorEventConnection(%p)", this); mService->cleanupConnection(this); } From c7839e897646ece32894d3b7d1685c3880d949fe Mon Sep 17 00:00:00 2001 From: Steve Block Date: Wed, 4 Jan 2012 20:05:49 +0000 Subject: [PATCH 44/51] Rename (IF_)LOGI(_IF) to (IF_)ALOGI(_IF) DO NOT MERGE See https://android-git.corp.google.com/g/156801 Bug: 5449033 Change-Id: Ib08fe86d23db91ee153e9f91a99a35c42b9208ea --- services/sensorservice/SensorInterface.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/services/sensorservice/SensorInterface.cpp b/services/sensorservice/SensorInterface.cpp index be8eaff73..468aa6197 100644 --- a/services/sensorservice/SensorInterface.cpp +++ b/services/sensorservice/SensorInterface.cpp @@ -34,7 +34,7 @@ HardwareSensor::HardwareSensor(const sensor_t& sensor) : mSensorDevice(SensorDevice::getInstance()), mSensor(&sensor) { - LOGI("%s", sensor.name); + ALOGI("%s", sensor.name); } HardwareSensor::~HardwareSensor() { From 3c20fbed7f3a916ced10f2ed5a272271b7d81ede Mon Sep 17 00:00:00 2001 From: Steve Block Date: Thu, 5 Jan 2012 23:22:43 +0000 Subject: [PATCH 45/51] Rename (IF_)LOGW(_IF) to (IF_)ALOGW(_IF) DO NOT MERGE See https://android-git.corp.google.com/g/157065 Bug: 5449033 Change-Id: I00a4b904f9449e6f93b7fd35eac28640d7929e69 --- services/sensorservice/Fusion.cpp | 2 +- services/sensorservice/SensorService.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/services/sensorservice/Fusion.cpp b/services/sensorservice/Fusion.cpp index d76f19c1a..b724ce24e 100644 --- a/services/sensorservice/Fusion.cpp +++ b/services/sensorservice/Fusion.cpp @@ -338,7 +338,7 @@ void Fusion::checkState() { if (!isPositiveSemidefinite(P[0][0], SYMMETRY_TOLERANCE) || !isPositiveSemidefinite(P[1][1], SYMMETRY_TOLERANCE)) { - LOGW("Sensor fusion diverged; resetting state."); + ALOGW("Sensor fusion diverged; resetting state."); P = 0; } } diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp index 3e4a2f59e..a1070d07f 100644 --- a/services/sensorservice/SensorService.cpp +++ b/services/sensorservice/SensorService.cpp @@ -286,7 +286,7 @@ bool SensorService::threadLoop() } } while (count >= 0 || Thread::exitPending()); - LOGW("Exiting SensorService::threadLoop => aborting..."); + ALOGW("Exiting SensorService::threadLoop => aborting..."); abort(); return false; } @@ -594,7 +594,7 @@ status_t SensorService::SensorEventConnection::sendEvents( if (size == -EAGAIN) { // the destination doesn't accept events anymore, it's probably // full. For now, we just drop the events on the floor. - //LOGW("dropping %d events on the floor", count); + //ALOGW("dropping %d events on the floor", count); return size; } From f5a1230d322c14c42331d0a1536b50c87742973b Mon Sep 17 00:00:00 2001 From: Steve Block Date: Fri, 6 Jan 2012 19:20:56 +0000 Subject: [PATCH 46/51] Rename (IF_)LOGE(_IF) to (IF_)ALOGE(_IF) DO NOT MERGE See https://android-git.corp.google.com/g/#/c/157220 Bug: 5449033 Change-Id: Ic9c19d30693bd56755f55906127cd6bd7126096c --- services/sensorservice/SensorDevice.cpp | 8 ++++---- services/sensorservice/SensorService.cpp | 8 ++++---- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/services/sensorservice/SensorDevice.cpp b/services/sensorservice/SensorDevice.cpp index 8f2350630..2244a8688 100644 --- a/services/sensorservice/SensorDevice.cpp +++ b/services/sensorservice/SensorDevice.cpp @@ -105,13 +105,13 @@ SensorDevice::SensorDevice() status_t err = hw_get_module(SENSORS_HARDWARE_MODULE_ID, (hw_module_t const**)&mSensorModule); - LOGE_IF(err, "couldn't load %s module (%s)", + ALOGE_IF(err, "couldn't load %s module (%s)", SENSORS_HARDWARE_MODULE_ID, strerror(-err)); if (mSensorModule) { err = sensors_open(&mSensorModule->common, &mSensorDevice); - LOGE_IF(err, "couldn't open device for module %s (%s)", + ALOGE_IF(err, "couldn't open device for module %s (%s)", SENSORS_HARDWARE_MODULE_ID, strerror(-err)); if (mSensorDevice) { @@ -219,7 +219,7 @@ status_t SensorDevice::activate(void* ident, int handle, int enabled) err = mSensorDevice->activate(mSensorDevice, handle, enabled); if (enabled) { - LOGE_IF(err, "Error activating sensor %d (%s)", handle, strerror(-err)); + ALOGE_IF(err, "Error activating sensor %d (%s)", handle, strerror(-err)); if (err == 0) { BatteryService::getInstance().enableSensor(handle); } @@ -256,7 +256,7 @@ status_t SensorDevice::Info::setDelayForIdent(void* ident, int64_t ns) { ssize_t index = rates.indexOfKey(ident); if (index < 0) { - LOGE("Info::setDelayForIdent(ident=%p, ns=%lld) failed (%s)", + ALOGE("Info::setDelayForIdent(ident=%p, ns=%lld) failed (%s)", ident, ns, strerror(-index)); return BAD_INDEX; } diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp index a1070d07f..dd6c426e9 100644 --- a/services/sensorservice/SensorService.cpp +++ b/services/sensorservice/SensorService.cpp @@ -234,7 +234,7 @@ bool SensorService::threadLoop() do { count = device.poll(buffer, numEventMax); if (count<0) { - LOGE("sensor poll failed (%s)", strerror(-count)); + ALOGE("sensor poll failed (%s)", strerror(-count)); break; } @@ -369,13 +369,13 @@ void SensorService::cleanupConnection(SensorEventConnection* c) if (c->hasSensor(handle)) { ALOGD_IF(DEBUG_CONNECTIONS, "%i: disabling handle=0x%08x", i, handle); SensorInterface* sensor = mSensorMap.valueFor( handle ); - LOGE_IF(!sensor, "mSensorMap[handle=0x%08x] is null!", handle); + ALOGE_IF(!sensor, "mSensorMap[handle=0x%08x] is null!", handle); if (sensor) { sensor->activate(c, false); } } SensorRecord* rec = mActiveSensors.valueAt(i); - LOGE_IF(!rec, "mActiveSensors[%d] is null (handle=0x%08x)!", i, handle); + ALOGE_IF(!rec, "mActiveSensors[%d] is null (handle=0x%08x)!", i, handle); ALOGD_IF(DEBUG_CONNECTIONS, "removing connection %p for sensor[%d].handle=0x%08x", c, i, handle); @@ -598,7 +598,7 @@ status_t SensorService::SensorEventConnection::sendEvents( return size; } - //LOGE_IF(size<0, "dropping %d events on the floor (%s)", + //ALOGE_IF(size<0, "dropping %d events on the floor (%s)", // count, strerror(-size)); return size < 0 ? status_t(size) : status_t(NO_ERROR); From 907103bf186cfdd2ed9eb3b6c36de53ade7b16f6 Mon Sep 17 00:00:00 2001 From: Mathias Agopian Date: Mon, 2 Apr 2012 18:38:02 -0700 Subject: [PATCH 47/51] usea socketpair instead of a pipe in BitTube Bug: 6252830 Change-Id: I363cc7e9f73a5b7d8bbccee312c6d8938c84e99a --- services/sensorservice/SensorService.cpp | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp index dd6c426e9..16ddd9115 100644 --- a/services/sensorservice/SensorService.cpp +++ b/services/sensorservice/SensorService.cpp @@ -35,6 +35,7 @@ #include #include +#include #include @@ -587,10 +588,9 @@ status_t SensorService::SensorEventConnection::sendEvents( count = numEvents; } - if (count == 0) - return 0; - - ssize_t size = mChannel->write(scratch, count*sizeof(sensors_event_t)); + // NOTE: ASensorEvent and sensors_event_t are the same type + ssize_t size = SensorEventQueue::write(mChannel, + reinterpret_cast(scratch), count); if (size == -EAGAIN) { // the destination doesn't accept events anymore, it's probably // full. For now, we just drop the events on the floor. @@ -598,9 +598,6 @@ status_t SensorService::SensorEventConnection::sendEvents( return size; } - //ALOGE_IF(size<0, "dropping %d events on the floor (%s)", - // count, strerror(-size)); - return size < 0 ? status_t(size) : status_t(NO_ERROR); } From a5c106a4f0afcf061728a1cb7c8c3b908728575d Mon Sep 17 00:00:00 2001 From: Mathias Agopian Date: Thu, 19 Apr 2012 18:18:24 -0700 Subject: [PATCH 48/51] fix a typo causing sensor event to be sorted in reverse chronological order Change-Id: I041c64616d88ed1abb5efc90ed9eb0d9baeb4832 --- services/sensorservice/SensorService.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp index 16ddd9115..d3b667f13 100644 --- a/services/sensorservice/SensorService.cpp +++ b/services/sensorservice/SensorService.cpp @@ -316,7 +316,7 @@ void SensorService::sortEventBuffer(sensors_event_t* buffer, size_t count) static int cmp(void const* lhs, void const* rhs) { sensors_event_t const* l = static_cast(lhs); sensors_event_t const* r = static_cast(rhs); - return r->timestamp - l->timestamp; + return l->timestamp - r->timestamp; } }; qsort(buffer, count, sizeof(sensors_event_t), compar::cmp); From d1920ffede9e9bc107104ad94c291ca0f0f18bc3 Mon Sep 17 00:00:00 2001 From: Mathias Agopian Date: Tue, 29 May 2012 19:46:14 -0700 Subject: [PATCH 49/51] Fix a stack corruption in sensorservice Bug: 6576732 Change-Id: If0f2fb0d0c35b932fb77cd262e676042145b28f9 --- services/sensorservice/SensorService.cpp | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp index d3b667f13..04ec82046 100644 --- a/services/sensorservice/SensorService.cpp +++ b/services/sensorservice/SensorService.cpp @@ -225,9 +225,10 @@ bool SensorService::threadLoop() { ALOGD("nuSensorService thread starting..."); - const size_t numEventMax = 16 * (1 + mVirtualSensorList.size()); - sensors_event_t buffer[numEventMax]; - sensors_event_t scratch[numEventMax]; + const size_t numEventMax = 16; + const size_t minBufferSize = numEventMax * mVirtualSensorList.size(); + sensors_event_t buffer[minBufferSize]; + sensors_event_t scratch[minBufferSize]; SensorDevice& device(SensorDevice::getInstance()); const size_t vcount = mVirtualSensorList.size(); @@ -255,10 +256,17 @@ bool SensorService::threadLoop() fusion.process(event[i]); } } - for (size_t i=0 ; i= minBufferSize) { + ALOGE("buffer too small to hold all events: " + "count=%u, k=%u, size=%u", + count, k, minBufferSize); + break; + } sensors_event_t out; - if (virtualSensors.valueAt(j)->process(&out, event[i])) { + SensorInterface* si = virtualSensors.valueAt(j); + if (si->process(&out, event[i])) { buffer[count + k] = out; k++; } From 8dd4fe8dd37b740282b48b7867eec26fcf95e027 Mon Sep 17 00:00:00 2001 From: Mathias Agopian Date: Wed, 30 May 2012 18:08:30 -0700 Subject: [PATCH 50/51] fix a potential buffer overflow in sensorservice Bug: 6580560 Change-Id: Icf6cafbca09174515a964a7cd69d8cc589ad52de --- services/sensorservice/SensorService.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp index 04ec82046..9e5e84b38 100644 --- a/services/sensorservice/SensorService.cpp +++ b/services/sensorservice/SensorService.cpp @@ -226,7 +226,7 @@ bool SensorService::threadLoop() ALOGD("nuSensorService thread starting..."); const size_t numEventMax = 16; - const size_t minBufferSize = numEventMax * mVirtualSensorList.size(); + const size_t minBufferSize = numEventMax + numEventMax * mVirtualSensorList.size(); sensors_event_t buffer[minBufferSize]; sensors_event_t scratch[minBufferSize]; SensorDevice& device(SensorDevice::getInstance()); From dc5b63e40ee697324d39fe105d6f12c2bb031fc6 Mon Sep 17 00:00:00 2001 From: Mathias Agopian Date: Mon, 18 Jun 2012 18:49:08 -0700 Subject: [PATCH 51/51] small improvement to sensor fusion 1) there was a typo when computing the system covariance a term in dT^3 was ommitted; the impact was was very limited because of how small this term is. 2) initialize the system covariance matrix with non-zero values for the gyro-bias part. this improves the initial bias estimation speed significantly. 3) added comments here and there Change-Id: I4328c9cca73e089889d5e74b9fda99d7831762dc --- services/sensorservice/Fusion.cpp | 40 +++++++++++++++++++++++++------ 1 file changed, 33 insertions(+), 7 deletions(-) diff --git a/services/sensorservice/Fusion.cpp b/services/sensorservice/Fusion.cpp index b724ce24e..2b6b79319 100644 --- a/services/sensorservice/Fusion.cpp +++ b/services/sensorservice/Fusion.cpp @@ -201,15 +201,15 @@ void Fusion::initFusion(const vec4_t& q, float dT) // q11 = su^2.dt // - // variance of integrated output at 1/dT Hz - // (random drift) - const float q00 = gyroVAR * dT; + const float dT2 = dT*dT; + const float dT3 = dT2*dT; + + // variance of integrated output at 1/dT Hz (random drift) + const float q00 = gyroVAR * dT + 0.33333f * biasVAR * dT3; // variance of drift rate ramp const float q11 = biasVAR * dT; - - const float u = q11 / dT; - const float q10 = 0.5f*u*dT*dT; + const float q10 = 0.5f * biasVAR * dT2; const float q01 = q10; GQGt[0][0] = q00; // rad^2 @@ -220,6 +220,22 @@ void Fusion::initFusion(const vec4_t& q, float dT) // initial covariance: Var{ x(t0) } // TODO: initialize P correctly P = 0; + + // it is unclear how to set the initial covariance. It does affect + // how quickly the fusion converges. Experimentally it would take + // about 10 seconds at 200 Hz to estimate the gyro-drift with an + // initial covariance of 0, and about a second with an initial covariance + // of about 1 deg/s. + const float covv = 0; + const float covu = 0.5f * (float(M_PI) / 180); + mat33_t& Pv = P[0][0]; + Pv[0][0] = covv; + Pv[1][1] = covv; + Pv[2][2] = covv; + mat33_t& Pu = P[1][1]; + Pu[0][0] = covu; + Pu[1][1] = covu; + Pu[2][2] = covu; } bool Fusion::hasEstimate() const { @@ -357,6 +373,11 @@ mat33_t Fusion::getRotationMatrix() const { mat34_t Fusion::getF(const vec4_t& q) { mat34_t F; + + // This is used to compute the derivative of q + // F = | [q.xyz]x | + // | -q.xyz | + F[0].x = q.w; F[1].x =-q.z; F[2].x = q.y; F[0].y = q.z; F[1].y = q.w; F[2].y =-q.x; F[0].z =-q.y; F[1].z = q.x; F[2].z = q.w; @@ -413,7 +434,12 @@ void Fusion::update(const vec3_t& z, const vec3_t& Bi, float sigma) { K[1] = transpose(P[1][0])*LtSi; // update... - // P -= K*H*P; + // P = (I-K*H) * P + // P -= K*H*P + // | K0 | * | L 0 | * P = | K0*L 0 | * | P00 P10 | = | K0*L*P00 K0*L*P10 | + // | K1 | | K1*L 0 | | P01 P11 | | K1*L*P00 K1*L*P10 | + // Note: the Joseph form is numerically more stable and given by: + // P = (I-KH) * P * (I-KH)' + K*R*R' const mat33_t K0L(K[0] * L); const mat33_t K1L(K[1] * L); P[0][0] -= K0L*P[0][0];