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

4 files changed, 33 insertions, 2 deletions

diff --git a/services/sensorservice/GravitySensor.cpp b/services/sensorservice/GravitySensor.cpp
index 18bd359..da72f9c 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 f9850b7..ff3bea7 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 e13e136..eeb6d1e 100644
--- a/services/sensorservice/SecondOrderLowPassFilter.cpp
+++ b/services/sensorservice/SecondOrderLowPassFilter.cpp
@@ -67,4 +67,23 @@ float BiquadFilter::operator()(float x)
 }
 
 // ---------------------------------------------------------------------------
+
+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 998ca35..85698ca 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