From 053786b12510056dd9417211cd92fe63e51604d2 Mon Sep 17 00:00:00 2001
From: Kristian Lauszus <lauszus@gmail.com>
Date: Fri, 1 Jun 2012 17:27:21 +0200
Subject: [PATCH] Updated getAngle() function

---
 PS3BT.cpp | 21 ++++-----------------
 PS3BT.h   |  2 +-
 2 files changed, 5 insertions(+), 18 deletions(-)

diff --git a/PS3BT.cpp b/PS3BT.cpp
index c369e3c9..a694eec4 100644
--- a/PS3BT.cpp
+++ b/PS3BT.cpp
@@ -465,7 +465,7 @@ uint8_t PS3BT::getAnalogHat(AnalogHat a)
         return 0;                        
     return (uint8_t)(l2capinbuf[(uint16_t)a]);            
 }
-int32_t PS3BT::getSensor(Sensor a)
+int16_t PS3BT::getSensor(Sensor a)
 {
     if (l2capinbuf == NULL)
         return 0;
@@ -505,22 +505,9 @@ double PS3BT::getAngle(Angle a) {
     } else if(PS3MoveConnected) {        
         // It's a Kionix KXSC4 inside the Motion controller
         const uint16_t zeroG = 0x8000;                
-        accXval = getSensor(aXmove);
-        accYval = getSensor(aYmove);
-        accZval = getSensor(aZmove);
-        
-        if(accXval < 0)
-            accXval += zeroG;
-        else
-            accXval -= zeroG;        
-        if(accYval < 0)
-            accYval += zeroG;
-        else
-            accYval -= zeroG;        
-        if(accZval < 0)
-            accZval += zeroG;
-        else
-            accZval -= zeroG;
+        accXval = -(int16_t)(getSensor(aXmove)-zeroG);
+        accYval = (int16_t)(getSensor(aYmove)-zeroG);
+        accZval = (int16_t)(getSensor(aZmove)-zeroG);      
     }
     
     // Convert to 360 degrees resolution
diff --git a/PS3BT.h b/PS3BT.h
index 3d071dfc..618747ec 100644
--- a/PS3BT.h
+++ b/PS3BT.h
@@ -349,7 +349,7 @@ public:
     bool getButton(Button b);
     uint8_t getAnalogButton(AnalogButton a);
     uint8_t getAnalogHat(AnalogHat a);
-    int32_t getSensor(Sensor a);
+    int16_t getSensor(Sensor a);
     double getAngle(Angle a);
     bool getStatus(Status c);  
     String getStatusString();