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

Commit 2e2a560c authored by Mathias Agopian's avatar Mathias Agopian
Browse files

use gyro timestamp directly in fusion 

we used to estimate the gyro rate and deduce the period from that
but it turns out this is causing problems.

Bug: 5192288
Change-Id: I8ca826d0e11e488587bcaa1720de99e92b82f191
parent 9e3cb55b

services/sensorservice/SensorFusion.cpp

+6 −5
Original line number Diff line number Diff line
@@ -53,8 +53,8 @@ SensorFusion::SensorFusion() 


        // 200 Hz for gyro events is a good compromise between precision

        // and power/cpu usage.

        mGyroRate = 200;

        mTargetDelayNs = 1000000000LL/mGyroRate;

        mEstimatedGyroRate = 200;

        mTargetDelayNs = 1000000000LL/mEstimatedGyroRate;

        mFusion.init();

    }

}
@@ -63,14 +63,15 @@ void SensorFusion::process(const sensors_event_t& event) { 
    if (event.type == mGyro.getType()) {

        if (mGyroTime != 0) {

            const float dT = (event.timestamp - mGyroTime) / 1000000000.0f;

            mFusion.handleGyro(vec3_t(event.data), dT);

            // here we estimate the gyro rate (useful for debugging)

            const float freq = 1 / dT;

            if (freq >= 100 && freq<1000) { // filter values obviously wrong

                const float alpha = 1 / (1 + dT); // 1s time-constant

                mGyroRate = freq + (mGyroRate - freq)*alpha;

                mEstimatedGyroRate = freq + (mEstimatedGyroRate - freq)*alpha;

            }

        }

        mGyroTime = event.timestamp;

        mFusion.handleGyro(vec3_t(event.data), 1.0f/mGyroRate);

    } else if (event.type == SENSOR_TYPE_MAGNETIC_FIELD) {

        const vec3_t mag(event.data);

        mFusion.handleMag(mag);
@@ -142,7 +143,7 @@ void SensorFusion::dump(String8& result, char* buffer, size_t SIZE) { 
            "b=< %g, %g, %g >\n",

            mEnabled ? "enabled" : "disabled",

            mClients.size(),

            mGyroRate,

            mEstimatedGyroRate,

            fusion.getAttitude().x,

            fusion.getAttitude().y,

            fusion.getAttitude().z,

services/sensorservice/SensorFusion.h

+2 −2
Original line number Diff line number Diff line
@@ -44,7 +44,7 @@ class SensorFusion : public Singleton<SensorFusion> { 
    Sensor mGyro;

    Fusion mFusion;

    bool mEnabled;

    float mGyroRate;

    float mEstimatedGyroRate;

    nsecs_t mTargetDelayNs;

    nsecs_t mGyroTime;

    vec4_t mAttitude;
@@ -60,7 +60,7 @@ public: 
    mat33_t getRotationMatrix() const { return mFusion.getRotationMatrix(); }

    vec4_t getAttitude() const { return mAttitude; }

    vec3_t getGyroBias() const { return mFusion.getBias(); }

    float getEstimatedRate() const { return mGyroRate; }

    float getEstimatedRate() const { return mEstimatedGyroRate; }



    status_t activate(void* ident, bool enabled);

    status_t setDelay(void* ident, int64_t ns);