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

15 files changed, 1296 insertions, 204 deletions

diff --git a/services/sensorservice/Android.mk b/services/sensorservice/Android.mk
index 75f690f1867b..7e17fdd58ede 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 000000000000..18bd359ea27d
--- /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 <stdint.h>
+#include <math.h>
+#include <sys/types.h>
+
+#include <utils/Errors.h>
+
+#include <hardware/sensors.h>
+
+#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 ; i<count ; i++) {
+        if (list[i].type == SENSOR_TYPE_ACCELEROMETER) {
+            mAccelerometer = Sensor(list + i);
+            break;
+        }
+    }
+}
+
+bool GravitySensor::process(sensors_event_t* outEvent,
+        const sensors_event_t& event)
+{
+    const static double NS2S = 1.0 / 1000000000.0;
+    if (event.type == SENSOR_TYPE_ACCELEROMETER) {
+        float x, y, z;
+        const double now = event.timestamp * NS2S;
+        if (mAccTime == 0) {
+            x = mX.init(event.acceleration.x);
+            y = mY.init(event.acceleration.y);
+            z = mZ.init(event.acceleration.z);
+        } else {
+            double dT = now - mAccTime;
+            mLowPass.setSamplingPeriod(dT);
+            x = mX(event.acceleration.x);
+            y = mY(event.acceleration.y);
+            z = mZ(event.acceleration.z);
+        }
+        mAccTime = now;
+        *outEvent = event;
+        outEvent->data[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 000000000000..f9850b75f34f
--- /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 <stdint.h>
+#include <sys/types.h>
+
+#include <gui/Sensor.h>
+
+#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 000000000000..2dc12dc50a33
--- /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 <stdint.h>
+#include <math.h>
+#include <sys/types.h>
+
+#include <utils/Errors.h>
+
+#include <hardware/sensors.h>
+
+#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 000000000000..ee918ce1c471
--- /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 <stdint.h>
+#include <sys/types.h>
+
+#include <gui/Sensor.h>
+
+#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 000000000000..6f4b8be45fde
--- /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 <stdint.h>
+#include <math.h>
+#include <sys/types.h>
+
+#include <utils/Errors.h>
+
+#include <hardware/sensors.h>
+
+#include "RotationVectorSensor.h"
+
+namespace android {
+// ---------------------------------------------------------------------------
+
+template <typename T>
+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<count ; i++) {
+        if (list[i].type == SENSOR_TYPE_ACCELEROMETER) {
+            mAcc = Sensor(list + i);
+        }
+        if (list[i].type == SENSOR_TYPE_MAGNETIC_FIELD) {
+            mMag = Sensor(list + i);
+        }
+    }
+    memset(mMagData, 0, sizeof(mMagData));
+}
+
+bool RotationVectorSensor::process(sensors_event_t* outEvent,
+        const sensors_event_t& event)
+{
+    const static double NS2S = 1.0 / 1000000000.0;
+    if (event.type == SENSOR_TYPE_MAGNETIC_FIELD) {
+        const double now = event.timestamp * NS2S;
+        if (mMagTime == 0) {
+            mMagData[0] = mMX.init(event.magnetic.x);
+            mMagData[1] = mMY.init(event.magnetic.y);
+            mMagData[2] = mMZ.init(event.magnetic.z);
+        } else {
+            double dT = now - mMagTime;
+            mMLowPass.setSamplingPeriod(dT);
+            mMagData[0] = mMX(event.magnetic.x);
+            mMagData[1] = mMY(event.magnetic.y);
+            mMagData[2] = mMZ(event.magnetic.z);
+        }
+        mMagTime = now;
+    }
+    if (event.type == SENSOR_TYPE_ACCELEROMETER) {
+        const double now = event.timestamp * NS2S;
+        float Ax, Ay, Az;
+        if (mAccTime == 0) {
+            Ax = mAX.init(event.acceleration.x);
+            Ay = mAY.init(event.acceleration.y);
+            Az = mAZ.init(event.acceleration.z);
+        } else {
+            double dT = now - mAccTime;
+            mALowPass.setSamplingPeriod(dT);
+            Ax = mAX(event.acceleration.x);
+            Ay = mAY(event.acceleration.y);
+            Az = mAZ(event.acceleration.z);
+        }
+        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 );
+        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 000000000000..e7f28c93a232
--- /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 <stdint.h>
+#include <sys/types.h>
+
+#include <gui/Sensor.h>
+
+#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 000000000000..e13e1362d6dd
--- /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 <stdint.h>
+#include <sys/types.h>
+#include <math.h>
+
+#include <cutils/log.h>
+
+#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 000000000000..998ca35b9b66
--- /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 <stdint.h>
+#include <sys/types.h>
+
+// ---------------------------------------------------------------------------
+
+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 000000000000..73f85ba35ca1
--- /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 <stdint.h>
+#include <math.h>
+#include <sys/types.h>
+
+#include <utils/Atomic.h>
+#include <utils/Errors.h>
+#include <utils/Singleton.h>
+
+#include <binder/BinderService.h>
+#include <binder/Parcel.h>
+#include <binder/IServiceManager.h>
+
+#include <hardware/sensors.h>
+
+#include "SensorDevice.h"
+
+namespace android {
+// ---------------------------------------------------------------------------
+class BatteryService : public Singleton<BatteryService> {
+    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<BatteryService>;
+    sp<IBinder> mBatteryStatService;
+
+    BatteryService() {
+        const sp<IServiceManager> 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 ; i<size_t(count) ; i++) {
+                mActivationCount.add(list[i].handle, model);
+                mSensorDevice->activate(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 ; i<size_t(count) ; i++) {
+        snprintf(buffer, SIZE, "handle=0x%08x, active-count=%d / %d\n",
+                list[i].handle,
+                mActivationCount.valueFor(list[i].handle).count,
+                mActivationCount.valueFor(list[i].handle).rates.size());
+        result.append(buffer);
+    }
+}
+
+ssize_t SensorDevice::getSensorList(sensor_t const** list) {
+    if (!mSensorModule) return NO_INIT;
+    ssize_t count = mSensorModule->get_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 ; i<info.rates.size() ; i++) {
+            if (info.rates.valueAt(i) < ns) {
+                nsecs_t cur = info.rates.valueAt(i);
+                if (cur < ns) {
+                    ns = cur;
+                }
+            }
+        }
+        mSensorDevice->setDelay(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 ; i<info.rates.size() ; i++) {
+            nsecs_t cur = info.rates.valueAt(i);
+            if (cur < ns) {
+                ns = cur;
+            }
+        }
+    }
+    return mSensorDevice->setDelay(mSensorDevice, handle, ns);
+}
+
+// ---------------------------------------------------------------------------
+}; // namespace android
+
diff --git a/services/sensorservice/SensorDevice.h b/services/sensorservice/SensorDevice.h
new file mode 100644
index 000000000000..63ecbcddecc4
--- /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 <stdint.h>
+#include <sys/types.h>
+
+#include <utils/KeyedVector.h>
+#include <utils/Singleton.h>
+#include <utils/String8.h>
+
+#include <gui/Sensor.h>
+
+// ---------------------------------------------------------------------------
+
+namespace android {
+// ---------------------------------------------------------------------------
+
+static const nsecs_t DEFAULT_EVENTS_PERIOD = 200000000; //    5 Hz
+
+class SensorDevice : public Singleton<SensorDevice> {
+    friend class Singleton<SensorDevice>;
+    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<void*, nsecs_t> rates;
+    };
+    DefaultKeyedVector<int, Info> 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 000000000000..93d23d9a40d2
--- /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 <stdint.h>
+#include <sys/types.h>
+
+#include <cutils/log.h>
+
+#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 000000000000..eebd5638f5d0
--- /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 <stdint.h>
+#include <sys/types.h>
+
+#include <utils/Singleton.h>
+
+#include <gui/Sensor.h>
+
+#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 22a45df0603c..ea5e5ccd8e25 100644
--- a/services/sensorservice/SensorService.cpp
+++ b/services/sensorservice/SensorService.cpp
@@ -15,6 +15,7 @@
  */
 
 #include <stdint.h>
+#include <math.h>
 #include <sys/types.h>
 
 #include <utils/SortedVector.h>
@@ -35,80 +36,15 @@
 #include <hardware/sensors.h>
 
 #include "SensorService.h"
+#include "GravitySensor.h"
+#include "LinearAccelerationSensor.h"
+#include "RotationVectorSensor.h"
 
 namespace android {
 // ---------------------------------------------------------------------------
 
-class BatteryService : public Singleton<BatteryService> {
-    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<BatteryService>;
-    sp<IBinder> mBatteryStatService;
-
-    BatteryService() {
-        const sp<IServiceManager> 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);
-
-    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));
+    SensorDevice& dev(SensorDevice::getInstance());
 
-        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<<SENSOR_TYPE_GRAVITY) |
+                (1<<SENSOR_TYPE_LINEAR_ACCELERATION) |
+                (1<<SENSOR_TYPE_ROTATION_VECTOR);
+        sensor_t const* list;
+        int count = dev.getSensorList(&list);
         mLastEventSeen.setCapacity(count);
         for (int i=0 ; i<count ; i++) {
-            Sensor sensor(list + i);
-            LOGI("%s", sensor.getName().string());
-            mSensorList.add(sensor);
-            if (mSensorDevice) {
-                mSensorDevice->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<<list[i].type);
+                    break;
             }
-            mLastEventSeen.add(sensor.getHandle(), event);
         }
 
-        if (mSensorDevice) {
-            run("SensorService", PRIORITY_URGENT_DISPLAY);
-            mInitCheck = NO_ERROR;
+        if (virtualSensorsNeeds & (1<<SENSOR_TYPE_GRAVITY)) {
+            registerVirtualSensor( new GravitySensor(list, count) );
+        }
+        if (virtualSensorsNeeds & (1<<SENSOR_TYPE_LINEAR_ACCELERATION)) {
+            registerVirtualSensor( new LinearAccelerationSensor(list, count) );
+        }
+        if (virtualSensorsNeeds & (1<<SENSOR_TYPE_ROTATION_VECTOR)) {
+            registerVirtualSensor( new RotationVectorSensor(list, count) );
         }
+
+        run("SensorService", PRIORITY_URGENT_DISPLAY);
+        mInitCheck = NO_ERROR;
     }
 }
 
+void SensorService::registerSensor(SensorInterface* s)
+{
+    sensors_event_t event;
+    memset(&event, 0, sizeof(event));
+
+    const Sensor sensor(s->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<mSensorMap.size() ; i++)
+        delete mSensorMap.valueAt(i);
 }
 
 status_t SensorService::dump(int fd, const Vector<String16>& args)
@@ -175,7 +134,7 @@ status_t SensorService::dump(int fd, const Vector<String16>& args)
         for (size_t i=0 ; i<mSensorList.size() ; i++) {
             const Sensor& s(mSensorList[i]);
             const sensors_event_t& e(mLastEventSeen.valueFor(s.getHandle()));
-            snprintf(buffer, SIZE, "%s (vendor=%s, handle=%d, maxRate=%.2fHz, last=<%5.1f,%5.1f,%5.1f>)\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<String16>& 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<String16>& args)
         result.append(buffer);
         for (size_t i=0 ; i<mActiveSensors.size() ; i++) {
             int handle = mActiveSensors.keyAt(i);
-            snprintf(buffer, SIZE, "%s (handle=%d, connections=%d)\n",
+            snprintf(buffer, SIZE, "%s (handle=0x%08x, connections=%d)\n",
                     getSensorName(handle).string(),
                     handle,
                     mActiveSensors.valueAt(i)->getNumConnections());
@@ -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);
 
+        // handle virtual sensors
+        if (count && vcount) {
+            const DefaultKeyedVector<int, SensorInterface*> virtualSensors(
+                    getActiveVirtualSensors());
+            const size_t activeVirtualSensorCount = virtualSensors.size();
+            if (activeVirtualSensorCount) {
+                size_t k = 0;
+                for (size_t i=0 ; i<size_t(count) ; i++) {
+                    sensors_event_t const * const event = buffer;
+                    for (size_t j=0 ; j<activeVirtualSensorCount ; j++) {
+                        sensors_event_t out;
+                        if (virtualSensors.valueAt(j)->process(&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<SensorEventConnection> > activeConnections(
                 getActiveConnections());
-
         size_t numConnections = activeConnections.size();
-        if (numConnections) {
-            for (size_t i=0 ; i<numConnections ; i++) {
-                sp<SensorEventConnection> connection(activeConnections[i].promote());
-                if (connection != 0) {
-                    connection->sendEvents(buffer, count, scratch);
-                }
+        for (size_t i=0 ; i<numConnections ; i++) {
+            sp<SensorEventConnection> 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<sensors_event_t const*>(lhs);
+            sensors_event_t const* r = static_cast<sensors_event_t const*>(rhs);
+            return r->timestamp - l->timestamp;
+        }
+    };
+    qsort(buffer, count, sizeof(sensors_event_t), compar::cmp);
+}
+
 SortedVector< wp<SensorService::SensorEventConnection> >
 SensorService::getActiveConnections() const
 {
@@ -264,6 +263,13 @@ SensorService::getActiveConnections() const
     return mActiveConnections;
 }
 
+DefaultKeyedVector<int, SensorInterface*>
+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<count ; i++) {
@@ -294,8 +300,13 @@ void SensorService::cleanupConnection(const wp<SensorEventConnection>& connectio
     for (size_t i=0 ; i<size ; ) {
         SensorRecord* rec = mActiveSensors.valueAt(i);
         if (rec && rec->removeConnection(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<SensorEventConnection>& 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) {
+        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);
+        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<SensorEventConnection>& 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<SensorEventConnection>& 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<count ; i++) {
-        sp<SensorEventConnection> 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<numEvents) {
             const int32_t curr = buffer[i].sensor;
-            if (mSensorInfo.indexOfKey(curr) >= 0) {
+            if (mSensorInfo.indexOf(curr) >= 0) {
                 do {
                     scratch[count++] = buffer[i++];
                 } while ((i<numEvents) && (buffer[i].sensor == curr));
diff --git a/services/sensorservice/SensorService.h b/services/sensorservice/SensorService.h
index c0922f553c76..540c7e2a5fe8 100644
--- a/services/sensorservice/SensorService.h
+++ b/services/sensorservice/SensorService.h
@@ -34,6 +34,8 @@
 #include <gui/ISensorServer.h>
 #include <gui/ISensorEventConnection.h>
 
+#include "SensorInterface.h"
+
 // ---------------------------------------------------------------------------
 
 struct sensors_poll_device_t;
@@ -50,7 +52,6 @@ class SensorService :
    friend class BinderService<SensorService>;
 
    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<SensorChannel> const mChannel;
         mutable Mutex mConnectionLock;
 
-        // protected mConnectionLock
-        struct SensorInfo {
-            SensorInfo() : ns(DEFAULT_EVENTS_PERIOD) { }
-            nsecs_t ns;
-        };
-        DefaultKeyedVector<int32_t, SensorInfo> mSensorInfo;
+        // protected by SensorService::mLock
+        SortedVector<int> mSensorInfo;
 
     public:
         SensorEventConnection(const sp<SensorService>& 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<SensorEventConnection> > getActiveConnections() const;
-    String8 getSensorName(int handle) const;
-    status_t recomputeEventsPeriodLocked(int32_t handle);
+    DefaultKeyedVector<int, SensorInterface*> 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<Sensor> mSensorList;
-    struct sensors_poll_device_t* mSensorDevice;
-    struct sensors_module_t* mSensorModule;
+    DefaultKeyedVector<int, SensorInterface*> mSensorMap;
+    Vector<SensorInterface *> mVirtualSensorList;
     Permission mDump;
     status_t mInitCheck;
 
     // protected by mLock
     mutable Mutex mLock;
     DefaultKeyedVector<int, SensorRecord*> mActiveSensors;
+    DefaultKeyedVector<int, SensorInterface*> mActiveVirtualSensors;
     SortedVector< wp<SensorEventConnection> > mActiveConnections;
 
     // The size of this vector is constant, only the items are mutable