• Main Page
  • Related Pages
  • Namespaces
  • Classes
  • Files
  • File List
  • File Members

AP_DCM.h

Go to the documentation of this file.
00001 #ifndef AP_DCM_h
00002 #define AP_DCM_h
00003 
00004 // teporarily include all other classes here
00005 // since this naming is a bit off from the
00006 // convention and the AP_DCM should be the top
00007 // header file
00008 #include "AP_DCM_HIL.h"
00009 
00010 #include <FastSerial.h>
00011 #include <AP_Math.h>
00012 #include <inttypes.h>
00013 #include "WProgram.h"
00014 #include <AP_Compass.h>
00015 #include <AP_ADC.h>
00016 #include <AP_GPS.h>
00017 #include <AP_IMU.h>
00018 
00019 
00020 class AP_DCM
00021 {
00022 public:
00023         // Constructors
00024         AP_DCM(IMU *imu, GPS *gps) :
00025                 _imu(imu),
00026                 _gps(gps),
00027                 _compass(0),
00028                 _dcm_matrix(1, 0, 0,
00029                                         0, 1, 0,
00030                                         0, 0, 1),
00031                 _course_over_ground_x(0),
00032                 _course_over_ground_y(1)
00033         {}
00034 
00035         AP_DCM(IMU *imu, GPS *gps, Compass *withCompass) :
00036                 _imu(imu),
00037                 _gps(gps),
00038                 _compass(withCompass),
00039                 _dcm_matrix(1, 0, 0,
00040                                         0, 1, 0,
00041                                         0, 0, 1),
00042                 _course_over_ground_x(0),
00043                 _course_over_ground_y(1)
00044         {}
00045 
00046         // Accessors
00047         Vector3f        get_gyro(void) {return _omega_integ_corr; }             // We return the raw gyro vector corrected for bias
00048         Vector3f        get_accel(void) { return _accel_vector; }
00049         Matrix3f        get_dcm_matrix(void) {return _dcm_matrix; }
00050         Matrix3f        get_dcm_transposed(void) {Matrix3f temp = _dcm_matrix;  return temp.transpose();}
00051 
00052         void            set_centripetal(bool b);
00053         void            set_compass(Compass *compass);
00054 
00055         // Methods
00056         void            update_DCM(float _G_Dt);
00057 
00058         float           get_health(void);
00059 
00060         long            roll_sensor;                                    // Degrees * 100
00061         long            pitch_sensor;                                   // Degrees * 100
00062         long            yaw_sensor;                                             // Degrees * 100
00063 
00064         float           roll;                                                   // Radians
00065         float           pitch;                                                  // Radians
00066         float           yaw;                                                    // Radians
00067 
00068         uint8_t         gyro_sat_count;
00069         uint8_t         renorm_sqrt_count;
00070         uint8_t         renorm_blowup_count;
00071 
00072 private:
00073         // Methods
00074         void            read_adc_raw(void);
00075         void            accel_adjust(void);
00076         float           read_adc(int select);
00077         void            matrix_update(float _G_Dt);
00078         void            normalize(void);
00079         Vector3f        renorm(Vector3f const &a, int &problem);
00080         void            drift_correction(void);
00081         void            euler_angles(void);
00082 
00083         // members
00084         Compass         * _compass;
00085         GPS                             * _gps;
00086         IMU                             * _imu;
00087 
00088         Matrix3f        _dcm_matrix;
00089 
00090         Vector3f        _accel_vector;                          // Store the acceleration in a vector
00091         Vector3f        _gyro_vector;                           // Store the gyros turn rate in a vector
00092         Vector3f        _omega_P;                                       // Omega Proportional correction
00093         Vector3f        _omega_I;                                       // Omega Integrator correction
00094         Vector3f        _omega_integ_corr;                      // Partially corrected Gyro_Vector data - used for centrepetal correction
00095         Vector3f        _omega;                                         // Corrected Gyro_Vector data
00096         Vector3f        _error_roll_pitch;
00097         Vector3f        _error_yaw;
00098         float           _errorCourse;
00099         float           _course_over_ground_x;          // Course overground X axis
00100         float           _course_over_ground_y;          // Course overground Y axis
00101         float           _health;
00102         bool            _centripetal;
00103 };
00104 
00105 #endif
00106 
00107 
00108 

Generated for ArduPilot Libraries by doxygen