Added 360 resolution

PS3BT.cpp

@@ -24,8 +24,7 @@ const uint8_t PS3BT::BTD_EVENT_PIPE  = 1;
 const uint8_t PS3BT::BTD_DATAIN_PIPE = 2;
 const uint8_t PS3BT::BTD_DATAOUT_PIPE = 3;
 
-prog_char OUTPUT_REPORT_BUFFER[] PROGMEM = 
+prog_char OUTPUT_REPORT_BUFFER[] PROGMEM = {
-{
     0x00, 0x00, 0x00, 0x00, 0x00, 
     0x00, 0x00, 0x00, 0x00, 0x00, 
     0xff, 0x27, 0x10, 0x00, 0x32, 
@@ -37,7 +36,7 @@ prog_char OUTPUT_REPORT_BUFFER[] PROGMEM =
     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 
 };
 
-PS3BT::PS3BT(USB *p, uint8_t btadr5 = 0, uint8_t btadr4 = 0, uint8_t btadr3 = 0, uint8_t btadr2 = 0, uint8_t btadr1 = 0, uint8_t btadr0 = 0):
+PS3BT::PS3BT(USB *p, uint8_t btadr5, uint8_t btadr4, uint8_t btadr3, uint8_t btadr2, uint8_t btadr1, uint8_t btadr0):
 pUsb(p), // pointer to USB class instance - mandatory
 bAddress(0), // device address - mandatory
 bNumEP(1), // if config descriptor needs to be parsed
@@ -516,72 +515,33 @@ uint32_t PS3BT::getSensor(Sensor a)
         return (((l2capinbuf[(uint16_t)a + 1] << 8) | l2capinbuf[(uint16_t)a]) - 0x8000);                
     }
 }
-double PS3BT::getAngle(Angle a, boolean resolution) // Boolean indicate if 360-degrees resolution is used or not - set false if you want to use both axis
+double PS3BT::getAngle(Angle a) {
-{
-    double accXin;
-    double accXval;
-    double angleX;
-    
-    double accYin;
-    double accYval;
-    double angleY;
-    
-    double accZin;
-    double accZval;
-    
     // Data for the Kionix KXPC4 used in DualShock 3
-    double sensivity = 204.6;//0.66/3.3*1023 (660mV/g)
+    const double sensivity = 204.6; // 0.66/3.3*1023 (660mV/g)
-    double zeroG = 511.5;//1.65/3.3*1023 (1,65V)
+    const double zeroG = 511.5; // 1.65/3.3*1023 (1,65V)
-    double R;//force vector
     
-    accXin = getSensor(aX);
+    double accXval = (zeroG - getSensor(aX)) / sensivity; // Convert to g's
-    accXval = (zeroG - accXin) / sensivity;//Convert to g's
     accXval *= 2;    
-    
+    double accYval = (zeroG - getSensor(aY)) / sensivity; // Convert to g's
-    accYin = getSensor(aY);
-    accYval = (zeroG - accYin) / sensivity;//Convert to g's
     accYval *= 2;    
-    
+    double accZval = (zeroG - getSensor(aZ)) / sensivity; // Convert to g's
-    accZin = getSensor(aZ);
-    accZval = (zeroG - accZin) / sensivity;//Convert to g's
     accZval *= 2;
     
-    R = sqrt(pow(accXval, 2) + pow(accYval, 2) + pow(accZval, 2));
+    double R = sqrt(accXval*accXval + accYval*accYval + accZval*accZval);
+    // convert read values to -π/2 to π/2 - Needed for atan2
+    double angleX = acos(accXval/R)-0.5*PI;
+    double angleY = acos(accYval/R)-0.5*PI;
+    double angleZ = acos(accZval/R)-0.5*PI;
     
-    if (a == Pitch)
+    // Convert to 360 degrees resolution
-    {
+    // atan2 outputs the value of -π to π (radians)
-        //the result will come out as radians, so it is multiplied by 180/pi, to convert to degrees
+    // We are then converting it to 0 to 2π and then to degrees    
-        //In the end it is minus by 90, so its 0 degrees when in horizontal postion
+    if (a == Pitch) {        
-        angleX = acos(accXval / R) * 180 / PI - 90;        
+        double angle = (atan2(-angleY,-angleZ)+PI)*RAD_TO_DEG;
-        if(resolution)
+        return angle;
-        {
+    } else {
-            if (accZval < 0)//Convert to 360 degrees resolution - set resolution false if you need both pitch and roll
+        double angle = (atan2(-angleX,-angleZ)+PI)*RAD_TO_DEG;
-            {
+        return angle;
-                if (angleX < 0)                    
-                    angleX = -180 - angleX;                    
-                else                    
-                    angleX = 180 - angleX;                    
-            }
-        }
-        return angleX;
-        
-    }
-    else
-    {
-        //the result will come out as radians, so it is multiplied by 180/pi, to convert to degrees
-        //In the end it is minus by 90, so its 0 degrees when in horizontal postion
-        angleY = acos(accYval / R) * 180 / PI - 90;        
-        if(resolution)
-        {
-            if (accZval < 0)//Convert to 360 degrees resolution - set resolution false if you need both pitch and roll
-            {
-                if (angleY < 0)                   
-                    angleY = -180 - angleY;
-                else
-                    angleY = 180 - angleY;
-            }
-        }
-        return angleY;
     }
 }
 bool PS3BT::getStatus(Status c)

PS3BT.h

@@ -339,7 +339,7 @@ public:
     uint8_t getAnalogButton(AnalogButton a);
     uint8_t getAnalogHat(AnalogHat a);
     uint32_t getSensor(Sensor a);
-    double getAngle(Angle a, boolean resolution);
+    double getAngle(Angle a);
     bool getStatus(Status c);  
     String getStatusString();    
     void disconnect(); // use this void to disconnect any of the controllers

examples/PS3BT/PS3BT.ino

@@ -123,9 +123,9 @@ void loop()
     }
     if(printAngle) {
       Serial.print(F("Pitch: "));               
-      Serial.print(BT.getAngle(Pitch,false));                  
+      Serial.print(BT.getAngle(Pitch));                  
       Serial.print(F("\tRoll: ")); 
-      Serial.println(BT.getAngle(Roll,false));
+      Serial.println(BT.getAngle(Roll));
     }
   }
   else if(BT.PS3MoveBTConnected)