Go to the documentation of this file.00001 #ifndef AP_DCM_h
00002 #define AP_DCM_h
00003
00004
00005
00006
00007
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
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
00047 Vector3f get_gyro(void) {return _omega_integ_corr; }
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
00056 void update_DCM(float _G_Dt);
00057
00058 float get_health(void);
00059
00060 long roll_sensor;
00061 long pitch_sensor;
00062 long yaw_sensor;
00063
00064 float roll;
00065 float pitch;
00066 float yaw;
00067
00068 uint8_t gyro_sat_count;
00069 uint8_t renorm_sqrt_count;
00070 uint8_t renorm_blowup_count;
00071
00072 private:
00073
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
00084 Compass * _compass;
00085 GPS * _gps;
00086 IMU * _imu;
00087
00088 Matrix3f _dcm_matrix;
00089
00090 Vector3f _accel_vector;
00091 Vector3f _gyro_vector;
00092 Vector3f _omega_P;
00093 Vector3f _omega_I;
00094 Vector3f _omega_integ_corr;
00095 Vector3f _omega;
00096 Vector3f _error_roll_pitch;
00097 Vector3f _error_yaw;
00098 float _errorCourse;
00099 float _course_over_ground_x;
00100 float _course_over_ground_y;
00101 float _health;
00102 bool _centripetal;
00103 };
00104
00105 #endif
00106
00107
00108