Merge "Sensor: Add RotationVectorSensor2" into ics (f1dc3c9d) · Commits · e / os / android_frameworks_base

services/sensorservice/Android.mk

+1 −0

Original line number	Diff line number	Diff line
		@@ -8,6 +8,7 @@ LOCAL_SRC_FILES:= \
		LinearAccelerationSensor.cpp \
		OrientationSensor.cpp \
		RotationVectorSensor.cpp \
		RotationVectorSensor2.cpp \
		SensorDevice.cpp \
		SensorFusion.cpp \
		SensorInterface.cpp \

services/sensorservice/RotationVectorSensor2.cpp

0 → 100644

+126 −0

Original line number	Diff line number	Diff line
		/*
		* Copyright (C) 2010 The Android Open Source Project
		* Copyright (C) 2012 The CyanogenMod 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 "RotationVectorSensor2.h"
		#include "vec.h"

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

		ANDROID_SINGLETON_STATIC_INSTANCE(RotationVectorSensor2)

		RotationVectorSensor2::RotationVectorSensor2()
		: mSensorDevice(SensorDevice::getInstance()),
		mEnabled(false), mHasData(false)
		{
		sensor_t const* list;
		ssize_t count = mSensorDevice.getSensorList(&list);
		if (count > 0) {
		for (size_t i=0 ; i<size_t(count) ; i++) {
		if (list[i].type == SENSOR_TYPE_ORIENTATION) {
		mOrientation = Sensor(list + i);
		}
		}
		}
		}

		bool RotationVectorSensor2::process(sensors_event_t* outEvent,
		const sensors_event_t& event)
		{
		if (mHasData && event.type == SENSOR_TYPE_ACCELEROMETER) {
		*outEvent = event;
		outEvent->data[0] = mData[1];
		outEvent->data[1] = mData[2];
		outEvent->data[2] = mData[3];
		outEvent->data[3] = mData[0];
		outEvent->sensor = '_rv2';
		outEvent->type = SENSOR_TYPE_ROTATION_VECTOR;

		mHasData = false;
		return true;
		}
		return false;
		}

		status_t RotationVectorSensor2::activate(void* ident, bool enabled) {
		mEnabled = enabled;
		return mSensorDevice.activate(this, mOrientation.getHandle(), enabled);
		}

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

		Sensor RotationVectorSensor2::getSensor() const {
		sensor_t hwSensor;
		hwSensor.name = "Rotation Vector Sensor 2";
		hwSensor.vendor = "CyanogenMod Project";
		hwSensor.version = 1;
		hwSensor.handle = '_rv2';
		hwSensor.type = SENSOR_TYPE_ROTATION_VECTOR;
		hwSensor.maxRange = 1;
		hwSensor.resolution = 1.0f / (1<<24);
		hwSensor.power = mOrientation.getPowerUsage();
		hwSensor.minDelay = mOrientation.getMinDelay();
		Sensor sensor(&hwSensor);
		return sensor;
		}

		void RotationVectorSensor2::process(const sensors_event_t& event) {
		if (event.type == SENSOR_TYPE_ORIENTATION) {
		const vec3_t v(event.data);

		// Convert euler angle to quarternion
		const float deg2rad = M_PI / 180;
		float halfAzi = (v[0] / 2) * deg2rad;
		float halfPitch = (v[1] / 2) * deg2rad;
		float halfRoll = (-v[2] / 2) * deg2rad; // roll is reverse

		float c1 = cosf(halfAzi);
		float s1 = sinf(halfAzi);
		float c2 = cosf(halfPitch);
		float s2 = sinf(halfPitch);
		float c3 = cosf(halfRoll);
		float s3 = sinf(halfRoll);
		mData[0] = c1c2c3 - s1s2s3;
		mData[1] = c1s2c3 - s1c2s3;
		mData[2] = c1c2s3 + s1s2c3;
		mData[3] = s1c2c3 + c1s2s3;

		// Misc fixes (a.k.a. "magic")
		if (v[0] < 180) {
		mData[1] = -mData[1];
		mData[3] = -mData[3];
		} else {
		mData[2] = -mData[2];
		}

		mHasData = true;
		}
		}

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

services/sensorservice/RotationVectorSensor2.h

0 → 100644

+63 −0

Original line number	Diff line number	Diff line
		/*
		* Copyright (C) 2010 The Android Open Source Project
		* Copyright (C) 2012 The CyanogenMod 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_ROTATION_VECTOR_SENSOR2_H
		#define ANDROID_ROTATION_VECTOR_SENSOR2_H

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

		#include <gui/Sensor.h>

		#include "SensorDevice.h"
		#include "SensorInterface.h"

		#include "quat.h"

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

		class RotationVectorSensor2 : public SensorInterface,
		public Singleton<RotationVectorSensor2> {
		friend class Singleton<RotationVectorSensor2>;

		SensorDevice& mSensorDevice;

		Sensor mOrientation;
		bool mEnabled;
		bool mHasData;
		quat_t mData;

		public:
		RotationVectorSensor2();
		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; }
		bool isEnabled() const { return mEnabled; }

		// Incoming data
		void process(const sensors_event_t& event);
		};

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

		#endif // ANDROID_ROTATION_VECTOR2_SENSOR_H

services/sensorservice/SensorService.cpp

+13 −0

Original line number	Diff line number	Diff line
		@@ -43,6 +43,7 @@
		#include "LinearAccelerationSensor.h"
		#include "OrientationSensor.h"
		#include "RotationVectorSensor.h"
		#include "RotationVectorSensor2.h"
		#include "SensorFusion.h"
		#include "SensorService.h"

		@@ -123,6 +124,12 @@ void SensorService::onFirstRef()
		if (atoi(value)) {
		registerVirtualSensor( new GyroDriftSensor() );
		}
		} else if (orientationIndex != -1) {
		// If we don't have a gyro but have a orientation sensor from
		// elsewhere, we can compute rotation vector from that.
		// (Google Maps expects rotation vector sensor to exist.)

		registerVirtualSensor( &RotationVectorSensor2::getInstance() );
		}

		// build the sensor list returned to users
		@@ -254,6 +261,12 @@ bool SensorService::threadLoop()
		fusion.process(event[i]);
		}
		}
		RotationVectorSensor2& rv2(RotationVectorSensor2::getInstance());
		if (rv2.isEnabled()) {
		for (size_t i=0 ; i<size_t(count) ; i++) {
		rv2.process(event[i]);
		}
		}
		for (size_t i=0 ; i<size_t(count) ; i++) {
		for (size_t j=0 ; j<activeVirtualSensorCount ; j++) {
		sensors_event_t out;