Arduino ESP32 BNO085 UART-RVC mode Serial recieving Bin2int

uart2drct_rvc_Bt__CPLT_02.ino

/* Test sketch for Adafruit BNO08x sensor in UART-RVC mode */

//#include "Adafruit_BNO08x_RVC.h"

//Adafruit_BNO08x_RVC rvc = Adafruit_BNO08x_RVC();

#include "BluetoothSerial.h"
#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
#endif

#if !defined(CONFIG_BT_SPP_ENABLED)
#error Serial Bluetooth not available or not enabled. It is only available for the ESP32 chip.
#endif

BluetoothSerial SerialBT;
uint64_t chipid;
char chipname[256];
static byte c[2];
char b0,b1,b2;
int i;
//===============================================
void setup() {
  // Wait for serial monitor to open
  Serial.begin(115200);
  while (!Serial)
    delay(10);

 // Serial.println("Adafruit BNO08x IMU - UART-RVC mode");

  Serial2.begin(115200,SERIAL_8N1,16,17); // RX16,TX17 This is the baud rate specified by the datasheet
  while (!Serial2)
    delay(10);
/*
  if (!rvc.begin(&Serial1)) { // connect to the sensor over hardware serial
    Serial.println("Could not find BNO08x!");
    while (1)
      delay(10);
  }

  Serial.println("BNO08x found!");
  */
//Bluetooth setup
  delay(1000);
  chipid = ESP.getEfuseMac();
  sprintf( chipname, "ESP32D_%04X", (uint16_t)(chipid >> 32));
  SerialBT.begin(chipname);
  Serial.print("BT chipname:");
  Serial.println(chipname);
}
int  bin2int(byte c0,byte c1){
   int yawi=c0+c1*256;
      if(c1>=128){
        yawi=yawi-65535;
      }
      return yawi;
}


void loop() {
  //BNO08x_RVC_Data heading;

 // if (!rvc.read(&heading)) {
 //   return;
 // }
 //delay(20);
 if(Serial2.available())
 {
   b0=Serial2.read();
   b1=Serial2.read();
   b2=Serial2.read();
   if(b0==0xaa && b1==0xaa){
     for(i=0;i<11;i++){
       c[i]=Serial2.read();
       //Serial.print(c[i],HEX);
       //Serial.print(",");
     }
     // c[0]=Serial2.read();
     // c[1]=Serial2.read();
      int yawi=bin2int(c[0],c[1]);
      int pitchi=bin2int(c[2],c[3]);
      int rolli=bin2int(c[4],c[5]);
      double yawf=(double)yawi*0.01;
      double pitchf=(double)pitchi*0.01;
      double rollf=(double)rolli*0.01;
      //Serial.println();
      //Serial.printf("c[0]=%d,c[1]=%d,yawi=%d,yawf=%4.2f\n\r",c[0],c[1],yawi,yawf);
      Serial.printf("%4.2f,%4.2f,%4.2f\n\r",yawf,pitchf,rollf);
      SerialBT.printf("%4.2f,%4.2f,%4.2f\n\r",yawf,pitchf,rollf);
   }
   }
 }