diff options
| -rw-r--r-- | services/sensorservice/Android.mk | 1 | ||||
| -rw-r--r-- | services/sensorservice/RotationVectorSensor2.cpp | 126 | ||||
| -rw-r--r-- | services/sensorservice/RotationVectorSensor2.h | 63 | ||||
| -rw-r--r-- | services/sensorservice/SensorService.cpp | 13 |
4 files changed, 203 insertions, 0 deletions
diff --git a/services/sensorservice/Android.mk b/services/sensorservice/Android.mk index e0cfaa6..773eca5 100644 --- a/services/sensorservice/Android.mk +++ b/services/sensorservice/Android.mk @@ -9,6 +9,7 @@ LOCAL_SRC_FILES:= \ LinearAccelerationSensor.cpp \ OrientationSensor.cpp \ RotationVectorSensor.cpp \ + RotationVectorSensor2.cpp \ SensorDevice.cpp \ SensorFusion.cpp \ SensorInterface.cpp \ diff --git a/services/sensorservice/RotationVectorSensor2.cpp b/services/sensorservice/RotationVectorSensor2.cpp new file mode 100644 index 0000000..f2873dd --- /dev/null +++ b/services/sensorservice/RotationVectorSensor2.cpp @@ -0,0 +1,126 @@ +/* + * 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] = c1*c2*c3 - s1*s2*s3; + mData[1] = c1*s2*c3 - s1*c2*s3; + mData[2] = c1*c2*s3 + s1*s2*c3; + mData[3] = s1*c2*c3 + c1*s2*s3; + + // 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 + diff --git a/services/sensorservice/RotationVectorSensor2.h b/services/sensorservice/RotationVectorSensor2.h new file mode 100644 index 0000000..872ec12 --- /dev/null +++ b/services/sensorservice/RotationVectorSensor2.h @@ -0,0 +1,63 @@ +/* + * 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 diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp index e3dcd02..8ff0872 100644 --- a/services/sensorservice/SensorService.cpp +++ b/services/sensorservice/SensorService.cpp @@ -45,6 +45,7 @@ #include "LinearAccelerationSensor.h" #include "OrientationSensor.h" #include "RotationVectorSensor.h" +#include "RotationVectorSensor2.h" #include "SensorFusion.h" #include "SensorService.h" @@ -136,6 +137,12 @@ void SensorService::onFirstRef() if (orientationIndex >= 0) { mUserSensorList.removeItemsAt(orientationIndex); } + } 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() ); } // debugging sensor list @@ -272,6 +279,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) && k<minBufferSize ; i++) { for (size_t j=0 ; j<activeVirtualSensorCount ; j++) { if (count + k >= minBufferSize) { |