Subversion Repositories FlightCtrl

/*
    APM_AHRS_DCM.cpp

    AHRS system using DCM matrices

    Based on DCM code by Doug Weibel, Jordi Mu�oz and Jose Julio. DIYDrones.com

    Adapted for the general ArduPilot AHRS interface by Andrew Tridgell

    This library is free software; you can redistribute it and/or
    modify it under the terms of the GNU Lesser General Public License
    as published by the Free Software Foundation; either version 2.1
    of the License, or (at your option) any later version.
*/
#include "AP_AHRS.h"
#include "output.h"
#include "profiler.h"
#include "analog.h"
#include "debug.h"
#include <stdio.h>

// this is the speed in cm/s above which we first get a yaw lock with
// the GPS
#define GPS_SPEED_MIN 300

// this is the speed in cm/s at which we stop using drift correction
// from the GPS and wait for the ground speed to get above GPS_SPEED_MIN
#define GPS_SPEED_RESET 100

// run a full DCM update round
void
AP_AHRS_DCM::update(int16_t attitude[3], float delta_t)
{
    // Get current values for gyros
    _gyro_vector  = gyro_attitude;
    _accel_vector = accel;

    // Integrate the DCM matrix using gyro inputs
    matrix_update(delta_t);

    // Normalize the DCM matrix
    normalize();

    // Perform drift correction
    //setCurrentProfiledActivity(DRIFT_CORRECTION);
    drift_correction(delta_t);

    // paranoid check for bad values in the DCM matrix
    //setCurrentProfiledActivity(CHECK_MATRIX);
    check_matrix();

    // Calculate pitch, roll, yaw for stabilization and navigation
    //setCurrentProfiledActivity(EULER_ANGLES);
    euler_angles();

    //setCurrentProfiledActivity(ANGLESOUTPUT);
    attitude[0] = roll * INT16DEG_PI_FACTOR;
    attitude[1] = pitch* INT16DEG_PI_FACTOR;
    attitude[2] = yaw  * INT16DEG_PI_FACTOR;

    // Just for info:
    int16_t sensor = degrees(roll) * 10;
    debugOut.analog[0] = sensor;

    sensor = degrees(pitch) * 10;
    debugOut.analog[1] = sensor;
   
    sensor = degrees(yaw) * 10;
    if (sensor < 0)
        sensor += 3600;
    debugOut.analog[2] = sensor;
}

// update the DCM matrix using only the gyros
void
AP_AHRS_DCM::matrix_update(float _G_Dt)
{
    //setCurrentProfiledActivity(MATRIX_UPDATE1);

    // _omega_integ_corr is used for _centripetal correction
    // (theoretically better than _omega)
    _omega_integ_corr = _gyro_vector + _omega_I;

    // Equation 16, adding proportional and integral correction terms
    _omega = _omega_integ_corr + _omega_P + _omega_yaw_P;

    // this is a replacement of the DCM matrix multiply (equation
    // 17), with known zero elements removed and the matrix
    // operations inlined. This runs much faster than the original
    // version of this code, as the compiler was doing a terrible
    // job of realising that so many of the factors were in common
    // or zero. It also uses much less stack, as we no longer need
    // two additional local matrices

    Vector3f r = _omega * _G_Dt;

    //setCurrentProfiledActivity(MATRIX_UPDATE2);

    _dcm_matrix.rotate(r);
}


// adjust an accelerometer vector for known acceleration forces
void
AP_AHRS_DCM::accel_adjust(Vector3f &accel)
{
    float veloc;
    // compensate for linear acceleration. This makes a
    // surprisingly large difference in the pitch estimate when
    // turning, plus on takeoff and landing
    //float acceleration = _gps->acceleration();
    //accel.x -= acceleration;

    // compensate for centripetal acceleration
    veloc = 0; //_gps->ground_speed * 0.01;

    // We are working with a modified version of equation 26 as
    // our IMU object reports acceleration in the positive axis
    // direction as positive

    // Equation 26 broken up into separate pieces
    accel.y -= _omega_integ_corr.z * veloc;
    accel.z += _omega_integ_corr.y * veloc;
}

/*
  reset the DCM matrix and omega. Used on ground start, and on
  extreme errors in the matrix
 */
void
AP_AHRS_DCM::reset(bool recover_eulers)
{
    if (_compass != NULL) {
        _compass->null_offsets_disable();
    }

    // reset the integration terms
    _omega_I.zero();
    _omega_P.zero();
    _omega_yaw_P.zero();
    _omega_integ_corr.zero();
    _omega.zero();

    // if the caller wants us to try to recover to the current
    // attitude then calculate the dcm matrix from the current
    // roll/pitch/yaw values
    if (recover_eulers && !isnan(roll) && !isnan(pitch) && !isnan(yaw)) {
        _dcm_matrix.from_euler(roll, pitch, yaw);
    } else {
        // otherwise make it flat
        _dcm_matrix.from_euler(0, 0, 0);
    }

    if (_compass != NULL) {
        _compass->null_offsets_enable();    // This call is needed to restart the nulling
        // Otherwise the reset in the DCM matrix can mess up
        // the nulling
    }
}

/*
  check the DCM matrix for pathological values
 */
void
AP_AHRS_DCM::check_matrix(void)
{
    if (_dcm_matrix.is_nan()) {
        //Serial.printf("ERROR: DCM matrix NAN\n");
        printf("ERROR: DCM matrix NAN\n");
        renorm_blowup_count++;
        reset(true);
        return;
    }
    // some DCM matrix values can lead to an out of range error in
    // the pitch calculation via asin().  These NaN values can
    // feed back into the rest of the DCM matrix via the
    // error_course value.
    if (!(_dcm_matrix.c.x < 1.0 &&
          _dcm_matrix.c.x > -1.0)) {
        // We have an invalid matrix. Force a normalisation.
        renorm_range_count++;
        normalize();

        if (_dcm_matrix.is_nan() ||
            fabs(_dcm_matrix.c.x) > 10) {
            // normalisation didn't fix the problem! We're
            // in real trouble. All we can do is reset
            //Serial.printf("ERROR: DCM matrix error. _dcm_matrix.c.x=%f\n",
            //     _dcm_matrix.c.x);
            printf("ERROR: DCM matrix error. _dcm_matrix.c.x=%f\n", (double)_dcm_matrix.c.x);
            renorm_blowup_count++;
            reset(true);
        }
    }
}

// renormalise one vector component of the DCM matrix
// this will return false if renormalization fails
bool
AP_AHRS_DCM::renorm(Vector3f const &a, Vector3f &result)
{
    float   renorm_val;

    // numerical errors will slowly build up over time in DCM,
    // causing inaccuracies. We can keep ahead of those errors
    // using the renormalization technique from the DCM IMU paper
    // (see equations 18 to 21).

    // For APM we don't bother with the taylor expansion
    // optimisation from the paper as on our 2560 CPU the cost of
    // the sqrt() is 44 microseconds, and the small time saving of
    // the taylor expansion is not worth the potential of
    // additional error buildup.

    // Note that we can get significant renormalisation values
    // when we have a larger delta_t due to a glitch eleswhere in
    // APM, such as a I2c timeout or a set of EEPROM writes. While
    // we would like to avoid these if possible, if it does happen
    // we don't want to compound the error by making DCM less
    // accurate.

    renorm_val = 1.0 / a.length();

    // keep the average for reporting
    _renorm_val_sum += renorm_val;
    _renorm_val_count++;

    if (!(renorm_val < 2.0 && renorm_val > 0.5)) {
        // this is larger than it should get - log it as a warning
        renorm_range_count++;
        if (!(renorm_val < 1.0e6 && renorm_val > 1.0e-6)) {
            // we are getting values which are way out of
            // range, we will reset the matrix and hope we
            // can recover our attitude using drift
            // correction before we hit the ground!
            //Serial.printf("ERROR: DCM renormalisation error. renorm_val=%f\n",
            //     renorm_val);
            printf("ERROR: DCM renormalisation error. renorm_val=%f\n", (double)renorm_val);
            renorm_blowup_count++;
            return false;
        }
    }

    result = a * renorm_val;
    return true;
}

/*************************************************
Direction Cosine Matrix IMU: Theory
William Premerlani and Paul Bizard

Numerical errors will gradually reduce the orthogonality conditions expressed by equation 5
to approximations rather than identities. In effect, the axes in the two frames of reference no
longer describe a rigid body. Fortunately, numerical error accumulates very slowly, so it is a
simple matter to stay ahead of it.
We call the process of enforcing the orthogonality conditions �renormalization�.
*/
void
AP_AHRS_DCM::normalize(void)
{
    float error;
    Vector3f t0, t1, t2;

    //setCurrentProfiledActivity(MATRIX_NORMALIZE1);

    error = _dcm_matrix.a * _dcm_matrix.b;                      // eq.18
    t0 = _dcm_matrix.a - (_dcm_matrix.b * (0.5f * error));      // eq.19
    t1 = _dcm_matrix.b - (_dcm_matrix.a * (0.5f * error));      // eq.19
    t2 = t0 % t1;

    //setCurrentProfiledActivity(MATRIX_NORMALIZE2);
    if (!renorm(t0, _dcm_matrix.a) ||
        !renorm(t1, _dcm_matrix.b) ||
        !renorm(t2, _dcm_matrix.c)) {
        // Our solution is blowing up and we will force back
        // to last euler angles
        reset(true);
    }
}


// perform drift correction. This function aims to update _omega_P and
// _omega_I with our best estimate of the short term and long term
// gyro error. The _omega_P value is what pulls our attitude solution
// back towards the reference vector quickly. The _omega_I term is an
// attempt to learn the long term drift rate of the gyros.
//
// This function also updates _omega_yaw_P with a yaw correction term
// from our yaw reference vector
void
AP_AHRS_DCM::drift_correction(float deltat)
{
    float error_course = 0;
    Vector3f accel;
    Vector3f error;
    float error_norm = 0;
    float yaw_deltat = 0;

    accel = _accel_vector;

    // if enabled, use the GPS to correct our accelerometer vector
    // for centripetal forces
    if(_centripetal &&
       _gps != NULL &&
       _gps->status() == GPS::GPS_OK) {
        accel_adjust(accel);
    }


    //*****Roll and Pitch***************

    // normalise the accelerometer vector to a standard length
    // this is important to reduce the impact of noise on the
    // drift correction, as very noisy vectors tend to have
    // abnormally high lengths. By normalising the length we
    // reduce their impact.
    float accel_length = accel.length();

    accel *= (_gravity / accel_length);
    if (accel.is_inf()) {
        // we can't do anything useful with this sample
        _omega_P.zero();
        return;
    }

    // calculate the error, in m/s^2, between the attitude
    // implied by the accelerometers and the attitude
    // in the current DCM matrix
    error =  _dcm_matrix.c % accel;

    // Limit max error to limit the effect of noisy values
    // on the algorithm. This limits the error to about 11
    // degrees
    error_norm = error.length();

    if (error_norm > 2) {
        error *= (2 / error_norm);
    }

    // we now want to calculate _omega_P and _omega_I. The
    // _omega_P value is what drags us quickly to the
    // accelerometer reading.
    _omega_P = error * _kp_roll_pitch;

    // the _omega_I is the long term accumulated gyro
    // error. This determines how much gyro drift we can
    // handle.
    _omega_I_sum += error * (_ki_roll_pitch * deltat);
    _omega_I_sum_time += deltat;

    // these sums support the reporting of the DCM state via MAVLink
    _error_rp_sum += error_norm;
    _error_rp_count++;

    // yaw drift correction

    // we only do yaw drift correction when we get a new yaw
    // reference vector. In between times we rely on the gyros for
    // yaw. Avoiding this calculation on every call to
    // update_DCM() saves a lot of time
    if (_compass && _compass->use_for_yaw()) {
        if (_compass->last_update != _compass_last_update) {
            yaw_deltat = 1.0e-6*(_compass->last_update - _compass_last_update);
            if (_have_initial_yaw && yaw_deltat < 2.0) {
                // Equation 23, Calculating YAW error
                // We make the gyro YAW drift correction based
                // on compass magnetic heading
                error_course = (_dcm_matrix.a.x * _compass->heading_y) - (_dcm_matrix.b.x * _compass->heading_x);
                _compass_last_update = _compass->last_update;
            } else {
                // this is our first estimate of the yaw,
                // or the compass has come back online after
                // no readings for 2 seconds.
                //
                // construct a DCM matrix based on the current
                // roll/pitch and the compass heading.
                // First ensure the compass heading has been
                // calculated
                _compass->calculate(_dcm_matrix);

                // now construct a new DCM matrix
                _compass->null_offsets_disable();
                _dcm_matrix.from_euler(roll, pitch, _compass->heading);
                _compass->null_offsets_enable();
                _have_initial_yaw = true;
                _compass_last_update = _compass->last_update;
                error_course = 0;
            }
        }
    } else if (_gps && _gps->status() == GPS::GPS_OK) {
        if (_gps->last_fix_time != _gps_last_update) {
            // Use GPS Ground course to correct yaw gyro drift
            if (_gps->ground_speed >= GPS_SPEED_MIN) {
                yaw_deltat = 1.0e-3*(_gps->last_fix_time - _gps_last_update);
                if (_have_initial_yaw && yaw_deltat < 2.0) {
                    float course_over_ground_x = cos(ToRad(_gps->ground_course/100.0));
                    float course_over_ground_y = sin(ToRad(_gps->ground_course/100.0));
                    // Equation 23, Calculating YAW error
                    error_course = (_dcm_matrix.a.x * course_over_ground_y) - (_dcm_matrix.b.x * course_over_ground_x);
                    _gps_last_update = _gps->last_fix_time;
                } else  {
                    // when we first start moving, set the
                    // DCM matrix to the current
                    // roll/pitch values, but with yaw
                    // from the GPS
                    if (_compass) {
                        _compass->null_offsets_disable();
                    }
                    _dcm_matrix.from_euler(roll, pitch, ToRad(_gps->ground_course));
                    if (_compass) {
                        _compass->null_offsets_enable();
                    }
                    _have_initial_yaw =  true;
                    error_course = 0;
                    _gps_last_update = _gps->last_fix_time;
                }
            } else if (_gps->ground_speed >= GPS_SPEED_RESET) {
                // we are not going fast enough to use GPS for
                // course correction, but we won't reset
                // _have_initial_yaw yet, instead we just let
                // the gyro handle yaw
                error_course = 0;
            } else {
                // we are moving very slowly. Reset
                // _have_initial_yaw and adjust our heading
                // rapidly next time we get a good GPS ground
                // speed
                error_course = 0;
                _have_initial_yaw = false;
            }
        }
    }

    // see if there is any error in our heading relative to the
    // yaw reference. This will be zero most of the time, as we
    // only calculate it when we get new data from the yaw
    // reference source
    if (yaw_deltat == 0 || error_course == 0) {
        // we don't have a new reference heading. Slowly
        // decay the _omega_yaw_P to ensure that if we have
        // lost the yaw reference sensor completely we don't
        // keep using a stale offset
        _omega_yaw_P *= 0.97;
        goto check_sum_time;
    }

    // ensure the course error is scaled from -PI to PI
    if (error_course > PI) {
        error_course -= 2*PI;
    } else if (error_course < -PI) {
        error_course += 2*PI;
    }

    // Equation 24, Applys the yaw correction to the XYZ rotation of the aircraft
    // this gives us an error in radians
    error = _dcm_matrix.c * error_course;

    // Adding yaw correction to proportional correction vector. We
    // allow the yaw reference source to affect all 3 components
    // of _omega_yaw_P as we need to be able to correctly hold a
    // heading when roll and pitch are non-zero
    _omega_yaw_P = error * _kp_yaw;

    // add yaw correction to integrator correction vector, but
    // only for the z gyro. We rely on the accelerometers for x
    // and y gyro drift correction. Using the compass or GPS for
    // x/y drift correction is too inaccurate, and can lead to
    // incorrect builups in the x/y drift. We rely on the
    // accelerometers to get the x/y components right
    _omega_I_sum.z += error.z * (_ki_yaw * yaw_deltat);

    // we keep the sum of yaw error for reporting via MAVLink.
    _error_yaw_sum += error_course;
    _error_yaw_count++;

check_sum_time:
    if (_omega_I_sum_time > 10) {
        // every 10 seconds we apply the accumulated
        // _omega_I_sum changes to _omega_I. We do this to
        // prevent short term feedback between the P and I
        // terms of the controller. The _omega_I vector is
        // supposed to refect long term gyro drift, but with
        // high noise it can start to build up due to short
        // term interactions. By summing it over 10 seconds we
        // prevent the short term interactions and can apply
        // the slope limit more accurately
        float drift_limit = _gyro_drift_limit * _omega_I_sum_time;
        _omega_I_sum.x = constrain(_omega_I_sum.x, -drift_limit, drift_limit);
        _omega_I_sum.y = constrain(_omega_I_sum.y, -drift_limit, drift_limit);
        _omega_I_sum.z = constrain(_omega_I_sum.z, -drift_limit, drift_limit);

        _omega_I += _omega_I_sum;

        // zero the sum
        _omega_I_sum.zero();
        _omega_I_sum_time = 0;
    }
}


// calculate the euler angles which will be used for high level
// navigation control
void
AP_AHRS_DCM::euler_angles(void)
{
    _dcm_matrix.to_euler(&roll, &pitch, &yaw);
}

/* reporting of DCM state for MAVLink */

// average error_roll_pitch since last call
float AP_AHRS_DCM::get_error_rp(void)
{
    if (_error_rp_count == 0) {
        // this happens when telemetry is setup on two
        // serial ports
        return _error_rp_last;
    }
    _error_rp_last = _error_rp_sum / _error_rp_count;
    _error_rp_sum = 0;
    _error_rp_count = 0;
    return _error_rp_last;
}

// average error_yaw since last call
float AP_AHRS_DCM::get_error_yaw(void)
{
    if (_error_yaw_count == 0) {
        // this happens when telemetry is setup on two
        // serial ports
        return _error_yaw_last;
    }
    _error_yaw_last = _error_yaw_sum / _error_yaw_count;
    _error_yaw_sum = 0;
    _error_yaw_count = 0;
    return _error_yaw_last;
}