diff --git a/src/drivers/MPU6000.cpp b/src/drivers/MPU6000.cpp
index beb8e47..22f2afb 100644
--- a/src/drivers/MPU6000.cpp
+++ b/src/drivers/MPU6000.cpp
@@ -72,7 +72,7 @@ bool mpu6000_spi::init(int sample_rate_div,int low_pass_filter){
     response=spi.write(0x00);
     deselect();
 
-    return true; // TODO
+    return false; // TODO
 }
 
 int mpu6000_spi::enableInterrupt()
@@ -207,7 +207,17 @@ float mpu6000_spi::read_acc(int axis){
 
 float mpu6000_spi::read_acc_deg(int axis)
 {
-    return asin(read_acc(axis))/PI*180;
+    float g(read_acc(axis));
+    
+    if (g > 1.0f)
+    {
+        g = 1.0f;
+    }
+    else if (g < -1.0f)
+    {
+        g = -1.0f;
+    }
+    return asin(g)/PI*180;
 }
  
 float mpu6000_spi::read_rot(int axis){