ardupilot/libraries/AP_AHRS/AP_AHRS_DCM.h

88 lines
2.4 KiB
C
Raw Normal View History

#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.
*/
class AP_AHRS_DCM : public AP_AHRS
{
public:
// Constructors
AP_AHRS_DCM(IMU *imu, GPS *&gps) : AP_AHRS(imu, gps)
{
_kp_roll_pitch = 0.13;
_kp_yaw = 0.4;
_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;
}
// 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(void);
void reset(bool recover_eulers = false);
// status reporting
float get_error_rp(void);
float get_error_yaw(void);
private:
float _kp_roll_pitch;
float _ki_roll_pitch;
float _kp_yaw;
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_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