| 0,0 → 1,97 |
|---|
| #ifndef AP_AHRS_H |
| #define AP_AHRS_H |
| /* |
| 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_Math.h" |
| #include <inttypes.h> |
| #include "AP_Compass.h" |
| #include "AP_GPS.h" |
| |
| class AP_AHRS { |
| public: |
| AP_AHRS(/*IMU *imu, */ GPS *&gps): |
| _gps(gps) |
| { |
| // base the ki values by the sensors maximum drift |
| // rate. The APM2 has gyros which are much less drift |
| // prone than the APM1, so we should have a lower ki, |
| // which will make us less prone to increasing omegaI |
| // incorrectly due to sensor noise |
| _gyro_drift_limit = ToRad(3.0/60); |
| } |
| |
| // Accessors |
| void set_centripetal(bool b) { _centripetal = b; } |
| bool get_centripetal(void) { return _centripetal; } |
| void set_compass(Compass *compass) { _compass = compass; } |
| |
| // Methods |
| virtual void update(int16_t attitude[3], float delta_t) = 0; |
| |
| // Euler angles (radians) |
| float roll; |
| float pitch; |
| float yaw; |
| |
| // return a smoothed and corrected gyro vector |
| virtual Vector3f get_gyro(void); |
| |
| // return the current estimate of the gyro drift |
| virtual Vector3f get_gyro_drift(void) = 0; |
| |
| // reset the current attitude, used on new IMU calibration |
| virtual void reset(bool recover_eulers=false) = 0; |
| |
| // how often our attitude representation has gone out of range |
| uint8_t renorm_range_count; |
| |
| // how often our attitude representation has blown up completely |
| uint8_t renorm_blowup_count; |
| |
| // return the average size of the roll/pitch error estimate |
| // since last call |
| virtual float get_error_rp(void)= 0; |
| |
| // return the average size of the yaw error estimate |
| // since last call |
| virtual float get_error_yaw(void) = 0; |
| |
| // return a DCM rotation matrix representing our current |
| // attitude |
| virtual Matrix3f get_dcm_matrix(void) = 0; |
| |
| protected: |
| // pointer to compass object, if enabled |
| Compass * _compass; |
| |
| // time in microseconds of last compass update |
| uint32_t _compass_last_update; |
| |
| // 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; |
| // IMU *_imu; |
| |
| // true if we are doing centripetal acceleration correction |
| bool _centripetal; |
| |
| // the limit of the gyro drift claimed by the sensors, in |
| // radians/s/s |
| float _gyro_drift_limit; |
| |
| // acceleration due to gravity in m/s/s |
| static const float _gravity = 9.80665; |
| }; |
| |
| #include "AP_AHRS_DCM.h" |
| //#include <AP_AHRS_Quaternion.h> |
| //#include <AP_AHRS_HIL.h> |
| |
| #endif // AP_AHRS_H |