Merge "sensorservice: Add legacy sensors fusion." into cm-10.1

10 files changed, 690 insertions, 0 deletions

diff --git a/services/sensorservice/Android.mk b/services/sensorservice/Android.mk
index 40fe6e1..c5926c6 100644
--- a/services/sensorservice/Android.mk
+++ b/services/sensorservice/Android.mk
@@ -15,6 +15,13 @@ LOCAL_SRC_FILES:= \
     SensorInterface.cpp \
     SensorService.cpp \
 
+# Legacy virtual sensors used in combination from accelerometer & magnetometer.
+LOCAL_SRC_FILES += \
+	legacy/SecondOrderLowPassFilter.cpp \
+	legacy/LegacyGravitySensor.cpp \
+	legacy/LegacyLinearAccelerationSensor.cpp \
+	legacy/LegacyRotationVectorSensor.cpp
+
 
 LOCAL_CFLAGS:= -DLOG_TAG=\"SensorService\"
 
diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp
index 8ff0872..be1c150 100644
--- a/services/sensorservice/SensorService.cpp
+++ b/services/sensorservice/SensorService.cpp
@@ -48,6 +48,9 @@
 #include "RotationVectorSensor2.h"
 #include "SensorFusion.h"
 #include "SensorService.h"
+#include "legacy/LegacyGravitySensor.h"
+#include "legacy/LegacyLinearAccelerationSensor.h"
+#include "legacy/LegacyRotationVectorSensor.h"
 
 namespace android {
 // ---------------------------------------------------------------------------
@@ -120,6 +123,12 @@ void SensorService::onFirstRef()
                 registerVirtualSensor( new OrientationSensor() );
                 registerVirtualSensor( new CorrectedGyroSensor(list, count) );
             }
+            else
+            {
+                registerVirtualSensor( new LegacyRotationVectorSensor() );
+                registerVirtualSensor( new LegacyGravitySensor(list, count) );
+                registerVirtualSensor( new LegacyLinearAccelerationSensor(list, count) );
+            }
 
             // build the sensor list returned to users
             mUserSensorList = mSensorList;
diff --git a/services/sensorservice/legacy/LegacyGravitySensor.cpp b/services/sensorservice/legacy/LegacyGravitySensor.cpp
new file mode 100644
index 0000000..e600e6a
--- /dev/null
+++ b/services/sensorservice/legacy/LegacyGravitySensor.cpp
@@ -0,0 +1,102 @@
+/*
+ * 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 "LegacyGravitySensor.h"
+#include "SensorDevice.h"
+#include "SensorFusion.h"
+
+namespace android {
+// ---------------------------------------------------------------------------
+
+LegacyGravitySensor::LegacyGravitySensor(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 ; i<count ; i++) {
+        if (list[i].type == SENSOR_TYPE_ACCELEROMETER) {
+            mAccelerometer = Sensor(list + i);
+            break;
+        }
+    }
+}
+
+bool LegacyGravitySensor::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;
+}
+
+status_t LegacyGravitySensor::activate(void* ident, bool enabled) {
+    return mSensorFusion.activate(this, enabled);
+}
+
+status_t LegacyGravitySensor::setDelay(void* ident, int handle, int64_t ns) {
+    return mSensorFusion.setDelay(this, ns);
+}
+
+Sensor LegacyGravitySensor::getSensor() const {
+    sensor_t hwSensor;
+    hwSensor.name       = "Gravity Sensor";
+    hwSensor.vendor     = "Google Inc.";
+    hwSensor.version    = 3;
+    hwSensor.handle     = '_grv';
+    hwSensor.type       = SENSOR_TYPE_GRAVITY;
+    hwSensor.maxRange   = GRAVITY_EARTH * 2;
+    hwSensor.resolution = mAccelerometer.getResolution();
+    hwSensor.power      = mSensorFusion.getPowerUsage();
+    hwSensor.minDelay   = mSensorFusion.getMinDelay();
+    Sensor sensor(&hwSensor);
+    return sensor;
+}
+
+// ---------------------------------------------------------------------------
+}; // namespace android
+
diff --git a/services/sensorservice/legacy/LegacyGravitySensor.h b/services/sensorservice/legacy/LegacyGravitySensor.h
new file mode 100644
index 0000000..afe37a9
--- /dev/null
+++ b/services/sensorservice/legacy/LegacyGravitySensor.h
@@ -0,0 +1,57 @@
+/*
+ * 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_LEGACY_GRAVITY_SENSOR_H
+#define ANDROID_LEGACY_GRAVITY_SENSOR_H
+
+#include <stdint.h>
+#include <sys/types.h>
+
+#include <gui/Sensor.h>
+
+#include "../SensorInterface.h"
+#include "SecondOrderLowPassFilter.h"
+
+// ---------------------------------------------------------------------------
+namespace android {
+// ---------------------------------------------------------------------------
+
+class SensorDevice;
+class SensorFusion;
+
+class LegacyGravitySensor : public SensorInterface {
+    SensorDevice& mSensorDevice;
+    SensorFusion& mSensorFusion;
+    Sensor mAccelerometer;
+    double mAccTime;
+
+    SecondOrderLowPassFilter mLowPass;
+    CascadedBiquadFilter mX, mY, mZ;
+
+public:
+    LegacyGravitySensor(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_LEGACY_GRAVITY_SENSOR_H
diff --git a/services/sensorservice/legacy/LegacyLinearAccelerationSensor.cpp b/services/sensorservice/legacy/LegacyLinearAccelerationSensor.cpp
new file mode 100644
index 0000000..f4e7604
--- /dev/null
+++ b/services/sensorservice/legacy/LegacyLinearAccelerationSensor.cpp
@@ -0,0 +1,84 @@
+/*
+ * 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 "LegacyLinearAccelerationSensor.h"
+#include "../SensorDevice.h"
+#include "../SensorFusion.h"
+
+namespace android {
+// ---------------------------------------------------------------------------
+
+LegacyLinearAccelerationSensor::LegacyLinearAccelerationSensor(sensor_t const* list, size_t count)
+    : mSensorDevice(SensorDevice::getInstance()),
+      mGravitySensor(list, count)
+{
+    mData[0] = mData[1] = mData[2] = 0;
+}
+
+bool LegacyLinearAccelerationSensor::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;
+}
+
+status_t LegacyLinearAccelerationSensor::activate(void* ident, bool enabled) {
+    return mGravitySensor.activate(this, enabled);
+}
+
+status_t LegacyLinearAccelerationSensor::setDelay(void* ident, int handle, int64_t ns) {
+    return mGravitySensor.setDelay(this, handle, ns);
+}
+
+Sensor LegacyLinearAccelerationSensor::getSensor() const {
+    Sensor gsensor(mGravitySensor.getSensor());
+    sensor_t hwSensor;
+    hwSensor.name       = "Linear Acceleration Sensor";
+    hwSensor.vendor     = "Google Inc.";
+    hwSensor.version    = gsensor.getVersion();
+    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/legacy/LegacyLinearAccelerationSensor.h b/services/sensorservice/legacy/LegacyLinearAccelerationSensor.h
new file mode 100644
index 0000000..90ad9fb
--- /dev/null
+++ b/services/sensorservice/legacy/LegacyLinearAccelerationSensor.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_LEGACY_LINEAR_ACCELERATION_SENSOR_H
+#define ANDROID_LEGACY_LINEAR_ACCELERATION_SENSOR_H
+
+#include <stdint.h>
+#include <sys/types.h>
+
+#include <gui/Sensor.h>
+
+#include "../SensorInterface.h"
+#include "LegacyGravitySensor.h"
+
+// ---------------------------------------------------------------------------
+namespace android {
+// ---------------------------------------------------------------------------
+
+class SensorDevice;
+class SensorFusion;
+
+class LegacyLinearAccelerationSensor : public SensorInterface {
+    SensorDevice& mSensorDevice;
+    LegacyGravitySensor mGravitySensor;
+    float mData[3];
+
+    virtual bool process(sensors_event_t* outEvent,
+            const sensors_event_t& event);
+public:
+    LegacyLinearAccelerationSensor(sensor_t const* list, size_t count);
+    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_LEGACY_LINEAR_ACCELERATION_SENSOR_H
diff --git a/services/sensorservice/legacy/LegacyRotationVectorSensor.cpp b/services/sensorservice/legacy/LegacyRotationVectorSensor.cpp
new file mode 100644
index 0000000..a58878d
--- /dev/null
+++ b/services/sensorservice/legacy/LegacyRotationVectorSensor.cpp
@@ -0,0 +1,156 @@
+/*
+ * 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 "LegacyRotationVectorSensor.h"
+
+namespace android {
+// ---------------------------------------------------------------------------
+
+template <typename T>
+static inline T clamp(T v) {
+    return v < 0 ? 0 : v;
+}
+
+LegacyRotationVectorSensor::LegacyRotationVectorSensor()
+    : mSensorDevice(SensorDevice::getInstance()),
+      mSensorFusion(SensorFusion::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)
+{
+}
+
+bool LegacyRotationVectorSensor::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 );
+        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 LegacyRotationVectorSensor::activate(void* ident, bool enabled) {
+    if (enabled) {
+        mMagTime = 0;
+        mAccTime = 0;
+    }
+    return mSensorFusion.activate(this, enabled);
+}
+
+status_t LegacyRotationVectorSensor::setDelay(void* ident, int handle, int64_t ns) {
+    return mSensorFusion.setDelay(this, ns);
+}
+
+Sensor LegacyRotationVectorSensor::getSensor() const {
+    sensor_t hwSensor;
+    hwSensor.name       = "Rotation Vector Sensor";
+    hwSensor.vendor     = "Google Inc.";
+    hwSensor.version    = 3;
+    hwSensor.handle     = '_rov';
+    hwSensor.type       = SENSOR_TYPE_ROTATION_VECTOR;
+    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/legacy/LegacyRotationVectorSensor.h b/services/sensorservice/legacy/LegacyRotationVectorSensor.h
new file mode 100644
index 0000000..4208819
--- /dev/null
+++ b/services/sensorservice/legacy/LegacyRotationVectorSensor.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_LEGACY_ROTATION_VECTOR_SENSOR_H
+#define ANDROID_LEGACY_ROTATION_VECTOR_SENSOR_H
+
+#include <stdint.h>
+#include <sys/types.h>
+
+#include <gui/Sensor.h>
+
+#include "../SensorDevice.h"
+#include "../SensorInterface.h"
+
+#include "../Fusion.h"
+#include "../SensorFusion.h"
+#include "SecondOrderLowPassFilter.h"
+
+// ---------------------------------------------------------------------------
+namespace android {
+// ---------------------------------------------------------------------------
+
+class LegacyRotationVectorSensor : public SensorInterface {
+    SensorDevice& mSensorDevice;
+    SensorFusion& mSensorFusion;
+    float mMagData[3];
+    double mAccTime;
+    double mMagTime;
+    SecondOrderLowPassFilter mALowPass;
+    CascadedBiquadFilter mAX, mAY, mAZ;
+    SecondOrderLowPassFilter mMLowPass;
+    CascadedBiquadFilter mMX, mMY, mMZ;
+
+public:
+    LegacyRotationVectorSensor();
+    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_LEGACY_ROTATION_VECTOR_SENSOR_H
diff --git a/services/sensorservice/legacy/SecondOrderLowPassFilter.cpp b/services/sensorservice/legacy/SecondOrderLowPassFilter.cpp
new file mode 100644
index 0000000..eeb6d1e
--- /dev/null
+++ b/services/sensorservice/legacy/SecondOrderLowPassFilter.cpp
@@ -0,0 +1,89 @@
+/*
+ * 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;
+}
+
+// ---------------------------------------------------------------------------
+
+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/legacy/SecondOrderLowPassFilter.h b/services/sensorservice/legacy/SecondOrderLowPassFilter.h
new file mode 100644
index 0000000..85698ca
--- /dev/null
+++ b/services/sensorservice/legacy/SecondOrderLowPassFilter.h
@@ -0,0 +1,73 @@
+/*
+ * 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);
+};
+
+/*
+ * 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
+
+#endif // ANDROID_SECOND_ORDER_LOW_PASS_FILTER_H