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
Loading
Loading
Loading
Loading
+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\"

+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;
+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
+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
+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
Loading