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

Commit 2e75e832 authored by Philip Chen's avatar Philip Chen
Browse files

Reland: SensorFusion: Do not use the sensor variables if they are not init'ed 

The member variables for the optional sensors (mMag & mGyro) could be
used even when they are not initialized, leading to failed fusion result
in corner cases.

Let's check those variables are properly initialized before using
them.

Also add minor fixes for the coding style.

Bug: 425971589
Test: CtsSensorTestCases for auto/wear CF
Test: HW smoke test on AL
Flag: EXEMPT bugfix
Change-Id: I8863081d91faa4ca6fcb82922ecc0e7c08d8bfc8
parent 3a622119

services/sensorservice/SensorFusion.cpp

+41 −21
Original line number Diff line number Diff line
@@ -82,10 +82,11 @@ SensorFusion::SensorFusion() 


void SensorFusion::process(const sensors_event_t& event) {

    // sensor additional info is not currently used in fusion algorithm

    if (event.type == SENSOR_TYPE_ADDITIONAL_INFO)

    if (event.type == SENSOR_TYPE_ADDITIONAL_INFO) {

        return;

    }



    if (event.sensor == mGyro.getHandle()) {

    if (mGyro.has_value() && event.sensor == mGyro.value().getHandle()) {

        float dT;

        if (event.timestamp - mGyroTime > 0 &&

            event.timestamp - mGyroTime < (int64_t)(5e7) ) { // 0.05sec
@@ -107,19 +108,20 @@ void SensorFusion::process(const sensors_event_t& event) { 
            }

        }

        mGyroTime = event.timestamp;

    } else if (event.sensor == mMag.getHandle()) {

    } else if (mMag.has_value() && event.sensor == mMag.value().getHandle()) {

        const vec3_t mag(event.data);

        for (int i = 0; i < NUM_FUSION_MODE; ++i) {

            if (mEnabled[i]) {

                mFusions[i].handleMag(mag);// fusion in no mag mode will ignore

                // fusion in no mag mode will ignore

                mFusions[i].handleMag(mag);

            }

        }

    } else if (event.sensor == mAcc.getHandle()) {

        float dT;

        if (event.timestamp - mAccTime > 0 &&

            event.timestamp - mAccTime < (int64_t)(1e8) ) { // 0.1sec

            dT = (event.timestamp - mAccTime) / 1000000000.0f;



            dT = (event.timestamp - mAccTime) / 1000000000.0f;

            const vec3_t acc(event.data);

            for (int i = 0; i < NUM_FUSION_MODE; ++i) {

                if (mEnabled[i]) {
@@ -160,14 +162,22 @@ status_t SensorFusion::activate(int mode, void* ident, bool enabled) { 
        }

    }



    if (mode != FUSION_NOMAG && !mMag.has_value()) {

        ALOGE("%s: magnetic sensor is expected but not present!", __func__);

        return NO_INIT;

    }

    if (mode != FUSION_NOGYRO && !mGyro.has_value()) {

        ALOGE("%s: gyroscope is expected but not present!", __func__);

        return NO_INIT;

    }



    mSensorDevice.activate(ident, mAcc.getHandle(), enabled);

    if (mode != FUSION_NOMAG) {

        mSensorDevice.activate(ident, mMag.getHandle(), enabled);

        mSensorDevice.activate(ident, mMag.value().getHandle(), enabled);

    }

    if (mode != FUSION_NOGYRO) {

        mSensorDevice.activate(ident, mGyro.getHandle(), enabled);

        mSensorDevice.activate(ident, mGyro.value().getHandle(), enabled);

    }



    return NO_ERROR;

}
@@ -176,12 +186,22 @@ status_t SensorFusion::setDelay(int mode, void* ident, int64_t ns) { 
    if (ns > (int64_t)5e7) {

        ns = (int64_t)(5e7);

    }



    if (mode != FUSION_NOMAG && !mMag.has_value()) {

        ALOGE("%s: magnetic sensor is expected but not present!", __func__);

        return NO_INIT;

    }

    if (mode != FUSION_NOGYRO && !mGyro.has_value()) {

        ALOGE("%s: gyroscope is expected but not present!", __func__);

        return NO_INIT;

    }



    mSensorDevice.batch(ident, mAcc.getHandle(), 0, ns, 0);

    if (mode != FUSION_NOMAG) {

        mSensorDevice.batch(ident, mMag.getHandle(), 0, ms2ns(10), 0);

        mSensorDevice.batch(ident, mMag.value().getHandle(), 0, ms2ns(10), 0);

    }

    if (mode != FUSION_NOGYRO) {

        mSensorDevice.batch(ident, mGyro.getHandle(), 0, mTargetDelayNs, 0);

        mSensorDevice.batch(ident, mGyro.value().getHandle(), 0, mTargetDelayNs, 0);

    }

    return NO_ERROR;

}
@@ -189,8 +209,8 @@ status_t SensorFusion::setDelay(int mode, void* ident, int64_t ns) { 


float SensorFusion::getPowerUsage(int mode) const {

    float power =   mAcc.getPowerUsage() +

                    ((mode != FUSION_NOMAG) ? mMag.getPowerUsage() : 0) +

                    ((mode != FUSION_NOGYRO) ? mGyro.getPowerUsage() : 0);

                    ((mode != FUSION_NOMAG) ? mMag.value().getPowerUsage() : 0) +

                    ((mode != FUSION_NOGYRO) ? mGyro.value().getPowerUsage() : 0);

    return power;

}

services/sensorservice/SensorFusion.h

+2 −2
Original line number Diff line number Diff line
@@ -42,8 +42,8 @@ class SensorFusion : public Singleton<SensorFusion> { 


    SensorDevice& mSensorDevice;

    Sensor mAcc;

    Sensor mMag;

    Sensor mGyro;

    std::optional<Sensor> mMag;

    std::optional<Sensor> mGyro;



    Fusion mFusions[NUM_FUSION_MODE]; // normal, no_mag, no_gyro