| 0,0 → 1,92 |
|---|
| #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. |
| */ |
| #include "AP_AHRS.h" |
| #include "GPS.h" |
| #include "Constants.h" |
| |
| class AP_AHRS_DCM : public AP_AHRS { |
| public: |
| // Constructors |
| AP_AHRS_DCM(GPS *&gps) : AP_AHRS(gps) { |
| _kp_roll_pitch = 0.13; |
| _kp_yaw = 0.2; |
| _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; |
| _compass = NULL; |
| } |
| |
| // 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(int16_t attitude[3], float delta_t); |
| void reset(bool recover_eulers = false); |
| |
| // status reporting |
| float get_error_rp(void); |
| float get_error_yaw(void); |
| |
| float _kp_yaw; |
| |
| private: |
| float _kp_roll_pitch; |
| float _ki_roll_pitch; |
| 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_I_sum; // summation vector for omegaI |
| float _omega_I_sum_time; |
| 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 |