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

diff --git a/include/lsm9ds1.hpp b/include/lsm9ds1.hpp
index 88c88f8..0629285 100644 (file)
--- 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 (file)
--- 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