Calculate Pitch and Roll Using Hexabitz Accelerometer

import processing.serial.*;
import processing.opengl.*;
import toxi.geom.*;
import toxi.processing.*; 

Serial myPort;  // Create object from Serial class
String portName = "COM35"; //change this
String output;

float [] acceleration = new float [3]; //AccX,AccY,AccZ
float [] YaPiRo = new float [3]; //Yaw/Pitch/Roll angles

public void settings() {
  size(300, 300, P3D);
}

void setup()
  {
  myPort = new Serial(this, portName, 921600);
  myPort.write("\r");
  delay(20);
  myPort.clear();
  sendMSG("stream acc 200 5000000");
  delay(20);
  myPort.clear();
  }

void draw()
  {
    getCordinate();
    acc2PiRo();
    
    background(0);
    // translate everything to the middle of the viewport
    pushMatrix();
    translate(width / 2, height / 2);
    //rotateY(YaPiRo[0]);
    rotateZ(YaPiRo[2]);
    rotateX(YaPiRo[1]);
    fill(255, 0, 0, 200);
    box(10, 10, 200);
    
    // draw front-facing tip in blue
    fill(0, 0, 255, 200);
    pushMatrix();
    translate(0, 0, -120);
    rotateX(PI/2);
    drawCylinder(0, 20, 20, 8);
    popMatrix();
    
    // draw wings and tail fin in green
    fill(0, 255, 0, 200);
    beginShape(TRIANGLES);
    vertex(-100,  2, 30); vertex(0,  2, -80); vertex(100,  2, 30);  // wing top layer
    vertex(-100, -2, 30); vertex(0, -2, -80); vertex(100, -2, 30);  // wing bottom layer
    vertex(-2, 0, 98); vertex(-2, -30, 98); vertex(-2, 0, 70);  // tail left layer
    vertex( 2, 0, 98); vertex( 2, -30, 98); vertex( 2, 0, 70);  // tail right layer
    endShape();
    beginShape(QUADS);
    vertex(-100, 2, 30); vertex(-100, -2, 30); vertex(  0, -2, -80); vertex(  0, 2, -80);
    vertex( 100, 2, 30); vertex( 100, -2, 30); vertex(  0, -2, -80); vertex(  0, 2, -80);
    vertex(-100, 2, 30); vertex(-100, -2, 30); vertex(100, -2,  30); vertex(100, 2,  30);
    vertex(-2,   0, 98); vertex(2,   0, 98); vertex(2, -30, 98); vertex(-2, -30, 98);
    vertex(-2,   0, 98); vertex(2,   0, 98); vertex(2,   0, 70); vertex(-2,   0, 70);
    vertex(-2, -30, 98); vertex(2, -30, 98); vertex(2,   0, 70); vertex(-2,   0, 70);
    endShape();
    
    popMatrix();
    
        
      
  }
void sendMSG(String msg){
  int strLenght = msg.length();
  for (int i = 0; i<strLenght; i++)
  {
    myPort.write(msg.charAt(i));
    delay(20);
  }
  myPort.write("\r");
}

void getCordinate()
{
      if (myPort.available()>1)
    {
      output = myPort.readStringUntil(10);
    }
    if(output != null)
    {
      if(output.charAt(0) == 'A'){
      float sign = 1.00;
      int Index = output.indexOf('X');
      if(Index!=-1){
      if(output.charAt(Index+3) == '-') {sign = -1; Index++;} else {sign = 1;}
      String temp = output.substring(Index+3,Index+7);
      acceleration[0] = sign*float(temp);
      Index = output.indexOf('Y');
      if(output.charAt(Index+3) == '-') {sign = -1; Index++;} else {sign = 1;}
      temp = output.substring(Index+3,Index+7);
      acceleration[1] = sign*float(temp);
      Index = output.indexOf('Z');
      if(output.charAt(Index+3) == '-') {sign = -1; Index++;} else {sign = 1;}
      temp = output.substring(Index+3,Index+7);
      acceleration[2] = sign*float(temp);
    print(output);
  }}
}
}

void acc2PiRo()
{
    YaPiRo[1] = atan2(acceleration[1] , acceleration[2]);
    YaPiRo[2] = atan2(-acceleration[0] , acceleration[2]);
}
void drawCylinder(float topRadius, float bottomRadius, float tall, int sides) {
    float angle = 0;
    float angleIncrement = TWO_PI / sides;
    beginShape(QUAD_STRIP);
    for (int i = 0; i < sides + 1; ++i) {
        vertex(topRadius*cos(angle), 0, topRadius*sin(angle));
        vertex(bottomRadius*cos(angle), tall, bottomRadius*sin(angle));
        angle += angleIncrement;
    }
    endShape();
    
    // If it is not a cone, draw the circular top cap
    if (topRadius != 0) {
        angle = 0;
        beginShape(TRIANGLE_FAN);
        
        // Center point
        vertex(0, 0, 0);
        for (int i = 0; i < sides + 1; i++) {
            vertex(topRadius * cos(angle), 0, topRadius * sin(angle));
            angle += angleIncrement;
        }
        endShape();
    }
  
    // If it is not a cone, draw the circular bottom cap
    if (bottomRadius != 0) {
        angle = 0;
        beginShape(TRIANGLE_FAN);
    
        // Center point
        vertex(0, tall, 0);
        for (int i = 0; i < sides + 1; i++) {
            vertex(bottomRadius * cos(angle), tall, bottomRadius * sin(angle));
            angle += angleIncrement;
        }
        endShape();
    }
}