ENH: Updated arduino sketch parameters

Arduino/imstkVRPNArduino_roll_only/imstkVRPNArduino_roll_only.ino

@@ -127,6 +127,7 @@ void setup() {
     // (115200 chosen because it is required for Teapot Demo output, but it's
     // really up to you depending on your project)
     Serial.begin(57600);
+    Serial.println("setup");
     while (!Serial); // wait for Leonardo enumeration, others continue immediately
 
     // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio
@@ -249,11 +250,12 @@ void loop() {
             accel[0] = aa.x;
             accel[1] = aa.y;
             accel[2] = aa.z;
-            ComplementaryFilter(accel, gyro, &m_pitch, &m_roll);
+            ComplementaryFilter(accel, gyro);
             Serial.print(m_roll);
-
+            Serial.print(",");
+            Serial.print(m_pitch);
             //dummy data to fill out 6 vrpn analog channels
-            Serial.println(",0,0,0,0,0");
+            Serial.println(",0,0,0,0");
             
         #endif
 
@@ -263,25 +265,25 @@ void loop() {
     }
 }
 
-void ComplementaryFilter(short accData[3], short gyrData[3], float *pitch, float *roll)
+void ComplementaryFilter(short accData[3], short gyrData[3])
 {
     float pitchAcc, rollAcc;               
  
     // Integrate the gyroscope data -> int(angularSpeed) = angle
-    *pitch += ((float)gyrData[0] / GYROSCOPE_SENSITIVITY) * dt; // Angle around the X-axis
-    *roll -= ((float)gyrData[1] / GYROSCOPE_SENSITIVITY) * dt;    // Angle around the Y-axis
+    m_pitch += ((float)gyrData[0] / GYROSCOPE_SENSITIVITY) * dt; // Angle around the X-axis
+    m_roll -= ((float)gyrData[1] / GYROSCOPE_SENSITIVITY) * dt;    // Angle around the Y-axis
  
     // Compensate for drift with accelerometer data if !bullshit
     // Sensitivity = -2 to 2 G at 16Bit -> 2G = 32768 && 0.5G = 8192
     int forceMagnitudeApprox = abs(accData[0]) + abs(accData[1]) + abs(accData[2]);
-    if (forceMagnitudeApprox > 8192 && forceMagnitudeApprox < 32768)
+    if (forceMagnitudeApprox > ACCELEROMETER_SENSITIVITY && forceMagnitudeApprox < 32768)
     {
   // Turning around the X axis results in a vector on the Y-axis
         pitchAcc = atan2f((float)accData[1], (float)accData[2]) * 180 / M_PI;
-        *pitch = *pitch * 0.5 + pitchAcc * 0.5;
+        m_pitch = m_pitch * 0.98 + pitchAcc * 0.02;
  
   // Turning around the Y axis results in a vector on the X-axis
         rollAcc = atan2f((float)accData[0], (float)accData[2]) * 180 / M_PI;
-        *roll = *roll * 0.5 + rollAcc * 0.5;
+        m_roll = m_roll * 0.95 + rollAcc * 0.05;
     }
 }