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

Commit b2397fff authored by Mathias Agopian's avatar Mathias Agopian Committed by Android (Google) Code Review
Browse files

Merge changes Id5584bc1,I8ca826d0 

* changes:
  revert parts of dc5b63e4, which made gyro drift estimation unstable
  use gyro timestamp directly in fusion
parents af0f8c0a 24f9bea8

services/sensorservice/Fusion.cpp

+0 −16
Original line number Diff line number Diff line
@@ -220,22 +220,6 @@ void Fusion::initFusion(const vec4_t& q, float dT) 
    // initial covariance: Var{ x(t0) }

    // TODO: initialize P correctly

    P = 0;



    // it is unclear how to set the initial covariance. It does affect

    // how quickly the fusion converges. Experimentally it would take

    // about 10 seconds at 200 Hz to estimate the gyro-drift with an

    // initial covariance of 0, and about a second with an initial covariance

    // of about 1 deg/s.

    const float covv = 0;

    const float covu = 0.5f * (float(M_PI) / 180);

    mat33_t& Pv = P[0][0];

    Pv[0][0] = covv;

    Pv[1][1] = covv;

    Pv[2][2] = covv;

    mat33_t& Pu = P[1][1];

    Pu[0][0] = covu;

    Pu[1][1] = covu;

    Pu[2][2] = covu;

}



bool Fusion::hasEstimate() const {

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);