From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Sun, 18 Aug 2019 19:58:46 +0000 (-0300)
Subject: Fix accelerometer and gyroscope axis to be right hand rule compliant and coincident... 
X-Git-Tag: f1~4
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=084bdb2944f51c2c1ed768be17cee5ee7eb723b7;p=imufusion.git

Fix accelerometer and gyroscope axis to be right hand rule compliant and coincident with magnetometer axis.
---

diff --git a/include/lsm9ds1.hpp b/include/lsm9ds1.hpp
index 88c88f8..0629285 100644
--- a/include/lsm9ds1.hpp
+++ b/include/lsm9ds1.hpp
@@ -64,7 +64,9 @@ class Lsm9ds1: public Imu
 	~Lsm9ds1(void);
 
 	/** @brief Accelerometer readings.
-		@return raw accelerometer readings.
+		@return raw accelerometer readings.  X axis signal is
+		inverted to make the system right hand rule compliant and
+		coincident with magnetometer axis.
 	*/
 	Eigen::Vector3d accel(void);
 
@@ -74,7 +76,9 @@ class Lsm9ds1: public Imu
 	Eigen::Vector3d mag(void);
 
 	/** @brief Gyroscope readings.
-		@return raw gyroscope readings.
+		@return raw gyroscope readings.  X axis signal is inverted
+		to make the system right hand rule compliant and coincident
+		with magnetometer axis.
 	*/
 	Eigen::Vector3d gyro(void);
 };
diff --git a/lib/lsm9ds1.cpp b/lib/lsm9ds1.cpp
index 06515ca..074196b 100644
--- a/lib/lsm9ds1.cpp
+++ b/lib/lsm9ds1.cpp
@@ -110,10 +110,17 @@ Eigen::Vector3d Lsm9ds1::accel(void)
 	axis3bit16_t accel;
 	if(status.status_imu.xlda && lsm9ds1_acceleration_raw_get(&imuCtx_,accel.u8bit)!=0)
 		throw Exception("Error getting acceleration.");
-	Eigen::Vector3d accelVector;
-	for(int i=0;i < 3;i++) accelVector[i]=lsm9ds1_from_fs4g_to_mpssq(accel.i16bit[i]);
-
-	return accelVector;
+	Eigen::Vector3d accelEigen;
+	for(int i=0;i < 3;i++) accelEigen[i]=lsm9ds1_from_fs4g_to_mpssq(accel.i16bit[i]);
+	
+	/* According to datasheet the accelerometer axis do not follow the
+	   right hand rule.  Furhtermore, they are no the same as the
+	   magnetometer axis, which do follow the right hand side.  Fix this
+	   to make all axis the coincident.
+	*/
+	accelEigen[0]*=-1.0;
+
+	return accelEigen;
 }
 
 Eigen::Vector3d Lsm9ds1::mag(void)
@@ -125,10 +132,10 @@ Eigen::Vector3d Lsm9ds1::mag(void)
 	axis3bit16_t mag;		
 	if(status.status_mag.zyxda && lsm9ds1_magnetic_raw_get(&magCtx_,mag.u8bit)!=0)
 		throw Exception("Error getting magnetometer.");
-	Eigen::Vector3d magVector;
-	for(int i=0;i < 3;i++) magVector[i]=lsm9ds1_from_fs16gauss_to_uT(mag.i16bit[i]);
+	Eigen::Vector3d magEigen;
+	for(int i=0;i < 3;i++) magEigen[i]=lsm9ds1_from_fs16gauss_to_uT(mag.i16bit[i]);
 
-	return magVector;
+	return magEigen;
 }
 
 Eigen::Vector3d Lsm9ds1::gyro(void)
@@ -140,10 +147,17 @@ Eigen::Vector3d Lsm9ds1::gyro(void)
 	axis3bit16_t gyro;
 	if(status.status_imu.gda && lsm9ds1_angular_rate_raw_get(&imuCtx_,gyro.u8bit)!=0)
 		throw Exception("Error getting gyroscope.");
-	Eigen::Vector3d gyroVector;
-	for(int i=0;i < 3;i++) gyroVector[i]=lsm9ds1_from_fs2000dps_to_rps(gyro.i16bit[i]);
-
-	return gyroVector;
+	Eigen::Vector3d gyroEigen;
+	for(int i=0;i < 3;i++) gyroEigen[i]=lsm9ds1_from_fs2000dps_to_rps(gyro.i16bit[i]);
+	
+	/* According to datasheet the gyroscope axis do not follow the
+	   right hand rule.  Furhtermore, they are no the same as the
+	   magnetometer axis, which do follow the right hand side.  Fix this
+	   to make all axis the coincident.
+	*/
+	gyroEigen[0]*=-1.0;
+
+	return gyroEigen;
 }
 
 }	// namespace