From 6038db3dda36198aa8e0599dcb34033da31c85f5 Mon Sep 17 00:00:00 2001
From: Mathias Agopian <mathias@google.com>
Date: Thu, 11 Nov 2010 17:58:51 -0800
Subject: [PATCH] Uses 4-th order low-pass for extracting gravity.

Most accelerometers have 8-bits accuracy so we beed to
reject 48dB in thestop-band, which requires a 4-th order
filter at the cut-off frequency we're using.

Change-Id: Ic00421d38d751641f86b1f3ad7663e6b44a91198
---
 services/sensorservice/GravitySensor.cpp      |  2 +-
 services/sensorservice/GravitySensor.h        |  2 +-
 .../SecondOrderLowPassFilter.cpp              | 19 +++++++++++++++++++
 .../sensorservice/SecondOrderLowPassFilter.h  | 12 ++++++++++++
 4 files changed, 33 insertions(+), 2 deletions(-)

diff --git a/services/sensorservice/GravitySensor.cpp b/services/sensorservice/GravitySensor.cpp
index 18bd359ea27d7..da72f9c4b5ee8 100644
--- a/services/sensorservice/GravitySensor.cpp
+++ b/services/sensorservice/GravitySensor.cpp
@@ -30,7 +30,7 @@ namespace android {
 GravitySensor::GravitySensor(sensor_t const* list, size_t count)
     : mSensorDevice(SensorDevice::getInstance()),
       mEnabled(false), mAccTime(0),
-      mLowPass(M_SQRT1_2, 1),
+      mLowPass(M_SQRT1_2, 1.5f),
       mX(mLowPass), mY(mLowPass), mZ(mLowPass)
 
 {
diff --git a/services/sensorservice/GravitySensor.h b/services/sensorservice/GravitySensor.h
index f9850b75f34ff..ff3bea77e5c16 100644
--- a/services/sensorservice/GravitySensor.h
+++ b/services/sensorservice/GravitySensor.h
@@ -37,7 +37,7 @@ class GravitySensor : public SensorInterface {
     double mAccTime;
 
     SecondOrderLowPassFilter mLowPass;
-    BiquadFilter mX, mY, mZ;
+    CascadedBiquadFilter mX, mY, mZ;
 
 public:
     GravitySensor(sensor_t const* list, size_t count);
diff --git a/services/sensorservice/SecondOrderLowPassFilter.cpp b/services/sensorservice/SecondOrderLowPassFilter.cpp
index e13e1362d6dd4..eeb6d1e84b0d4 100644
--- a/services/sensorservice/SecondOrderLowPassFilter.cpp
+++ b/services/sensorservice/SecondOrderLowPassFilter.cpp
@@ -66,5 +66,24 @@ float BiquadFilter::operator()(float x)
     return y;
 }
 
+// ---------------------------------------------------------------------------
+
+CascadedBiquadFilter::CascadedBiquadFilter(const SecondOrderLowPassFilter& s)
+    : mA(s), mB(s)
+{
+}
+
+float CascadedBiquadFilter::init(float x)
+{
+    mA.init(x);
+    mB.init(x);
+    return x;
+}
+
+float CascadedBiquadFilter::operator()(float x)
+{
+    return mB(mA(x));
+}
+
 // ---------------------------------------------------------------------------
 }; // namespace android
diff --git a/services/sensorservice/SecondOrderLowPassFilter.h b/services/sensorservice/SecondOrderLowPassFilter.h
index 998ca35b9b660..85698ca636bbb 100644
--- a/services/sensorservice/SecondOrderLowPassFilter.h
+++ b/services/sensorservice/SecondOrderLowPassFilter.h
@@ -54,6 +54,18 @@ public:
     float operator()(float in);
 };
 
+/*
+ * Two cascaded biquad IIR filters
+ * (4-poles IIR)
+ */
+class CascadedBiquadFilter {
+    BiquadFilter mA;
+    BiquadFilter mB;
+public:
+    CascadedBiquadFilter(const SecondOrderLowPassFilter& s);
+    float init(float in);
+    float operator()(float in);
+};
 
 // ---------------------------------------------------------------------------
 }; // namespace android