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

Commit 33a004b2 authored by Peng Xu's avatar Peng Xu Committed by Prashant Malani
Browse files

DO NOT MERGE ANYWHERE Add AOSP Geomag and Game Rotation, and Gravity 

Providing AOSP software implementation of Geomag Rotation Vector, Game
Rotation Vector and Gravity sensors for platforms that does not have
hardware implementation of these sensors but do have primitive sensors
(accelerometers, gyrometers and magnetometers).

Previously, AOSP Gravity sensor is enabled only when all primitive sensors are
available. This is changed so that AOSP Gravity will be available even
no magnetometer is in the device.

Related bug/feature request:
    * b/17508800
    * b/22610016

Change-Id: I4e2d3e544884047d66e7fdbce2282f1f8234eae9
(cherry picked from commit f66684a6)
parent 86d52723

services/sensorservice/CorrectedGyroSensor.cpp

+2 −2
Original line number Original line Diff line number Diff line
@@ -58,12 +58,12 @@ bool CorrectedGyroSensor::process(sensors_event_t* outEvent, 




status_t CorrectedGyroSensor::activate(void* ident, bool enabled) {

status_t CorrectedGyroSensor::activate(void* ident, bool enabled) {

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

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

    return mSensorFusion.activate(ident, enabled);

    return mSensorFusion.activate(FUSION_9AXIS, ident, enabled);

}

}





status_t CorrectedGyroSensor::setDelay(void* ident, int /*handle*/, int64_t ns) {

status_t CorrectedGyroSensor::setDelay(void* ident, int /*handle*/, int64_t ns) {

    mSensorDevice.setDelay(ident, mGyro.getHandle(), ns);

    mSensorDevice.setDelay(ident, mGyro.getHandle(), ns);

    return mSensorFusion.setDelay(ident, ns);

    return mSensorFusion.setDelay(FUSION_9AXIS, ident, ns);

}

}





Sensor CorrectedGyroSensor::getSensor() const {

Sensor CorrectedGyroSensor::getSensor() const {

services/sensorservice/Fusion.cpp

+127 −40
Original line number Original line Diff line number Diff line
@@ -24,28 +24,44 @@ namespace android { 




// -----------------------------------------------------------------------

// -----------------------------------------------------------------------





/*==================== BEGIN FUSION SENSOR PARAMETER =========================*/



/* Note:

 *   If a platform uses software fusion, it is necessary to tune the following

 *   parameters to fit the hardware sensors prior to release.

 *

 *   The DEFAULT_ parameters will be used in FUSION_9AXIS and FUSION_NOMAG mode.

 *   The GEOMAG_ parameters will be used in FUSION_NOGYRO mode.

 */



/*

/*

 * gyroVAR gives the measured variance of the gyro's output per

 * GYRO_VAR gives the measured variance of the gyro's output per

 * Hz (or variance at 1 Hz). This is an "intrinsic" parameter of the gyro,

 * Hz (or variance at 1 Hz). This is an "intrinsic" parameter of the gyro,

 * which is independent of the sampling frequency.

 * which is independent of the sampling frequency.

 *

 *

 * The variance of gyro's output at a given sampling period can be

 * The variance of gyro's output at a given sampling period can be

 * calculated as:

 * calculated as:

 *      variance(T) = gyroVAR / T

 *      variance(T) = GYRO_VAR / T

 *

 *

 * The variance of the INTEGRATED OUTPUT at a given sampling period can be

 * The variance of the INTEGRATED OUTPUT at a given sampling period can be

 * calculated as:

 * calculated as:

 *       variance_integrate_output(T) = gyroVAR * T

 *       variance_integrate_output(T) = GYRO_VAR * T

 *

 */

 */

static const float gyroVAR = 1e-7;      // (rad/s)^2 / Hz

static const float DEFAULT_GYRO_VAR = 1e-7;      // (rad/s)^2 / Hz

static const float biasVAR = 1e-8;      // (rad/s)^2 / s (guessed)

static const float DEFAULT_GYRO_BIAS_VAR = 1e-12;  // (rad/s)^2 / s (guessed)

static const float GEOMAG_GYRO_VAR = 1e-4;      // (rad/s)^2 / Hz

static const float GEOMAG_GYRO_BIAS_VAR = 1e-8;  // (rad/s)^2 / s (guessed)





/*

/*

 * Standard deviations of accelerometer and magnetometer

 * Standard deviations of accelerometer and magnetometer

 */

 */

static const float accSTDEV  = 0.05f;   // m/s^2 (measured 0.08 / CDD 0.05)

static const float DEFAULT_ACC_STDEV  = 0.015f; // m/s^2 (measured 0.08 / CDD 0.05)

static const float magSTDEV  = 0.5f;    // uT    (measured 0.7  / CDD 0.5)

static const float DEFAULT_MAG_STDEV  = 0.1f;   // uT    (measured 0.7  / CDD 0.5)

static const float GEOMAG_ACC_STDEV  = 0.05f; // m/s^2 (measured 0.08 / CDD 0.05)

static const float GEOMAG_MAG_STDEV  = 0.1f;   // uT    (measured 0.7  / CDD 0.5)





/* ====================== END FUSION SENSOR PARAMETER ========================*/





static const float SYMMETRY_TOLERANCE = 1e-10f;

static const float SYMMETRY_TOLERANCE = 1e-10f;
@@ -54,7 +70,8 @@ static const float SYMMETRY_TOLERANCE = 1e-10f; 
 * ill-conditioning and div by zeros.

 * ill-conditioning and div by zeros.

 * Threshhold: 10% of g, in m/s^2

 * Threshhold: 10% of g, in m/s^2

 */

 */

static const float FREE_FALL_THRESHOLD = 0.981f;

static const float NOMINAL_GRAVITY = 9.81f;

static const float FREE_FALL_THRESHOLD = 0.1f * (NOMINAL_GRAVITY);

static const float FREE_FALL_THRESHOLD_SQ =

static const float FREE_FALL_THRESHOLD_SQ =

        FREE_FALL_THRESHOLD*FREE_FALL_THRESHOLD;

        FREE_FALL_THRESHOLD*FREE_FALL_THRESHOLD;
@@ -87,6 +104,9 @@ static const float MIN_VALID_CROSS_PRODUCT_MAG = 1.0e-3; 
static const float MIN_VALID_CROSS_PRODUCT_MAG_SQ =

static const float MIN_VALID_CROSS_PRODUCT_MAG_SQ =

    MIN_VALID_CROSS_PRODUCT_MAG*MIN_VALID_CROSS_PRODUCT_MAG;

    MIN_VALID_CROSS_PRODUCT_MAG*MIN_VALID_CROSS_PRODUCT_MAG;





static const float W_EPS = 1e-4f;

static const float SQRT_3 = 1.732f;

static const float WVEC_EPS = 1e-4f/SQRT_3;

// -----------------------------------------------------------------------

// -----------------------------------------------------------------------





template <typename TYPE, size_t C, size_t R>

template <typename TYPE, size_t C, size_t R>
@@ -173,7 +193,7 @@ Fusion::Fusion() { 
    init();

    init();

}

}





void Fusion::init() {

void Fusion::init(int mode) {

    mInitState = 0;

    mInitState = 0;





    mGyroRate = 0;

    mGyroRate = 0;
@@ -183,6 +203,19 @@ void Fusion::init() { 
    mCount[2] = 0;

    mCount[2] = 0;





    mData = 0;

    mData = 0;

    mMode = mode;



    if (mMode != FUSION_NOGYRO) { //normal or game rotation

        mParam.gyroVar = DEFAULT_GYRO_VAR;

        mParam.gyroBiasVar = DEFAULT_GYRO_BIAS_VAR;

        mParam.accStdev = DEFAULT_ACC_STDEV;

        mParam.magStdev = DEFAULT_MAG_STDEV;

    } else {

        mParam.gyroVar = GEOMAG_GYRO_VAR;

        mParam.gyroBiasVar = GEOMAG_GYRO_BIAS_VAR;

        mParam.accStdev = GEOMAG_ACC_STDEV;

        mParam.magStdev = GEOMAG_MAG_STDEV;

    }

}

}





void Fusion::initFusion(const vec4_t& q, float dT)

void Fusion::initFusion(const vec4_t& q, float dT)
@@ -205,11 +238,11 @@ void Fusion::initFusion(const vec4_t& q, float dT) 
    const float dT3 = dT2*dT;

    const float dT3 = dT2*dT;





    // variance of integrated output at 1/dT Hz (random drift)

    // variance of integrated output at 1/dT Hz (random drift)

    const float q00 = gyroVAR * dT + 0.33333f * biasVAR * dT3;

    const float q00 = mParam.gyroVar * dT + 0.33333f * mParam.gyroBiasVar * dT3;





    // variance of drift rate ramp

    // variance of drift rate ramp

    const float q11 = biasVAR * dT;

    const float q11 = mParam.gyroBiasVar * dT;

    const float q10 = 0.5f * biasVAR * dT2;

    const float q10 = 0.5f * mParam.gyroBiasVar * dT2;

    const float q01 = q10;

    const float q01 = q10;





    GQGt[0][0] =  q00;      // rad^2

    GQGt[0][0] =  q00;      // rad^2
@@ -223,7 +256,9 @@ void Fusion::initFusion(const vec4_t& q, float dT) 
}

}





bool Fusion::hasEstimate() const {

bool Fusion::hasEstimate() const {

    return (mInitState == (MAG|ACC|GYRO));

    return ((mInitState & MAG) || (mMode == FUSION_NOMAG)) &&

           ((mInitState & GYRO) || (mMode == FUSION_NOGYRO)) &&

           (mInitState & ACC);

}

}





bool Fusion::checkInitComplete(int what, const vec3_t& d, float dT) {

bool Fusion::checkInitComplete(int what, const vec3_t& d, float dT) {
@@ -234,6 +269,9 @@ bool Fusion::checkInitComplete(int what, const vec3_t& d, float dT) { 
        mData[0] += d * (1/length(d));

        mData[0] += d * (1/length(d));

        mCount[0]++;

        mCount[0]++;

        mInitState |= ACC;

        mInitState |= ACC;

        if (mMode == FUSION_NOGYRO ) {

            mGyroRate = dT;

        }

    } else if (what == MAG) {

    } else if (what == MAG) {

        mData[1] += d * (1/length(d));

        mData[1] += d * (1/length(d));

        mCount[1]++;

        mCount[1]++;
@@ -242,25 +280,29 @@ bool Fusion::checkInitComplete(int what, const vec3_t& d, float dT) { 
        mGyroRate = dT;

        mGyroRate = dT;

        mData[2] += d*dT;

        mData[2] += d*dT;

        mCount[2]++;

        mCount[2]++;

        if (mCount[2] == 64) {

            // 64 samples is good enough to estimate the gyro drift and

            // doesn't take too much time.

        mInitState |= GYRO;

        mInitState |= GYRO;

    }

    }

    }





    if (mInitState == (MAG|ACC|GYRO)) {

    if (hasEstimate()) {

        // Average all the values we collected so far

        // Average all the values we collected so far

        mData[0] *= 1.0f/mCount[0];

        mData[0] *= 1.0f/mCount[0];

        if (mMode != FUSION_NOMAG) {

            mData[1] *= 1.0f/mCount[1];

            mData[1] *= 1.0f/mCount[1];

        }

        mData[2] *= 1.0f/mCount[2];

        mData[2] *= 1.0f/mCount[2];





        // calculate the MRPs from the data collection, this gives us

        // calculate the MRPs from the data collection, this gives us

        // a rough estimate of our initial state

        // a rough estimate of our initial state

        mat33_t R;

        mat33_t R;

        vec3_t  up(mData[0]);

        vec3_t  up(mData[0]);

        vec3_t east(cross_product(mData[1], up));

        vec3_t  east;

        east *= 1/length(east);



        if (mMode != FUSION_NOMAG) {

            east = normalize(cross_product(mData[1], up));

        } else {

            east = getOrthogonal(up);

        }



        vec3_t north(cross_product(up, east));

        vec3_t north(cross_product(up, east));

        R << east << north << up;

        R << east << north << up;

        const vec4_t q = matrixToQuat(R);

        const vec4_t q = matrixToQuat(R);
@@ -278,21 +320,43 @@ void Fusion::handleGyro(const vec3_t& w, float dT) { 
    predict(w, dT);

    predict(w, dT);

}

}





status_t Fusion::handleAcc(const vec3_t& a) {

status_t Fusion::handleAcc(const vec3_t& a, float dT) {

    if (!checkInitComplete(ACC, a, dT))

        return BAD_VALUE;



    // ignore acceleration data if we're close to free-fall

    // ignore acceleration data if we're close to free-fall

    if (length_squared(a) < FREE_FALL_THRESHOLD_SQ) {

    const float l = length(a);

    if (l < FREE_FALL_THRESHOLD) {

        return BAD_VALUE;

        return BAD_VALUE;

    }

    }





    if (!checkInitComplete(ACC, a))

    const float l_inv = 1.0f/l;

        return BAD_VALUE;



    if ( mMode == FUSION_NOGYRO ) {

        //geo mag

        vec3_t w_dummy;

        w_dummy = x1; //bias

        predict(w_dummy, dT);

    }





    const float l = 1/length(a);

    if ( mMode == FUSION_NOMAG) {

    update(a*l, Ba, accSTDEV*l);

        vec3_t m;

        m = getRotationMatrix()*Bm;

        update(m, Bm, mParam.magStdev);

    }



    vec3_t unityA = a * l_inv;

    const float d = sqrtf(fabsf(l- NOMINAL_GRAVITY));

    const float p = l_inv * mParam.accStdev*expf(d);



    update(unityA, Ba, p);

    return NO_ERROR;

    return NO_ERROR;

}

}





status_t Fusion::handleMag(const vec3_t& m) {

status_t Fusion::handleMag(const vec3_t& m) {

    if (!checkInitComplete(MAG, m))

        return BAD_VALUE;



    // the geomagnetic-field should be between 30uT and 60uT

    // the geomagnetic-field should be between 30uT and 60uT

    // reject if too large to avoid spurious magnetic sources

    // reject if too large to avoid spurious magnetic sources

    const float magFieldSq = length_squared(m);

    const float magFieldSq = length_squared(m);
@@ -304,9 +368,6 @@ status_t Fusion::handleMag(const vec3_t& m) { 
        return BAD_VALUE;

        return BAD_VALUE;

    }

    }





    if (!checkInitComplete(MAG, m))

        return BAD_VALUE;



    // Orthogonalize the magnetic field to the gravity field, mapping it into

    // Orthogonalize the magnetic field to the gravity field, mapping it into

    // tangent to Earth.

    // tangent to Earth.

    const vec3_t up( getRotationMatrix() * Ba );

    const vec3_t up( getRotationMatrix() * Ba );
@@ -324,10 +385,10 @@ status_t Fusion::handleMag(const vec3_t& m) { 
    // then pass it in as the update.

    // then pass it in as the update.

    vec3_t north( cross_product(up, east) );

    vec3_t north( cross_product(up, east) );





    const float l = 1 / length(north);

    const float l_inv = 1 / length(north);

    north *= l;

    north *= l_inv;





    update(north, Bm, magSTDEV*l);

    update(north, Bm,  mParam.magStdev*l_inv);

    return NO_ERROR;

    return NO_ERROR;

}

}
@@ -372,8 +433,11 @@ mat34_t Fusion::getF(const vec4_t& q) { 
void Fusion::predict(const vec3_t& w, float dT) {

void Fusion::predict(const vec3_t& w, float dT) {

    const vec4_t q  = x0;

    const vec4_t q  = x0;

    const vec3_t b  = x1;

    const vec3_t b  = x1;

    const vec3_t we = w - b;

    vec3_t we = w - b;





    if (length(we) < WVEC_EPS) {

        we = (we[0]>0.f)?WVEC_EPS:-WVEC_EPS;

    }

    // q(k+1) = O(we)*q(k)

    // q(k+1) = O(we)*q(k)

    // --------------------

    // --------------------

    //

    //
@@ -406,7 +470,7 @@ void Fusion::predict(const vec3_t& w, float dT) { 
    const mat33_t wx2(wx*wx);

    const mat33_t wx2(wx*wx);

    const float lwedT = length(we)*dT;

    const float lwedT = length(we)*dT;

    const float hlwedT = 0.5f*lwedT;

    const float hlwedT = 0.5f*lwedT;

    const float ilwe = 1/length(we);

    const float ilwe = 1.f/length(we);

    const float k0 = (1-cosf(lwedT))*(ilwe*ilwe);

    const float k0 = (1-cosf(lwedT))*(ilwe*ilwe);

    const float k1 = sinf(lwedT);

    const float k1 = sinf(lwedT);

    const float k2 = cosf(hlwedT);

    const float k2 = cosf(hlwedT);
@@ -422,6 +486,7 @@ void Fusion::predict(const vec3_t& w, float dT) { 
    Phi[1][0] = wx*k0 - I33dT - wx2*(ilwe*ilwe*ilwe)*(lwedT-k1);

    Phi[1][0] = wx*k0 - I33dT - wx2*(ilwe*ilwe*ilwe)*(lwedT-k1);





    x0 = O*q;

    x0 = O*q;



    if (x0.w < 0)

    if (x0.w < 0)

        x0 = -x0;

        x0 = -x0;
@@ -466,15 +531,37 @@ void Fusion::update(const vec3_t& z, const vec3_t& Bi, float sigma) { 




    const vec3_t e(z - Bb);

    const vec3_t e(z - Bb);

    const vec3_t dq(K[0]*e);

    const vec3_t dq(K[0]*e);

    const vec3_t db(K[1]*e);





    q += getF(q)*(0.5f*dq);

    q += getF(q)*(0.5f*dq);

    x0 = normalize_quat(q);

    x0 = normalize_quat(q);



    if (mMode != FUSION_NOMAG) {

        const vec3_t db(K[1]*e);

        x1 += db;

        x1 += db;

    }





    checkState();

    checkState();

}

}





vec3_t Fusion::getOrthogonal(const vec3_t &v) {

    vec3_t w;

    if (fabsf(v[0])<= fabsf(v[1]) && fabsf(v[0]) <= fabsf(v[2]))  {

        w[0]=0.f;

        w[1] = v[2];

        w[2] = -v[1];

    } else if (fabsf(v[1]) <= fabsf(v[2])) {

        w[0] = v[2];

        w[1] = 0.f;

        w[2] = -v[0];

    }else {

        w[0] = v[1];

        w[1] = -v[0];

        w[2] = 0.f;

    }

    return normalize(w);

}





// -----------------------------------------------------------------------

// -----------------------------------------------------------------------





}; // namespace android

}; // namespace android

services/sensorservice/Fusion.h

+19 −2
Original line number Original line Diff line number Diff line
@@ -27,6 +27,13 @@ namespace android { 




typedef mat<float, 3, 4> mat34_t;

typedef mat<float, 3, 4> mat34_t;





enum FUSION_MODE{

    FUSION_9AXIS, // use accel gyro mag

    FUSION_NOMAG, // use accel gyro (game rotation, gravity)

    FUSION_NOGYRO, // use accel mag (geomag rotation)

    NUM_FUSION_MODE

};



class Fusion {

class Fusion {

    /*

    /*

     * the state vector is made of two sub-vector containing respectively:

     * the state vector is made of two sub-vector containing respectively:
@@ -55,9 +62,9 @@ class Fusion { 




public:

public:

    Fusion();

    Fusion();

    void init();

    void init(int mode = FUSION_9AXIS);

    void handleGyro(const vec3_t& w, float dT);

    void handleGyro(const vec3_t& w, float dT);

    status_t handleAcc(const vec3_t& a);

    status_t handleAcc(const vec3_t& a, float dT);

    status_t handleMag(const vec3_t& m);

    status_t handleMag(const vec3_t& m);

    vec4_t getAttitude() const;

    vec4_t getAttitude() const;

    vec3_t getBias() const;

    vec3_t getBias() const;
@@ -65,12 +72,21 @@ public: 
    bool hasEstimate() const;

    bool hasEstimate() const;





private:

private:

    struct Parameter {

        float gyroVar;

        float gyroBiasVar;

        float accStdev;

        float magStdev;

    } mParam;



    mat<mat33_t, 2, 2> Phi;

    mat<mat33_t, 2, 2> Phi;

    vec3_t Ba, Bm;

    vec3_t Ba, Bm;

    uint32_t mInitState;

    uint32_t mInitState;

    float mGyroRate;

    float mGyroRate;

    vec<vec3_t, 3> mData;

    vec<vec3_t, 3> mData;

    size_t mCount[3];

    size_t mCount[3];

    int mMode;



    enum { ACC=0x1, MAG=0x2, GYRO=0x4 };

    enum { ACC=0x1, MAG=0x2, GYRO=0x4 };

    bool checkInitComplete(int, const vec3_t& w, float d = 0);

    bool checkInitComplete(int, const vec3_t& w, float d = 0);

    void initFusion(const vec4_t& q0, float dT);

    void initFusion(const vec4_t& q0, float dT);
@@ -78,6 +94,7 @@ private: 
    void predict(const vec3_t& w, float dT);

    void predict(const vec3_t& w, float dT);

    void update(const vec3_t& z, const vec3_t& Bi, float sigma);

    void update(const vec3_t& z, const vec3_t& Bi, float sigma);

    static mat34_t getF(const vec4_t& p);

    static mat34_t getF(const vec4_t& p);

    static vec3_t getOrthogonal(const vec3_t &v);

};

};





}; // namespace android

}; // namespace android

services/sensorservice/GravitySensor.cpp

+4 −4
Original line number Original line Diff line number Diff line
@@ -47,9 +47,9 @@ bool GravitySensor::process(sensors_event_t* outEvent, 
    const static double NS2S = 1.0 / 1000000000.0;

    const static double NS2S = 1.0 / 1000000000.0;

    if (event.type == SENSOR_TYPE_ACCELEROMETER) {

    if (event.type == SENSOR_TYPE_ACCELEROMETER) {

        vec3_t g;

        vec3_t g;

        if (!mSensorFusion.hasEstimate())

        if (!mSensorFusion.hasEstimate(FUSION_NOMAG))

            return false;

            return false;

        const mat33_t R(mSensorFusion.getRotationMatrix());

        const mat33_t R(mSensorFusion.getRotationMatrix(FUSION_NOMAG));

        // FIXME: we need to estimate the length of gravity because

        // FIXME: we need to estimate the length of gravity because

        // the accelerometer may have a small scaling error. This

        // the accelerometer may have a small scaling error. This

        // translates to an offset in the linear-acceleration sensor.

        // translates to an offset in the linear-acceleration sensor.
@@ -67,11 +67,11 @@ bool GravitySensor::process(sensors_event_t* outEvent, 
}

}





status_t GravitySensor::activate(void* ident, bool enabled) {

status_t GravitySensor::activate(void* ident, bool enabled) {

    return mSensorFusion.activate(ident, enabled);

    return mSensorFusion.activate(FUSION_NOMAG, ident, enabled);

}

}





status_t GravitySensor::setDelay(void* ident, int /*handle*/, int64_t ns) {

status_t GravitySensor::setDelay(void* ident, int /*handle*/, int64_t ns) {

    return mSensorFusion.setDelay(ident, ns);

    return mSensorFusion.setDelay(FUSION_NOMAG, ident, ns);

}

}





Sensor GravitySensor::getSensor() const {

Sensor GravitySensor::getSensor() const {

services/sensorservice/OrientationSensor.cpp

+2 −2
Original line number Original line Diff line number Diff line
@@ -66,11 +66,11 @@ bool OrientationSensor::process(sensors_event_t* outEvent, 
}

}





status_t OrientationSensor::activate(void* ident, bool enabled) {

status_t OrientationSensor::activate(void* ident, bool enabled) {

    return mSensorFusion.activate(ident, enabled);

    return mSensorFusion.activate(FUSION_9AXIS, ident, enabled);

}

}





status_t OrientationSensor::setDelay(void* ident, int /*handle*/, int64_t ns) {

status_t OrientationSensor::setDelay(void* ident, int /*handle*/, int64_t ns) {

    return mSensorFusion.setDelay(ident, ns);

    return mSensorFusion.setDelay(FUSION_9AXIS, ident, ns);

}

}





Sensor OrientationSensor::getSensor() const {

Sensor OrientationSensor::getSensor() const {