9-axis sensor fusion with Kalman filter (73e0bc80) · Commits · e / os / android_frameworks_base

include/gui/Sensor.h

+5 −1

Original line number	Diff line number	Diff line
		@@ -21,8 +21,9 @@
		#include <sys/types.h>

		#include <utils/Errors.h>
		#include <utils/String8.h>
		#include <utils/Flattenable.h>
		#include <utils/String8.h>
		#include <utils/Timers.h>

		#include <hardware/sensors.h>

		@@ -64,6 +65,8 @@ public:
		float getResolution() const;
		float getPowerUsage() const;
		int32_t getMinDelay() const;
		nsecs_t getMinDelayNs() const;
		int32_t getVersion() const;

		// Flattenable interface
		virtual size_t getFlattenedSize() const;
		@@ -83,6 +86,7 @@ private:
		float mResolution;
		float mPower;
		int32_t mMinDelay;
		int32_t mVersion;
		};

		// ----------------------------------------------------------------------------

libs/gui/Sensor.cpp

+8 −0

Original line number	Diff line number	Diff line
		@@ -89,6 +89,14 @@ int32_t Sensor::getMinDelay() const {
		return mMinDelay;
		}

		nsecs_t Sensor::getMinDelayNs() const {
		return getMinDelay() * 1000;
		}

		int32_t Sensor::getVersion() const {
		return mVersion;
		}

		size_t Sensor::getFlattenedSize() const
		{
		return sizeof(int32_t) + ((mName.length() + 3) & ~3) +

services/sensorservice/Android.mk

+11 −6

Original line number	Diff line number	Diff line
		@@ -2,13 +2,18 @@ LOCAL_PATH:= $(call my-dir)
		include $(CLEAR_VARS)

		LOCAL_SRC_FILES:= \
		CorrectedGyroSensor.cpp \
		Fusion.cpp \
		GravitySensor.cpp \
		LinearAccelerationSensor.cpp \
		OrientationSensor.cpp \
		RotationVectorSensor.cpp \
		SensorService.cpp \
		SensorInterface.cpp \
		SecondOrderLowPassFilter.cpp \
		SensorDevice.cpp \
		SecondOrderLowPassFilter.cpp
		SensorFusion.cpp \
		SensorInterface.cpp \
		SensorService.cpp \


		LOCAL_CFLAGS:= -DLOG_TAG=\"SensorService\"

services/sensorservice/CorrectedGyroSensor.cpp

0 → 100644

+86 −0

Original line number	Diff line number	Diff line
		/*
		* Copyright (C) 2011 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 "CorrectedGyroSensor.h"
		#include "SensorDevice.h"
		#include "SensorFusion.h"

		namespace android {
		// ---------------------------------------------------------------------------

		CorrectedGyroSensor::CorrectedGyroSensor(sensor_t const* list, size_t count)
		: mSensorDevice(SensorDevice::getInstance()),
		mSensorFusion(SensorFusion::getInstance())
		{
		for (size_t i=0 ; i<count ; i++) {
		if (list[i].type == SENSOR_TYPE_GYROSCOPE) {
		mGyro = Sensor(list + i);
		break;
		}
		}
		}

		bool CorrectedGyroSensor::process(sensors_event_t* outEvent,
		const sensors_event_t& event)
		{
		if (event.type == SENSOR_TYPE_GYROSCOPE) {
		const vec3_t bias(mSensorFusion.getGyroBias() * mSensorFusion.getEstimatedRate());
		*outEvent = event;
		outEvent->data[0] -= bias.x;
		outEvent->data[1] -= bias.y;
		outEvent->data[2] -= bias.z;
		outEvent->sensor = '_cgy';
		return true;
		}
		return false;
		}

		status_t CorrectedGyroSensor::activate(void* ident, bool enabled) {
		mSensorDevice.activate(this, mGyro.getHandle(), enabled);
		return mSensorFusion.activate(this, enabled);
		}

		status_t CorrectedGyroSensor::setDelay(void* ident, int handle, int64_t ns) {
		mSensorDevice.setDelay(this, mGyro.getHandle(), ns);
		return mSensorFusion.setDelay(this, ns);
		}

		Sensor CorrectedGyroSensor::getSensor() const {
		sensor_t hwSensor;
		hwSensor.name = "Corrected Gyroscope Sensor";
		hwSensor.vendor = "Google Inc.";
		hwSensor.version = 1;
		hwSensor.handle = '_cgy';
		hwSensor.type = SENSOR_TYPE_GYROSCOPE;
		hwSensor.maxRange = mGyro.getMaxValue();
		hwSensor.resolution = mGyro.getResolution();
		hwSensor.power = mSensorFusion.getPowerUsage();
		hwSensor.minDelay = mGyro.getMinDelay();
		Sensor sensor(&hwSensor);
		return sensor;
		}

		// ---------------------------------------------------------------------------
		}; // namespace android

services/sensorservice/CorrectedGyroSensor.h

0 → 100644

+52 −0

Original line number	Diff line number	Diff line
		/*
		* Copyright (C) 2011 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_CORRECTED_GYRO_SENSOR_H
		#define ANDROID_CORRECTED_GYRO_SENSOR_H

		#include <stdint.h>
		#include <sys/types.h>

		#include <gui/Sensor.h>

		#include "SensorInterface.h"

		// ---------------------------------------------------------------------------
		namespace android {
		// ---------------------------------------------------------------------------

		class SensorDevice;
		class SensorFusion;

		class CorrectedGyroSensor : public SensorInterface {
		SensorDevice& mSensorDevice;
		SensorFusion& mSensorFusion;
		Sensor mGyro;

		public:
		CorrectedGyroSensor(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_CORRECTED_GYRO_SENSOR_H