Subversion Repositories FlightCtrl

Rev

Go to most recent revision | Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
2189 - 1
#ifndef AP_AHRS_DCM_H
2
#define AP_AHRS_DCM_H
3
/*
4
  DCM based AHRS (Attitude Heading Reference System) interface for
5
  ArduPilot
6
 
7
  This library is free software; you can redistribute it and/or
8
  modify it under the terms of the GNU Lesser General Public
9
  License as published by the Free Software Foundation; either
10
  version 2.1 of the License, or (at your option) any later version.
11
*/
12
#include "AP_AHRS.h"
13
#include "GPS.h"
14
#include "Constants.h"
15
 
16
class AP_AHRS_DCM : public AP_AHRS {
17
public:
18
        // Constructors
19
        AP_AHRS_DCM(GPS *&gps) : AP_AHRS(gps) {
20
                _kp_roll_pitch = 0.13;
21
                _kp_yaw        = 0.2;
22
                _dcm_matrix(Vector3f(1, 0, 0),
23
                            Vector3f(0, 1, 0),
24
                            Vector3f(0, 0, 1));
25
 
26
                // base the ki values on the sensors drift rate
27
                _ki_roll_pitch = _gyro_drift_limit * 5;
28
                _ki_yaw        = _gyro_drift_limit * 8;
29
                _compass = NULL;
30
        }
31
 
32
        // return the smoothed gyro vector corrected for drift
33
        Vector3f        get_gyro(void) {return _omega; }
34
        Matrix3f        get_dcm_matrix(void) {return _dcm_matrix; }
35
 
36
        // return the current drift correction integrator value
37
        Vector3f        get_gyro_drift(void) {return _omega_I; }
38
 
39
        // Methods
40
        void            update(int16_t attitude[3], float delta_t);
41
        void            reset(bool recover_eulers = false);
42
 
43
        // status reporting
44
        float           get_error_rp(void);
45
        float           get_error_yaw(void);
46
 
47
        float           _kp_yaw;
48
 
49
private:
50
        float           _kp_roll_pitch;
51
        float           _ki_roll_pitch;
52
        float           _ki_yaw;
53
        bool            _have_initial_yaw;
54
 
55
        // Methods
56
        void            accel_adjust(Vector3f &accel);
57
        void            matrix_update(float _G_Dt);
58
        void            normalize(void);
59
        void            check_matrix(void);
60
        bool            renorm(Vector3f const &a, Vector3f &result);
61
        void            drift_correction(float deltat);
62
        void            euler_angles(void);
63
 
64
        // primary representation of attitude
65
        Matrix3f        _dcm_matrix;
66
 
67
        Vector3f        _gyro_vector;           // Store the gyros turn rate in a vector
68
        Vector3f        _accel_vector;          // current accel vector
69
 
70
        Vector3f        _omega_P;               // accel Omega Proportional correction
71
        Vector3f        _omega_yaw_P;           // yaw Omega Proportional correction
72
        Vector3f        _omega_I;               // Omega Integrator correction
73
        Vector3f        _omega_I_sum;           // summation vector for omegaI
74
        float           _omega_I_sum_time;
75
        Vector3f        _omega_integ_corr;      // Partially corrected Gyro_Vector data - used for centrepetal correction
76
        Vector3f        _omega;                 // Corrected Gyro_Vector data
77
 
78
        // state to support status reporting
79
        float           _renorm_val_sum;
80
        uint16_t        _renorm_val_count;
81
        float           _error_rp_sum;
82
        uint16_t        _error_rp_count;
83
        float           _error_rp_last;
84
        float           _error_yaw_sum;
85
        uint16_t        _error_yaw_count;
86
        float           _error_yaw_last;
87
 
88
        // time in millis when we last got a GPS heading
89
        uint32_t        _gps_last_update;
90
};
91
 
92
#endif // AP_AHRS_DCM_H