From f8a6737f3d812b6b151990ad1695b55bfb0d8dda Mon Sep 17 00:00:00 2001
From: Mathias Agopian <mathias@google.com>
Date: Wed, 8 Jun 2011 20:06:50 -0700
Subject: [PATCH] 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 d1b10f799c1b0..0ae79294f9a7f 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<<SENSOR_TYPE_GRAVITY) |
@@ -82,6 +82,9 @@ void SensorService::onFirstRef()
         for (int i=0 ; i<count ; i++) {
             registerSensor( new HardwareSensor(list[i]) );
             switch (list[i].type) {
+                case SENSOR_TYPE_ORIENTATION:
+                    orientationIndex = i;
+                    break;
                 case SENSOR_TYPE_GYROSCOPE:
                     hasGyro = true;
                     break;
@@ -120,6 +123,18 @@ void SensorService::onFirstRef()
             }
         }
 
+        // build the sensor list returned to users
+        mUserSensorList = mSensorList;
+        if (hasGyro &&
+                (virtualSensorsNeeds & (1<<SENSOR_TYPE_ROTATION_VECTOR))) {
+            // if we have the fancy sensor fusion, and it's not provided by the
+            // HAL, use our own (fused) orientation sensor by removing the
+            // HAL supplied one form the user list.
+            if (orientationIndex >= 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<count ; i++) {
-        const Sensor& sensor(mSensorList[i]);
+        const Sensor& sensor(mUserSensorList[i]);
         if (sensor.getHandle() == handle) {
             return sensor.getName();
         }
@@ -328,7 +343,7 @@ String8 SensorService::getSensorName(int handle) const {
 
 Vector<Sensor> SensorService::getSensorList()
 {
-    return mSensorList;
+    return mUserSensorList;
 }
 
 sp<ISensorEventConnection> SensorService::createSensorEventConnection()
diff --git a/services/sensorservice/SensorService.h b/services/sensorservice/SensorService.h
index 77a7e34e85109..4d0f1d9d9c5d2 100644
--- a/services/sensorservice/SensorService.h
+++ b/services/sensorservice/SensorService.h
@@ -114,6 +114,7 @@ class SensorService :
 
     // constants
     Vector<Sensor> mSensorList;
+    Vector<Sensor> mUserSensorList;
     DefaultKeyedVector<int, SensorInterface*> mSensorMap;
     Vector<SensorInterface *> mVirtualSensorList;
     Permission mDump;