There is a bug in AP_AHRS_DCM::drift_correction()

[luweiagi]

There is two bug in this function:

(1) first bug: from here https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_AHRS/AP_AHRS_DCM.cpp#L1024

if (now - _last_wind_time > 2000 && airspeed_sensor_enabled()) {
    // when flying straight use airspeed to get wind estimate if available
    const Vector3f airspeed = _dcm_matrix.colx() * AP::airspeed()->get_airspeed();
    const Vector3f wind = velocity - (airspeed * get_EAS2TAS());
    _wind = _wind * 0.92f + wind * 0.08f;
}

we can see that AP::airspeed()->get_airspeed() means EAS NOT TAS, because TAS = EAS * get_EAS2TAS(). but, at here https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_AHRS/AP_AHRS_DCM.cpp#L710

        float airspeed = _last_airspeed;
#if AP_AIRSPEED_ENABLED
        if (airspeed_sensor_enabled()) {
            airspeed = AP::airspeed()->get_airspeed();
        }
#endif

        // use airspeed to estimate our ground velocity in
        // earth frame by subtracting the wind
        velocity = _dcm_matrix.colx() * airspeed;

        // add in wind estimate
        velocity += _wind;

we can see that AP::airspeed()->get_airspeed() is treated as TAS, because it is added with wind directly. So the above code snippet can be rewritten as:

        float airspeed = _last_airspeed;
#if AP_AIRSPEED_ENABLED
        if (airspeed_sensor_enabled()) {
            airspeed = AP::airspeed()->get_airspeed();
        }
#endif

        // use airspeed to estimate our ground velocity in
        // earth frame by subtracting the wind
        velocity = _dcm_matrix.colx() * airspeed  * get_EAS2TAS();

        // add in wind estimate
        velocity += _wind;

So, the question is AP::airspeed()->get_airspeed() means TAS or EAS on earth?

=========================================================== (2) second bug: from here https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_AHRS/AP_AHRS_DCM.cpp#L489

    float yaw_deltat;

should be fixed as:

    float yaw_deltat = 0.0f;

because this var is used at the end of this function:

    // don't update the drift term if we lost the yaw reference
    // for more than 2 seconds
    if (yaw_deltat < 2.0f && spin_rate < ToRad(SPIN_RATE_LIMIT)) {
        // also add to the I term
        _omega_I_sum.z += error_z * _ki_yaw * yaw_deltat;
    }

so yaw_deltat must be initialized as 0.0f.

[luweiagi]

@peterbarker I don't know who is responsible for maintaining and improving the code of EKF and DCM

[amilcarlucas]

that would probably be @priseborough

[luweiagi]

@priseborough Hi, Paul, does this bug exist?