Donate to e Foundation | Murena handsets with /e/OS | Own a part of Murena! Learn more

Commit a299428e authored by Pawit Pornkitprasan's avatar Pawit Pornkitprasan Committed by Ricardo Cerqueira
Browse files

Sensor: Add RotationVectorSensor2 

Some ICS apps (namely, Google Maps) expects a rotation vector to be
available. Newer devices, this is provided by either Android's
sensor fusion (requires Gyro) or by hardware sensor fusion (MPL).

Older devices will lack this virtual sensor and compass in Google
Maps will not work. To fix this, we can provide our own rotation
vector sensor by converting the values from the orientation sensor.
(They are basically the same information in different formats.)

Thanks to Unhelpful for the help with related math.

Change-Id: Id3bad65659174a08cfe945e9af6e48ca012a5135

(cherry-picked from ics)

Change-Id: Ib0eeb683e3177d96b9b2c7bc2916b19ddaec8ae5
parent 7c5d3dcf

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] = 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

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
@@ -44,6 +44,7 @@ 
#include "LinearAccelerationSensor.h"

#include "OrientationSensor.h"

#include "RotationVectorSensor.h"

#include "RotationVectorSensor2.h"

#include "SensorFusion.h"

#include "SensorService.h"
@@ -124,6 +125,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
@@ -256,6 +263,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) {