diff --git a/include/sta/drivers/bmi088.hpp b/include/sta/drivers/bmi088.hpp
index 80e486b..629ff83 100644
--- a/include/sta/drivers/bmi088.hpp
+++ b/include/sta/drivers/bmi088.hpp
@@ -95,9 +95,11 @@ namespace sta
         bool busRead(Part part, uint8_t reg, uint8_t * buffer);
         bool busWrite(Part part, uint8_t reg, uint8_t value);
 
-        void getRawRotation(uint16_t* x, uint16_t* y, uint16_t* z);
-        void getRawAcceleration(uint16_t* x, uint16_t* y, uint16_t* z);
+        void getRawRotation(int16_t* x, int16_t* y, int16_t* z);
+        void getRawAcceleration(int16_t* x, int16_t* y, int16_t* z);
 
+        float lsbToMps2(int16_t val);
+        float lsbToDPS(int16_t val);
         void convertRawToActual(uint16_t* i, float* f, float f_range);
 
         Device* gyro_device;
diff --git a/src/bmi088.cpp b/src/bmi088.cpp
index 7ee93aa..9bcd7b4 100644
--- a/src/bmi088.cpp
+++ b/src/bmi088.cpp
@@ -1,6 +1,9 @@
 #include <sta/drivers/bmi088.hpp>
 
 #include <sta/debug/assert.hpp>
+#include <math.h>
+
+#define GRAVITY		9.80665f
 
 namespace sta
 {
@@ -65,30 +68,31 @@ namespace sta
     {
     	busWrite(GYROSCOPE, BMI088_REG_GYRO_BANDWIDTH, bandwidth);
     }
-    void BMI088::getRawRotation(uint16_t* x, uint16_t* y, uint16_t* z)
+    void BMI088::getRawRotation(int16_t* x, int16_t* y, int16_t* z)
     {
     	uint8_t x_lsb, x_msb, y_lsb, y_msb, z_lsb, z_msb;
 
 		busRead(GYROSCOPE, BMI088_REG_GYRO_RATE_X_LSB, &x_lsb);
 		busRead(GYROSCOPE, BMI088_REG_GYRO_RATE_X_MSB, &x_msb);
-		*x = ((uint16_t)x_msb)<<8 | (uint16_t)x_lsb;
+		*x = (int16_t)(((uint16_t)x_msb)<<8 | (uint16_t)x_lsb);
 
 		busRead(GYROSCOPE, BMI088_REG_GYRO_RATE_Y_LSB, &y_lsb);
 		busRead(GYROSCOPE, BMI088_REG_GYRO_RATE_Y_MSB, &y_msb);
-		*y = ((uint16_t)y_msb)<<8 | (uint16_t)y_lsb;
+		*y = (int16_t)(((uint16_t)y_msb)<<8 | (uint16_t)y_lsb);
 
 		busRead(GYROSCOPE, BMI088_REG_GYRO_RATE_Z_LSB, &z_lsb);
 		busRead(GYROSCOPE, BMI088_REG_GYRO_RATE_Z_MSB, &z_msb);
-		*z = ((uint16_t)z_msb)<<8 | (uint16_t)z_lsb;
+		*z = (int16_t)(((uint16_t)z_msb)<<8 | (uint16_t)z_lsb);
 
     }
     void BMI088::getRotation(float* x, float* y, float* z)
     {
-    	uint16_t i_x,i_y,i_z;
+    	int16_t i_x,i_y,i_z;
 		getRawRotation(&i_x, &i_y, &i_z);
-		convertRawToActual(&i_x, x, f_gyro_range);
-		convertRawToActual(&i_y, y, f_gyro_range);
-		convertRawToActual(&i_z, z, f_gyro_range);
+
+		*x = lsbToDPS(i_x);
+		*y = lsbToDPS(i_y);
+		*z = lsbToDPS(i_z);
     }
 
 
@@ -103,16 +107,16 @@ namespace sta
     	switch(range)
     	{
     	case _3G:
-    		f_accel_range = 3*9.80665;
+    		f_accel_range = 3*GRAVITY;
     		break;
     	case _6G:
-    		f_accel_range = 6*9.80665;
+    		f_accel_range = 6*GRAVITY;
     		break;
     	case _12G:
-    		f_accel_range = 12*9.80665;
+    		f_accel_range = 12*GRAVITY;
     		break;
     	case _24G:
-    		f_accel_range = 24*9.80665;
+    		f_accel_range = 24*GRAVITY;
     		break;
     	default:
     		break;
@@ -123,30 +127,31 @@ namespace sta
     	uint8_t data = bandwidth<<4 | odr;//TODO are your sure? shift three or four times?
     	busWrite(ACCELEROMETER, BMI088_REG_ACC_CONF, data);
     }
-    void BMI088::getRawAcceleration(uint16_t* x, uint16_t* y, uint16_t* z)
+    void BMI088::getRawAcceleration(int16_t* x, int16_t* y, int16_t* z)
     {
     	uint8_t x_lsb, x_msb, y_lsb, y_msb, z_lsb, z_msb;
 
 		busRead(ACCELEROMETER, BMI088_REG_ACC_X_LSB, &x_lsb);
 		busRead(ACCELEROMETER, BMI088_REG_ACC_X_MSB, &x_msb);
-		*x = ((uint16_t)x_msb)<<8 | (uint16_t)x_lsb;
+		*x = (int16_t)(((uint16_t)x_msb)<<8 | (uint16_t)x_lsb);
 
 		busRead(ACCELEROMETER, BMI088_REG_ACC_Y_LSB, &y_lsb);
 		busRead(ACCELEROMETER, BMI088_REG_ACC_Y_MSB, &y_msb);
-		*y = ((uint16_t)y_msb)<<8 | (uint16_t)y_lsb;
+		*y = (int16_t)(((uint16_t)y_msb)<<8 | (uint16_t)y_lsb);
 
 		busRead(ACCELEROMETER, BMI088_REG_ACC_Z_LSB, &z_lsb);
 		busRead(ACCELEROMETER, BMI088_REG_ACC_Z_MSB, &z_msb);
-		*z = ((uint16_t)z_msb)<<8 | (uint16_t)z_lsb;
+		*z = (int16_t)(((uint16_t)z_msb)<<8 | (uint16_t)z_lsb);
 
     }
     void BMI088::getAcceleration(float* x, float* y, float* z)
     {
-    	uint16_t i_x,i_y,i_z;
+    	int16_t i_x,i_y,i_z;
 		getRawAcceleration(&i_x, &i_y, &i_z);
-		convertRawToActual(&i_x, x, f_accel_range);
-		convertRawToActual(&i_y, y, f_accel_range);
-		convertRawToActual(&i_z, z, f_accel_range);
+
+		*x = lsbToMps2(i_x);
+		*y = lsbToMps2(i_y);
+		*z = lsbToMps2(i_z);
     }
 
 
@@ -190,6 +195,22 @@ namespace sta
 		return true;
     }
 
+    float BMI088::lsbToMps2(int16_t val)
+    {
+    	double power = 2;
+		float half_scale = (float)((pow((double)power, (double)16) / 2.0f));
+
+		return (val * f_accel_range) / half_scale;
+    }
+
+	float BMI088::lsbToDPS(int16_t val)
+	{
+		double power = 2;
+		float half_scale = (float)((pow((double)power, (double)16) / 2.0f));
+
+		return (f_gyro_range / (half_scale)) * (val);
+	}
+
     void BMI088::convertRawToActual(uint16_t* i, float* f, float f_range){
 		int16_t sign = (int16_t)((*i & 0b1000000000000000) >> 15);
 		if(sign)