From e7492a99ca9f1893aae546c9eda7a4570603cdac Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Sun, 2 Feb 2020 21:31:09 +0100
Subject: [PATCH] Solved a bug in the reconstruction of the gyro data (it was
 unsigned).

---
 src/darwin_robot.cpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/darwin_robot.cpp b/src/darwin_robot.cpp
index 428377f..a860607 100644
--- a/src/darwin_robot.cpp
+++ b/src/darwin_robot.cpp
@@ -404,9 +404,9 @@ void CDarwinRobot::imu_get_gyro_data(double *x,double *y,double *z)
   if(this->robot_device==NULL)
     throw CDarwinRobotException(_HERE_,"Invalid robot device");
   this->robot_device->read_registers(IMU_GYRO_X_OFFSET,data,6);
-  *x=((double)(data[0]+(data[1]<<8)))/1000.0;
-  *y=((double)(data[2]+(data[3]<<8)))/1000.0;
-  *z=((double)(data[4]+(data[5]<<8)))/1000.0;
+  *x=((double)((signed short int)(data[0]+(data[1]<<8))))/1000.0;
+  *y=((double)((signed short int)(data[2]+(data[3]<<8))))/1000.0;
+  *z=((double)((signed short int)(data[4]+(data[5]<<8))))/1000.0;
 }
 
 // motion manager interface
-- 
GitLab