Browse Source

AP_AHRS: View: get_gyro_latest should include pitch trim.

gps-1.3.1
Iampete1 3 years ago committed by Andrew Tridgell
parent
commit
e56b6d2765
  1. 4
      libraries/AP_AHRS/AP_AHRS_View.cpp

4
libraries/AP_AHRS/AP_AHRS_View.cpp

@ -84,9 +84,7 @@ void AP_AHRS_View::update() @@ -84,9 +84,7 @@ void AP_AHRS_View::update()
// return a smoothed and corrected gyro vector using the latest ins data (which may not have been consumed by the EKF yet)
Vector3f AP_AHRS_View::get_gyro_latest(void) const {
Vector3f gyro_latest = ahrs.get_gyro_latest();
gyro_latest.rotate(rotation);
return gyro_latest;
return rot_view * ahrs.get_gyro_latest();
}
// rotate a 2D vector from earth frame to body frame

Loading…
Cancel
Save