Details | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 2189 | - | 1 | #ifndef AP_AHRS_H |
| 2 | #define AP_AHRS_H |
||
| 3 | /* |
||
| 4 | AHRS (Attitude Heading Reference System) interface for ArduPilot |
||
| 5 | |||
| 6 | This library is free software; you can redistribute it and/or |
||
| 7 | modify it under the terms of the GNU Lesser General Public |
||
| 8 | License as published by the Free Software Foundation; either |
||
| 9 | version 2.1 of the License, or (at your option) any later version. |
||
| 10 | */ |
||
| 11 | |||
| 12 | #include "AP_Math.h" |
||
| 13 | #include <inttypes.h> |
||
| 14 | #include "AP_Compass.h" |
||
| 15 | #include "AP_GPS.h" |
||
| 16 | |||
| 17 | class AP_AHRS { |
||
| 18 | public: |
||
| 19 | AP_AHRS(/*IMU *imu, */ GPS *&gps): |
||
| 20 | _gps(gps) |
||
| 21 | { |
||
| 22 | // base the ki values by the sensors maximum drift |
||
| 23 | // rate. The APM2 has gyros which are much less drift |
||
| 24 | // prone than the APM1, so we should have a lower ki, |
||
| 25 | // which will make us less prone to increasing omegaI |
||
| 26 | // incorrectly due to sensor noise |
||
| 27 | _gyro_drift_limit = ToRad(3.0/60); |
||
| 28 | } |
||
| 29 | |||
| 30 | // Accessors |
||
| 31 | void set_centripetal(bool b) { _centripetal = b; } |
||
| 32 | bool get_centripetal(void) { return _centripetal; } |
||
| 33 | void set_compass(Compass *compass) { _compass = compass; } |
||
| 34 | |||
| 35 | // Methods |
||
| 36 | virtual void update(int16_t attitude[3], float delta_t) = 0; |
||
| 37 | |||
| 38 | // Euler angles (radians) |
||
| 39 | float roll; |
||
| 40 | float pitch; |
||
| 41 | float yaw; |
||
| 42 | |||
| 43 | // return a smoothed and corrected gyro vector |
||
| 44 | virtual Vector3f get_gyro(void); |
||
| 45 | |||
| 46 | // return the current estimate of the gyro drift |
||
| 47 | virtual Vector3f get_gyro_drift(void) = 0; |
||
| 48 | |||
| 49 | // reset the current attitude, used on new IMU calibration |
||
| 50 | virtual void reset(bool recover_eulers=false) = 0; |
||
| 51 | |||
| 52 | // how often our attitude representation has gone out of range |
||
| 53 | uint8_t renorm_range_count; |
||
| 54 | |||
| 55 | // how often our attitude representation has blown up completely |
||
| 56 | uint8_t renorm_blowup_count; |
||
| 57 | |||
| 58 | // return the average size of the roll/pitch error estimate |
||
| 59 | // since last call |
||
| 60 | virtual float get_error_rp(void)= 0; |
||
| 61 | |||
| 62 | // return the average size of the yaw error estimate |
||
| 63 | // since last call |
||
| 64 | virtual float get_error_yaw(void) = 0; |
||
| 65 | |||
| 66 | // return a DCM rotation matrix representing our current |
||
| 67 | // attitude |
||
| 68 | virtual Matrix3f get_dcm_matrix(void) = 0; |
||
| 69 | |||
| 70 | protected: |
||
| 71 | // pointer to compass object, if enabled |
||
| 72 | Compass * _compass; |
||
| 73 | |||
| 74 | // time in microseconds of last compass update |
||
| 75 | uint32_t _compass_last_update; |
||
| 76 | |||
| 77 | // note: we use ref-to-pointer here so that our caller can change the GPS without our noticing |
||
| 78 | // IMU under us without our noticing. |
||
| 79 | GPS *&_gps; |
||
| 80 | // IMU *_imu; |
||
| 81 | |||
| 82 | // true if we are doing centripetal acceleration correction |
||
| 83 | bool _centripetal; |
||
| 84 | |||
| 85 | // the limit of the gyro drift claimed by the sensors, in |
||
| 86 | // radians/s/s |
||
| 87 | float _gyro_drift_limit; |
||
| 88 | |||
| 89 | // acceleration due to gravity in m/s/s |
||
| 90 | static const float _gravity = 9.80665; |
||
| 91 | }; |
||
| 92 | |||
| 93 | #include "AP_AHRS_DCM.h" |
||
| 94 | //#include <AP_AHRS_Quaternion.h> |
||
| 95 | //#include <AP_AHRS_HIL.h> |
||
| 96 | |||
| 97 | #endif // AP_AHRS_H |