Subversion Repositories FlightCtrl

#ifndef AP_AHRS_DCM_H
#define AP_AHRS_DCM_H
/*
  DCM based AHRS (Attitude Heading Reference System) interface for
  ArduPilot

  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 "GPS.h"
#include "Constants.h"

class AP_AHRS_DCM : public AP_AHRS {
public:
        // Constructors
        AP_AHRS_DCM(GPS *&gps) : AP_AHRS(gps) {
                _kp_roll_pitch = 0.13;
                _kp_yaw        = 0.2;
                _dcm_matrix(Vector3f(1, 0, 0),
                            Vector3f(0, 1, 0),
                            Vector3f(0, 0, 1));

                // base the ki values on the sensors drift rate
                _ki_roll_pitch = _gyro_drift_limit * 5;
                _ki_yaw        = _gyro_drift_limit * 8;
                _compass = NULL;
        }

        // return the smoothed gyro vector corrected for drift
        Vector3f        get_gyro(void) {return _omega; }
        Matrix3f        get_dcm_matrix(void) {return _dcm_matrix; }

        // return the current drift correction integrator value
        Vector3f        get_gyro_drift(void) {return _omega_I; }

        // Methods
        void            update(int16_t attitude[3], float delta_t);
        void            reset(bool recover_eulers = false);

        // status reporting
        float           get_error_rp(void);
        float           get_error_yaw(void);

        float           _kp_yaw;

private:
        float           _kp_roll_pitch;
        float           _ki_roll_pitch;
        float           _ki_yaw;
        bool            _have_initial_yaw;

        // Methods
        void            accel_adjust(Vector3f &accel);
        void            matrix_update(float _G_Dt);
        void            normalize(void);
        void            check_matrix(void);
        bool            renorm(Vector3f const &a, Vector3f &result);
        void            drift_correction(float deltat);
        void            euler_angles(void);

        // primary representation of attitude
        Matrix3f        _dcm_matrix;

        Vector3f        _gyro_vector;           // Store the gyros turn rate in a vector
        Vector3f        _accel_vector;          // current accel vector

        Vector3f        _omega_P;               // accel Omega Proportional correction
        Vector3f        _omega_yaw_P;           // yaw Omega Proportional correction
        Vector3f        _omega_I;               // Omega Integrator correction
        Vector3f        _omega_I_sum;           // summation vector for omegaI
        float           _omega_I_sum_time;
        Vector3f        _omega_integ_corr;      // Partially corrected Gyro_Vector data - used for centrepetal correction
        Vector3f        _omega;                 // Corrected Gyro_Vector data

        // state to support status reporting
        float           _renorm_val_sum;
        uint16_t        _renorm_val_count;
        float           _error_rp_sum;
        uint16_t        _error_rp_count;
        float           _error_rp_last;
        float           _error_yaw_sum;
        uint16_t        _error_yaw_count;
        float           _error_yaw_last;

        // time in millis when we last got a GPS heading
        uint32_t        _gps_last_update;
};

#endif // AP_AHRS_DCM_H