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 |