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

Commit 336a2b9b authored by Philip Chen's avatar Philip Chen
Browse files

Reland^2: 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.

Fix: 425971589
Bug: 427618429
Bug: 427628159

Test: hw_smoke_test for brya-trunk_staging-userdebug
Test: boot gcar_x86_64-trunk_staging-userdebug
Flag: EXEMPT bugfix
Change-Id: I29007474b1b65822381446d21b5242c9fb4c49ea
parent 1e93a845

services/sensorservice/SensorFusion.cpp

+48 −22
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,21 +186,37 @@ 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;

}





float SensorFusion::getPowerUsage(int mode) const {

    float power =   mAcc.getPowerUsage() +

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

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

    float power = mAcc.getPowerUsage();



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

        power += mMag.value().getPowerUsage();

    }

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

        power += mGyro.value().getPowerUsage();

    }



    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