Permalink
Browse files

Sensors: backport of virtual RotationVector from ICS

- attempt to fix the magnetometer issues in some apps

Change-Id: I2999cba68367283613d4b357e533306a8b931a92
  • Loading branch information...
1 parent 5975567 commit 5951c645df4aaea7a56f0f7fcf14c309bd635805 @C3C0 committed May 24, 2012
@@ -5,6 +5,7 @@ LOCAL_SRC_FILES:= \
GravitySensor.cpp \
LinearAccelerationSensor.cpp \
RotationVectorSensor.cpp \
+ RotationVectorSensor2.cpp \
SensorService.cpp \
SensorInterface.cpp \
SensorDevice.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
+
@@ -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
@@ -39,6 +39,7 @@
#include "GravitySensor.h"
#include "LinearAccelerationSensor.h"
#include "RotationVectorSensor.h"
+#include "RotationVectorSensor2.h"
namespace android {
// ---------------------------------------------------------------------------
@@ -57,36 +58,54 @@ void SensorService::onFirstRef()
SensorDevice& dev(SensorDevice::getInstance());
if (dev.initCheck() == NO_ERROR) {
- uint32_t virtualSensorsNeeds =
- (1<<SENSOR_TYPE_GRAVITY) |
- (1<<SENSOR_TYPE_LINEAR_ACCELERATION) |
- (1<<SENSOR_TYPE_ROTATION_VECTOR);
sensor_t const* list;
- int count = dev.getSensorList(&list);
- mLastEventSeen.setCapacity(count);
- for (int i=0 ; i<count ; i++) {
- registerSensor( new HardwareSensor(list[i]) );
- switch (list[i].type) {
- case SENSOR_TYPE_GRAVITY:
- case SENSOR_TYPE_LINEAR_ACCELERATION:
- case SENSOR_TYPE_ROTATION_VECTOR:
- virtualSensorsNeeds &= ~(1<<list[i].type);
- break;
+ ssize_t count = dev.getSensorList(&list);
+ if (count > 0) {
+ ssize_t orientationIndex = -1;
+ bool hasGyro = false;
+ uint32_t virtualSensorsNeeds =
+ (1<<SENSOR_TYPE_GRAVITY) |
+ (1<<SENSOR_TYPE_LINEAR_ACCELERATION) |
+ (1<<SENSOR_TYPE_ROTATION_VECTOR);
+
+ mLastEventSeen.setCapacity(count);
+ for (ssize_t i=0 ; i<count ; i++) {
+ registerSensor( new HardwareSensor(list[i]) );
+ switch (list[i].type) {
+ case SENSOR_TYPE_ORIENTATION:
+ orientationIndex = i;
+ break;
+ case SENSOR_TYPE_GYROSCOPE:
+ hasGyro = true;
+ break;
+ case SENSOR_TYPE_GRAVITY:
+ case SENSOR_TYPE_LINEAR_ACCELERATION:
+ case SENSOR_TYPE_ROTATION_VECTOR:
+ virtualSensorsNeeds &= ~(1<<list[i].type);
+ break;
+ }
}
- }
- if (virtualSensorsNeeds & (1<<SENSOR_TYPE_GRAVITY)) {
- registerVirtualSensor( new GravitySensor(list, count) );
- }
- if (virtualSensorsNeeds & (1<<SENSOR_TYPE_LINEAR_ACCELERATION)) {
- registerVirtualSensor( new LinearAccelerationSensor(list, count) );
- }
- if (virtualSensorsNeeds & (1<<SENSOR_TYPE_ROTATION_VECTOR)) {
- registerVirtualSensor( new RotationVectorSensor(list, count) );
- }
+ if (hasGyro) {
+ // Always instantiate Android's virtual sensors. Since they are
+ // instantiated behind sensors from the HAL, they won't
+ // interfere with applications, unless they looks specifically
+ // for them (by name).
+
+ registerVirtualSensor( new RotationVectorSensor(list, count) );
+ registerVirtualSensor( new GravitySensor(list, count) );
+ registerVirtualSensor( new LinearAccelerationSensor(list, count) );
+ } 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() );
+ }
- run("SensorService", PRIORITY_URGENT_DISPLAY);
- mInitCheck = NO_ERROR;
+ run("SensorService", PRIORITY_URGENT_DISPLAY);
+ mInitCheck = NO_ERROR;
+ }
}
}
@@ -184,13 +203,19 @@ bool SensorService::threadLoop()
// handle virtual sensors
if (count && vcount) {
+ sensors_event_t const * const event = buffer;
const DefaultKeyedVector<int, SensorInterface*> virtualSensors(
getActiveVirtualSensors());
const size_t activeVirtualSensorCount = virtualSensors.size();
if (activeVirtualSensorCount) {
size_t k = 0;
+ 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++) {
- sensors_event_t const * const event = buffer;
for (size_t j=0 ; j<activeVirtualSensorCount ; j++) {
sensors_event_t out;
if (virtualSensors.valueAt(j)->process(&out, event[i])) {
Oops, something went wrong. Retry.

0 comments on commit 5951c64

Please sign in to comment.