diff --git a/services/sensorservice/GravitySensor.cpp b/services/sensorservice/GravitySensor.cpp index 18bd359ea..da72f9c4b 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 f9850b75f..ff3bea77e 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 e13e1362d..eeb6d1e84 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 998ca35b9..85698ca63 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