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

@@ -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)
 
 {

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);

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

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