From b6a089f90e9ae0f4322d0d6f7b974b940c4b4e8b Mon Sep 17 00:00:00 2001 From: "Bailey K. Srimoungchanh" Date: Wed, 20 Jul 2022 03:10:24 -0500 Subject: [PATCH] AP_AHRS: Fixed body_to_earth body_to_earth was not implemented consistently with earth_to_body. Changed the order of operation to be consistent and rotate reference frame correctly. --- libraries/AP_AHRS/AP_AHRS.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 96f8691220..ccce982edd 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -535,7 +535,7 @@ public: // convert a vector from body to earth frame Vector3f body_to_earth(const Vector3f &v) const { - return v * get_rotation_body_to_ned(); + return get_rotation_body_to_ned() * v; } // convert a vector from earth to body frame