summaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
-rw-r--r--services/sensorservice/Android.mk7
-rw-r--r--services/sensorservice/SensorService.cpp9
-rw-r--r--services/sensorservice/legacy/LegacyGravitySensor.cpp102
-rw-r--r--services/sensorservice/legacy/LegacyGravitySensor.h57
-rw-r--r--services/sensorservice/legacy/LegacyLinearAccelerationSensor.cpp84
-rw-r--r--services/sensorservice/legacy/LegacyLinearAccelerationSensor.h53
-rw-r--r--services/sensorservice/legacy/LegacyRotationVectorSensor.cpp156
-rw-r--r--services/sensorservice/legacy/LegacyRotationVectorSensor.h60
-rw-r--r--services/sensorservice/legacy/SecondOrderLowPassFilter.cpp89
-rw-r--r--services/sensorservice/legacy/SecondOrderLowPassFilter.h73
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