Sensor: Add RotationVectorSensor2

(ported from 4.1)

Some ICS apps (namely, Google Maps) expects a rotation vector to be
available. Newer devices, this is provided by either Android's
sensor fusion (requires Gyro) or by hardware sensor fusion (MPL).

Older devices will lack this virtual sensor and compass in Google
Maps will not work. To fix this, we can provide our own rotation
vector sensor by converting the values from the orientation sensor.
(They are basically the same information in different formats.)

Thanks to Unhelpful for the help with related math.

Change-Id: I39489b3a5ce7c7d890768614357f32cc491bd6d9

4 files changed, 203 insertions, 0 deletions

diff --git a/services/sensorservice/Android.mk b/services/sensorservice/Android.mk
index e0cfaa6..773eca5 100644
--- a/services/sensorservice/Android.mk
+++ b/services/sensorservice/Android.mk
@@ -9,6 +9,7 @@ LOCAL_SRC_FILES:= \
     LinearAccelerationSensor.cpp \
     OrientationSensor.cpp \
     RotationVectorSensor.cpp \
+    RotationVectorSensor2.cpp \
     SensorDevice.cpp \
     SensorFusion.cpp \
     SensorInterface.cpp \
diff --git a/services/sensorservice/RotationVectorSensor2.cpp b/services/sensorservice/RotationVectorSensor2.cpp
new file mode 100644
index 0000000..f2873dd
--- /dev/null
+++ b/services/sensorservice/RotationVectorSensor2.cpp
@@ -0,0 +1,126 @@
+/*
+ * Copyright (C) 2010 The Android Open Source Project
+ * Copyright (C) 2012 The CyanogenMod 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 <stdint.h>
+#include <math.h>
+#include <sys/types.h>
+
+#include <utils/Errors.h>
+
+#include <hardware/sensors.h>
+
+#include "RotationVectorSensor2.h"
+#include "vec.h"
+
+namespace android {
+// ---------------------------------------------------------------------------
+
+ANDROID_SINGLETON_STATIC_INSTANCE(RotationVectorSensor2)
+
+RotationVectorSensor2::RotationVectorSensor2()
+    : mSensorDevice(SensorDevice::getInstance()),
+      mEnabled(false), mHasData(false)
+{
+    sensor_t const* list;
+    ssize_t count = mSensorDevice.getSensorList(&list);
+    if (count > 0) {
+        for (size_t i=0 ; i<size_t(count) ; i++) {
+            if (list[i].type == SENSOR_TYPE_ORIENTATION) {
+                mOrientation = Sensor(list + i);
+            }
+        }
+    }
+}
+
+bool RotationVectorSensor2::process(sensors_event_t* outEvent,
+        const sensors_event_t& event)
+{
+    if (mHasData && event.type == SENSOR_TYPE_ACCELEROMETER) {
+        *outEvent = event;
+        outEvent->data[0] = mData[1];
+        outEvent->data[1] = mData[2];
+        outEvent->data[2] = mData[3];
+        outEvent->data[3] = mData[0];
+        outEvent->sensor = '_rv2';
+        outEvent->type = SENSOR_TYPE_ROTATION_VECTOR;
+
+        mHasData = false;
+        return true;
+    }
+    return false;
+}
+
+status_t RotationVectorSensor2::activate(void* ident, bool enabled) {
+    mEnabled = enabled;
+    return mSensorDevice.activate(this, mOrientation.getHandle(), enabled);
+}
+
+status_t RotationVectorSensor2::setDelay(void* ident, int handle, int64_t ns) {
+    return mSensorDevice.setDelay(this, mOrientation.getHandle(), ns);
+}
+
+Sensor RotationVectorSensor2::getSensor() const {
+    sensor_t hwSensor;
+    hwSensor.name       = "Rotation Vector Sensor 2";
+    hwSensor.vendor     = "CyanogenMod Project";
+    hwSensor.version    = 1;
+    hwSensor.handle     = '_rv2';
+    hwSensor.type       = SENSOR_TYPE_ROTATION_VECTOR;
+    hwSensor.maxRange   = 1;
+    hwSensor.resolution = 1.0f / (1<<24);
+    hwSensor.power      = mOrientation.getPowerUsage();
+    hwSensor.minDelay   = mOrientation.getMinDelay();
+    Sensor sensor(&hwSensor);
+    return sensor;
+}
+
+void RotationVectorSensor2::process(const sensors_event_t& event) {
+    if (event.type == SENSOR_TYPE_ORIENTATION) {
+        const vec3_t v(event.data);
+
+        // Convert euler angle to quarternion
+        const float deg2rad = M_PI / 180;
+        float halfAzi = (v[0] / 2) * deg2rad;
+        float halfPitch = (v[1] / 2) * deg2rad;
+        float halfRoll = (-v[2] / 2) * deg2rad; // roll is reverse
+
+        float c1 = cosf(halfAzi);
+        float s1 = sinf(halfAzi);
+        float c2 = cosf(halfPitch);
+        float s2 = sinf(halfPitch);
+        float c3 = cosf(halfRoll);
+        float s3 = sinf(halfRoll);
+        mData[0] = c1*c2*c3 - s1*s2*s3;
+        mData[1] = c1*s2*c3 - s1*c2*s3;
+        mData[2] = c1*c2*s3 + s1*s2*c3;
+        mData[3] = s1*c2*c3 + c1*s2*s3;
+
+        // Misc fixes (a.k.a. "magic")
+        if (v[0] < 180) {
+            mData[1] = -mData[1];
+            mData[3] = -mData[3];
+        } else {
+            mData[2] = -mData[2];
+        }
+
+        mHasData = true;
+    }
+}
+
+// ---------------------------------------------------------------------------
+}; // namespace android
+
diff --git a/services/sensorservice/RotationVectorSensor2.h b/services/sensorservice/RotationVectorSensor2.h
new file mode 100644
index 0000000..872ec12
--- /dev/null
+++ b/services/sensorservice/RotationVectorSensor2.h
@@ -0,0 +1,63 @@
+/*
+ * Copyright (C) 2010 The Android Open Source Project
+ * Copyright (C) 2012 The CyanogenMod 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_SENSOR2_H
+#define ANDROID_ROTATION_VECTOR_SENSOR2_H
+
+#include <stdint.h>
+#include <sys/types.h>
+
+#include <gui/Sensor.h>
+
+#include "SensorDevice.h"
+#include "SensorInterface.h"
+
+#include "quat.h"
+
+// ---------------------------------------------------------------------------
+namespace android {
+// ---------------------------------------------------------------------------
+
+class RotationVectorSensor2 : public SensorInterface,
+                              public Singleton<RotationVectorSensor2> {
+    friend class Singleton<RotationVectorSensor2>;
+
+    SensorDevice& mSensorDevice;
+
+    Sensor mOrientation;
+    bool mEnabled;
+    bool mHasData;
+    quat_t mData;
+
+public:
+    RotationVectorSensor2();
+    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; }
+    bool isEnabled() const { return mEnabled; }
+
+    // Incoming data
+    void process(const sensors_event_t& event);
+};
+
+// ---------------------------------------------------------------------------
+}; // namespace android
+
+#endif // ANDROID_ROTATION_VECTOR2_SENSOR_H
diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp
index e3dcd02..8ff0872 100644
--- a/services/sensorservice/SensorService.cpp
+++ b/services/sensorservice/SensorService.cpp
@@ -45,6 +45,7 @@
 #include "LinearAccelerationSensor.h"
 #include "OrientationSensor.h"
 #include "RotationVectorSensor.h"
+#include "RotationVectorSensor2.h"
 #include "SensorFusion.h"
 #include "SensorService.h"
 
@@ -136,6 +137,12 @@ void SensorService::onFirstRef()
                 if (orientationIndex >= 0) {
                     mUserSensorList.removeItemsAt(orientationIndex);
                 }
+            } else if (orientationIndex != -1) {
+                // If we don't have a gyro but have a orientation sensor from
+                // elsewhere, we can compute rotation vector from that.
+                // (Google Maps expects rotation vector sensor to exist.)
+
+                registerVirtualSensor( &RotationVectorSensor2::getInstance() );
             }
 
             // debugging sensor list
@@ -272,6 +279,12 @@ bool SensorService::threadLoop()
                         fusion.process(event[i]);
                     }
                 }
+                RotationVectorSensor2& rv2(RotationVectorSensor2::getInstance());
+                if (rv2.isEnabled()) {
+                    for (size_t i=0 ; i<size_t(count) ; i++) {
+                        rv2.process(event[i]);
+                    }
+                }
                 for (size_t i=0 ; i<size_t(count) && k<minBufferSize ; i++) {
                     for (size_t j=0 ; j<activeVirtualSensorCount ; j++) {
                         if (count + k >= minBufferSize) {