Get Euler Angles from GY-955 through Serial connection (with Kalman filter)

I get Euler Angles from GY-955 by Serial protocol. If you use Software serial (Virtual serial port) you can only use baud rate of 9600 to get data from GY-955. But if you use Arduino Mega 2560, you can use Serial1 or Serial2 or Serial3 (Actual Serial ports) and set baud rate of 115200 to get data from GY-955 (fast and exact).

My code for All Arduino boards with Software serial:

#include <SoftwareSerial.h>
static const int RXPin = 69, TXPin = 68; // announce your pins
SoftwareSerial Portone(RXPin, TXPin);
unsigned char Re_buf[30],counter=0;
int16_t DATA[7];
float ROLL,PITCH,YAW;
float Q4[4];  // Quaternions
void setup() {
/*
The default baud rate of Gy 955 is 9600. when you get data from GY 955 by serial protocol, if you connect SR pin to GND pin when sensor is working,
it will stop and its baud rate will change, (if its baud rate was 9600, it will change to 115200 and vice versa),
restart arduiono and set new baud rate for Portone to get data by new baud rate and upload the program on Arduino*/

Serial.begin(115200);  
Portone.begin(9600); 
Portone.write(0XAA); 
Portone.write(0X38);    
Portone.write(0XE2);// for initialization
delay(1000);
}

void loop() {
serialEvent();
Serial.print("RPY: ");
Serial.print( ROLL);
Serial.print(",");
Serial.print( PITCH);
Serial.print(",");
Serial.println( YAW);
Serial.print("Q4: ");
Serial.print( Q4[0]);
Serial.print(",");
Serial.print( Q4[1]);
Serial.print(",");
Serial.print( Q4[2]);
Serial.print(",");
Serial.print( Q4[3]);
Serial.print(";"); 
} // End of void loop

void serialEvent() {
unsigned char i=0,sum=0;
while (Portone.available()) {   
Re_buf[counter]=(unsigned char)Portone.read();
if(counter==0&&Re_buf[0]!=0x5A) return;              
counter++;       
if(counter==20)                //package is complete
{    
   counter=0;                 
   for(i=0;i<19;i++)
      sum+=Re_buf[ i ] ; // for check sum
   if(sum==Re_buf[ i ] )    // if data is valid (Now i is equal to 19)   
  {     
   DATA[0]=(int16_t)(Re_buf[4]<<8|Re_buf[5]);
   DATA[1]=(int16_t)(Re_buf[6]<<8 |Re_buf[7]);
   DATA[2]=(int16_t)(Re_buf[8]<<8 |Re_buf[9]);
   DATA[3]=(int16_t)(Re_buf[10]<<8 |Re_buf[11]);
   DATA[4]=(int16_t)(Re_buf[12]<<8 |Re_buf[13]);
   DATA[5]=(int16_t)(Re_buf[14]<<8 |Re_buf[15]);
   DATA[6]=(int16_t)(Re_buf[16]<<8 |Re_buf[17]);
   YAW= (float)((uint16_t)DATA[0])/100;
   ROLL=(float)DATA[1]/100;
   PITCH=  (float)DATA[2]/100;
   Q4[0]= (float)DATA[3]/10000;
   Q4[1]= (float)DATA[4]/10000;
   Q4[2]= (float)DATA[5]/10000;
   Q4[3]= (float)DATA[6]/10000;
  }      
}
}  // End of while
} // End of serialEvent

My code for Arduino Mega 2560 with Actual Serial ports:

// Connect RX of GY 955 to TX3 Arduino Mega and TX of GY 955 to RX3 Arduino Mega 
unsigned char Re_buf[30],counter=0;
int16_t DATA[7];
float ROLL,PITCH,YAW;
float Q4[4];//Quaternions
void setup() {
/*
when you get data from GY 955 by serial protocol, if you connect SR pin to GND pin when sensor is working, it will stop and its baud rate will change, 
(if its baud rate was 9600, it will change to 115200 and vice versa),restart arduiono and set new baud rate to get data by new baud rate and
upload the program on Arduino*/

Serial.begin(115200);  
Serial3.begin(9600); 
Serial3.write(0XAA); 
Serial3.write(0X38);    
Serial3.write(0XE2);// for initialization
delay(1000);
}

void loop() {
serialEvent();
Serial.print("RPY: ");
Serial.print( ROLL);
Serial.print(",");
Serial.print( PITCH);
Serial.print(",");
Serial.println( YAW);
Serial.print("Q4: ");
Serial.print( Q4[0]);
Serial.print(",");
Serial.print( Q4[1]);
Serial.print(",");
Serial.print( Q4[2]);
Serial.print(",");
Serial.print( Q4[3]);
Serial.print(";"); 
}  // End of void loop
void serialEvent() {
unsigned char i=0,sum=0;
while (Serial3.available()) {   
Re_buf[counter]=(unsigned char)Serial3.read();
if(counter==0&&Re_buf[0]!=0x5A) return;              
counter++;       
if(counter==20)                //package is complete
{    
   counter=0;                 
   for(i=0;i<19;i++)
      sum+=Re_buf[ i ]  ; // for check sum
   if(sum==Re_buf[ i ] )    // if data is valid (Now i is equal to 19)
  {     
   DATA[0]=(int16_t)(Re_buf[4]<<8|Re_buf[5]);
   DATA[1]=(int16_t)(Re_buf[6]<<8|Re_buf[7]);
   DATA[2]=(int16_t)(Re_buf[8]<<8|Re_buf[9]);
   DATA[3]=(int16_t)(Re_buf[10]<<8|Re_buf[11]);
   DATA[4]=(int16_t)(Re_buf[12]<<8|Re_buf[13]);
   DATA[5]=(int16_t)(Re_buf[14]<<8|Re_buf[15]);
   DATA[6]=(int16_t)(Re_buf[16]<<8|Re_buf[17]);
   YAW= (float)((uint16_t)DATA[0])/100;
   ROLL=(float)DATA[1]/100;
   PITCH=  (float)DATA[2]/100;
   Q4[0]= (float)DATA[3]/10000;
   Q4[1]= (float)DATA[4]/10000;
   Q4[2]= (float)DATA[5]/10000;
   Q4[3]= (float)DATA[6]/10000;
  }      
}
}  //End of while
} // End of serialEvent

Note: don't connect SR and S1 pin to anything for Serial connection.

I used GY-955 by this code through Serial connection for my system, but the final result of GY-955 with I2C connection was very better in comparison to GY-955 Serial connection. Here is the code to get Euler angles from GY-955 through I2C connection :

Also I used GY-25 for my system, but the final result of GY-955 through I2C connection was very better in comparison to GY-25. Here is the code to get Euler angles from GY-25 through Serial connection: Run GY 25 in Arduino IDE (with Kalman filter) - Sensors - Arduino Forum

If you have any question, you can ask me in comments or by a pm.