From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Fri, 12 Sep 2025 07:06:40 +0000 (-0300)
Subject: Fix RPY angles direction.
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;ds=sidebyside;p=bno055_driver.git

Fix RPY angles direction.
---

diff --git a/src/bno055_node.cpp b/src/bno055_node.cpp
index f44d816..b8471d1 100644
--- a/src/bno055_node.cpp
+++ b/src/bno055_node.cpp
@@ -270,10 +270,11 @@ void Bno055Node::sample(void)
 
 	struct bno055_euler_t euler;
 	if(bno055_read_euler_hrp(&euler) == BNO055_SUCCESS)
-                rpy_ << euler.r/BNO055_EULER_DIV_RAD,
-                euler.p/BNO055_EULER_DIV_RAD,
+                rpy_ << -euler.p/BNO055_EULER_DIV_RAD,
+                -euler.r/BNO055_EULER_DIV_RAD,
                 euler.h/BNO055_EULER_DIV_RAD;
         else RCLCPP_ERROR_STREAM(get_logger(),"Error reading Euler angles.\n");
+        	rpy_[2]=atan2(sin(-rpy_[2]),cos(rpy_[2]));
 
 	s8 temp;
 	if(bno055_read_temp_data(&temp) == BNO055_SUCCESS)