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

Commit be01b40f authored by Rudolf Tammekivi's avatar Rudolf Tammekivi Committed by Steve Kondik
Browse files

sensorservice: Add legacy sensors fusion. 

This change allows devices with accelerometer & magnetometer but
without gyroscope to still get virtual sensors such as Gravity,
Linear Acceleration and Rotation Vector.

Change-Id: Ibb90b70845c766ab52c843557446e34649a7d6d9
parent 8ff026f1

services/sensorservice/Android.mk

+7 −0
Original line number Diff line number Diff line
@@ -15,6 +15,13 @@ LOCAL_SRC_FILES:= \ 
    SensorInterface.cpp \

    SensorService.cpp \



# Legacy virtual sensors used in combination from accelerometer & magnetometer.

LOCAL_SRC_FILES += \

	legacy/SecondOrderLowPassFilter.cpp \

	legacy/LegacyGravitySensor.cpp \

	legacy/LegacyLinearAccelerationSensor.cpp \

	legacy/LegacyRotationVectorSensor.cpp





LOCAL_CFLAGS:= -DLOG_TAG=\"SensorService\"

services/sensorservice/SensorService.cpp

+9 −0
Original line number Diff line number Diff line
@@ -49,6 +49,9 @@ 
#include "RotationVectorSensor2.h"

#include "SensorFusion.h"

#include "SensorService.h"

#include "legacy/LegacyGravitySensor.h"

#include "legacy/LegacyLinearAccelerationSensor.h"

#include "legacy/LegacyRotationVectorSensor.h"



namespace android {

// ---------------------------------------------------------------------------
@@ -123,6 +126,12 @@ void SensorService::onFirstRef() 
                registerVirtualSensor( new OrientationSensor() );

                registerVirtualSensor( new CorrectedGyroSensor(list, count) );

            }

            else

            {

                registerVirtualSensor( new LegacyRotationVectorSensor() );

                registerVirtualSensor( new LegacyGravitySensor(list, count) );

                registerVirtualSensor( new LegacyLinearAccelerationSensor(list, count) );

            }



            // build the sensor list returned to users

            mUserSensorList = mSensorList;

services/sensorservice/legacy/LegacyGravitySensor.cpp

0 → 100644
+102 −0
Original line number Diff line number Diff line 
/*

 * Copyright (C) 2010 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 "LegacyGravitySensor.h"

#include "SensorDevice.h"

#include "SensorFusion.h"



namespace android {

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



LegacyGravitySensor::LegacyGravitySensor(sensor_t const* list, size_t count)

    : mSensorDevice(SensorDevice::getInstance()),

      mSensorFusion(SensorFusion::getInstance()),

      mAccTime(0),

      mLowPass(M_SQRT1_2, 1.5f),

      mX(mLowPass), mY(mLowPass), mZ(mLowPass)

{

    for (size_t i=0 ; i<count ; i++) {

        if (list[i].type == SENSOR_TYPE_ACCELEROMETER) {

            mAccelerometer = Sensor(list + i);

            break;

        }

    }

}



bool LegacyGravitySensor::process(sensors_event_t* outEvent,

        const sensors_event_t& event)

{

    const static double NS2S = 1.0 / 1000000000.0;

    if (event.type == SENSOR_TYPE_ACCELEROMETER) {

        float x, y, z;

        const double now = event.timestamp * NS2S;

        if (mAccTime == 0) {

            x = mX.init(event.acceleration.x);

            y = mY.init(event.acceleration.y);

            z = mZ.init(event.acceleration.z);

        } else {

            double dT = now - mAccTime;

            mLowPass.setSamplingPeriod(dT);

            x = mX(event.acceleration.x);

            y = mY(event.acceleration.y);

            z = mZ(event.acceleration.z);

        }

        mAccTime = now;

        *outEvent = event;

        outEvent->data[0] = x;

        outEvent->data[1] = y;

        outEvent->data[2] = z;

        outEvent->sensor = '_grv';

        outEvent->type = SENSOR_TYPE_GRAVITY;

        return true;

    }

    return false;

}



status_t LegacyGravitySensor::activate(void* ident, bool enabled) {

    return mSensorFusion.activate(this, enabled);

}



status_t LegacyGravitySensor::setDelay(void* ident, int handle, int64_t ns) {

    return mSensorFusion.setDelay(this, ns);

}



Sensor LegacyGravitySensor::getSensor() const {

    sensor_t hwSensor;

    hwSensor.name       = "Gravity Sensor";

    hwSensor.vendor     = "Google Inc.";

    hwSensor.version    = 3;

    hwSensor.handle     = '_grv';

    hwSensor.type       = SENSOR_TYPE_GRAVITY;

    hwSensor.maxRange   = GRAVITY_EARTH * 2;

    hwSensor.resolution = mAccelerometer.getResolution();

    hwSensor.power      = mSensorFusion.getPowerUsage();

    hwSensor.minDelay   = mSensorFusion.getMinDelay();

    Sensor sensor(&hwSensor);

    return sensor;

}



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

}; // namespace android

services/sensorservice/legacy/LegacyGravitySensor.h

0 → 100644
+57 −0
Original line number Diff line number Diff line 
/*

 * Copyright (C) 2010 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_LEGACY_GRAVITY_SENSOR_H

#define ANDROID_LEGACY_GRAVITY_SENSOR_H



#include <stdint.h>

#include <sys/types.h>



#include <gui/Sensor.h>



#include "../SensorInterface.h"

#include "SecondOrderLowPassFilter.h"



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

namespace android {

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



class SensorDevice;

class SensorFusion;



class LegacyGravitySensor : public SensorInterface {

    SensorDevice& mSensorDevice;

    SensorFusion& mSensorFusion;

    Sensor mAccelerometer;

    double mAccTime;



    SecondOrderLowPassFilter mLowPass;

    CascadedBiquadFilter mX, mY, mZ;



public:

    LegacyGravitySensor(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_LEGACY_GRAVITY_SENSOR_H

services/sensorservice/legacy/LegacyLinearAccelerationSensor.cpp

0 → 100644
+84 −0
Original line number Diff line number Diff line 
/*

 * Copyright (C) 2010 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 "LegacyLinearAccelerationSensor.h"

#include "../SensorDevice.h"

#include "../SensorFusion.h"



namespace android {

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



LegacyLinearAccelerationSensor::LegacyLinearAccelerationSensor(sensor_t const* list, size_t count)

    : mSensorDevice(SensorDevice::getInstance()),

      mGravitySensor(list, count)

{

    mData[0] = mData[1] = mData[2] = 0;

}



bool LegacyLinearAccelerationSensor::process(sensors_event_t* outEvent,

        const sensors_event_t& event)

{

    bool result = mGravitySensor.process(outEvent, event);

    if (result) {

        if (event.type == SENSOR_TYPE_ACCELEROMETER) {

            mData[0] = event.acceleration.x;

            mData[1] = event.acceleration.y;

            mData[2] = event.acceleration.z;

        }

        outEvent->data[0] = mData[0] - outEvent->data[0];

        outEvent->data[1] = mData[1] - outEvent->data[1];

        outEvent->data[2] = mData[2] - outEvent->data[2];

        outEvent->sensor = '_lin';

        outEvent->type = SENSOR_TYPE_LINEAR_ACCELERATION;

    }

    return result;

}



status_t LegacyLinearAccelerationSensor::activate(void* ident, bool enabled) {

    return mGravitySensor.activate(this, enabled);

}



status_t LegacyLinearAccelerationSensor::setDelay(void* ident, int handle, int64_t ns) {

    return mGravitySensor.setDelay(this, handle, ns);

}



Sensor LegacyLinearAccelerationSensor::getSensor() const {

    Sensor gsensor(mGravitySensor.getSensor());

    sensor_t hwSensor;

    hwSensor.name       = "Linear Acceleration Sensor";

    hwSensor.vendor     = "Google Inc.";

    hwSensor.version    = gsensor.getVersion();

    hwSensor.handle     = '_lin';

    hwSensor.type       = SENSOR_TYPE_LINEAR_ACCELERATION;

    hwSensor.maxRange   = gsensor.getMaxValue();

    hwSensor.resolution = gsensor.getResolution();

    hwSensor.power      = gsensor.getPowerUsage();

    hwSensor.minDelay   = gsensor.getMinDelay();

    Sensor sensor(&hwSensor);

    return sensor;

}



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

}; // namespace android