diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index ee91374c57..cd801a3843 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -58,6 +58,10 @@ public: _airspeed = airspeed; } + IMU* get_imu() { + return _imu; + } + // Methods virtual void update(void) = 0; @@ -71,6 +75,15 @@ public: int32_t pitch_sensor; int32_t yaw_sensor; + float get_pitch_rate_earth(void) { + Vector3f omega = get_gyro(); + return cos(roll) * omega.y - sin(roll) * omega.z; + } + float get_roll_rate_earth(void) { + Vector3f omega = get_gyro(); + return omega.x + tan(pitch)*(omega.y*sin(roll) + omega.z*cos(roll)); + } + // return a smoothed and corrected gyro vector virtual Vector3f get_gyro(void) = 0;