ardupilot/libraries/AP_DCM/AP_DCM.h

103 lines
2.6 KiB
C
Raw Normal View History

#ifndef AP_DCM_h
#define AP_DCM_h
// teporarily include all other classes here
// since this naming is a bit off from the
// convention and the AP_DCM should be the top
// header file
#include "AP_DCM_HIL.h"
#include <FastSerial.h>
#include <AP_Math.h>
#include <inttypes.h>
#include "WProgram.h"
#include <AP_Compass.h>
#include <AP_ADC.h>
#include <AP_GPS.h>
#include <AP_IMU.h>
class AP_DCM
{
public:
// Constructors
AP_DCM(IMU *imu, GPS *&gps, Compass *withCompass = NULL) :
_imu(imu),
_gps(gps),
_compass(withCompass),
_dcm_matrix(1, 0, 0,
0, 1, 0,
0, 0, 1),
_course_over_ground_x(0),
_course_over_ground_y(1),
_health(1.)
{}
// Accessors
Vector3f get_gyro(void) {return _omega_integ_corr; } // We return the raw gyro vector corrected for bias
Vector3f get_accel(void) { return _accel_vector; }
Matrix3f get_dcm_matrix(void) {return _dcm_matrix; }
Matrix3f get_dcm_transposed(void) {Matrix3f temp = _dcm_matrix; return temp.transpose();}
void set_centripetal(bool b);
void set_compass(Compass *compass);
// Methods
void update_DCM(float _G_Dt);
float get_health(void);
long roll_sensor; // Degrees * 100
long pitch_sensor; // Degrees * 100
long yaw_sensor; // Degrees * 100
float roll; // Radians
float pitch; // Radians
float yaw; // Radians
uint8_t gyro_sat_count;
uint8_t renorm_sqrt_count;
uint8_t renorm_blowup_count;
private:
// Methods
void read_adc_raw(void);
void accel_adjust(void);
float read_adc(int select);
void matrix_update(float _G_Dt);
void normalize(void);
Vector3f renorm(Vector3f const &a, int &problem);
void drift_correction(void);
void euler_angles(void);
// members
Compass * _compass;
// note: we use ref-to-pointer here so that our caller can change the GPS without our noticing
// IMU under us without our noticing.
GPS *&_gps; // note: this is a reference to a pointer owned by the caller
IMU *_imu;
Matrix3f _dcm_matrix;
Vector3f _accel_vector; // Store the acceleration in a vector
Vector3f _gyro_vector; // Store the gyros turn rate in a vector
Vector3f _omega_P; // Omega Proportional correction
Vector3f _omega_I; // Omega Integrator correction
Vector3f _omega_integ_corr; // Partially corrected Gyro_Vector data - used for centrepetal correction
Vector3f _omega; // Corrected Gyro_Vector data
Vector3f _error_roll_pitch;
Vector3f _error_yaw;
float _errorCourse;
float _course_over_ground_x; // Course overground X axis
float _course_over_ground_y; // Course overground Y axis
float _health;
bool _centripetal;
};
#endif