BNO055 ESP32 Rotation Test Gyro Integral Cal

BNGyroSekibun00_read_all_data_BT.ino

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include "BluetoothSerial.h"
/* This driver uses the Adafruit unified sensor library (Adafruit_Sensor),
   which provides a common 'type' for sensor data and some helper functions.

   To use this driver you will also need to download the Adafruit_Sensor
   library and include it in your libraries folder.

   You should also assign a unique ID to this sensor for use with
   the Adafruit Sensor API so that you can identify this particular
   sensor in any data logs, etc.  To assign a unique ID, simply
   provide an appropriate value in the constructor below (12345
   is used by default in this example).

   Connections
   ===========
   Connect SCL to analog 5
   Connect SDA to analog 4
   Connect VDD to 3.3-5V DC
   Connect GROUND to common ground

   History
   =======
   2015/MAR/03  - First release (KTOWN)
*/

/* Set the delay between fresh samples */
uint16_t BNO055_SAMPLERATE_DELAY_MS = 100;

// Check I2C device address and correct line below (by default address is 0x29 or 0x28)
//                                   id, address
Adafruit_BNO055 bno = Adafruit_BNO055(55, 0x28, &Wire);
#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];
//
const double  degrad =57.295779 ;
static int t,t_1,t_2;
volatile int tmag,magflag,tg,tg_1,tl,tl_1;
volatile double gxsum,gysum,gzsum,gx,gx_1,gy,gy_1,gz,gz_1;
volatile double lx,lx_1,ly,ly_1,lz,lz_1;
volatile double sxsum,sysum,szsum,sx,sx_1,sy,sy_1,sz,sz_1,d;
volatile double  grx,gry,grz,tgr;
void setup(void)
{
  Serial.begin(115200);
 pinMode(33, INPUT_PULLUP);
  attachInterrupt(33, magin, FALLING);
  while (!Serial) delay(10);  // wait for serial port to open!

  Serial.println("Orientation Sensor Test"); Serial.println("");

  /* Initialise the sensor */
  if (!bno.begin())
  {
    /* There was a problem detecting the BNO055 ... check your connections */
    Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
    while (1);
  }

  delay(1000);
  chipid = ESP.getEfuseMac();
  sprintf( chipname, "ESP32D_%04X", (uint16_t)(chipid >> 32));
  SerialBT.begin(chipname);
  SerialBT.print("BT chipname:");
  SerialBT.println(chipname);

  gxsum=0;
  gysum=0;
  gzsum=0;
}
//MAG PIN intgerrupt
void magin()
{
   tmag=millis();
   magflag=1;
   gxsum=0;
   gysum=0;
   gzsum=0;

   sxsum=0;
   sysum=0;
   szsum=0;
}

void loop(void)
{
  t=millis();
  sensors_event_t orientationData , angVelocityData , linearAccelData, magnetometerData, accelerometerData, gravityData;
  bno.getEvent(&angVelocityData, Adafruit_BNO055::VECTOR_GYROSCOPE);
  //bno.getEvent(&linearAccelData, Adafruit_BNO055::VECTOR_LINEARACCEL);
  bno.getEvent(&gravityData, Adafruit_BNO055::VECTOR_GRAVITY);
 // SerialBT.printf("MAG=%d\n\r",digitalRead(33));
 if(t-t_1>20)//50Hz
 {
  t_1=t;
  printEvent(&angVelocityData);
  //printEvent(&linearAccelData);
  printEvent(&gravityData);

    
 SerialBT.printf("[GYR],%4.4f,%4.4f,%4.4f,gsum=,%4.4f,%4.4f,%4.4f,%d\n\r",gx,gy,gz,gxsum*degrad,gysum*degrad,gzsum*degrad,tg);
 SerialBT.printf("[GRV],%4.4f,%4.4f,%4.4f,%d\n\r",grx,gry,grz,tgr);
 }
 if(magflag==1)
 {
    SerialBT.printf("[MAG]=,%d\n\r",tmag);
    magflag=0;
 }
 
}

void printEvent(sensors_event_t* event) {
  double x = -1000000, y = -1000000 , z = -1000000; //dumb values, easy to spot problem
  if (event->type == SENSOR_TYPE_ACCELEROMETER) {
    //SerialBT.print("Accl:");
    x = event->acceleration.x;
    y = event->acceleration.y;
    z = event->acceleration.z;
    //
    SerialBT.printf("[BNO0],aX=,%4.4f,aY=,%4.4f,aZ=,%4.4f,%d\n\r",x,y,z,millis());
  }
  else if (event->type == SENSOR_TYPE_ORIENTATION) {
    //SerialBT.print("Orient:");
    x = event->orientation.x;
    y = event->orientation.y;
    z = event->orientation.z;
    //
    SerialBT.printf("[BNO1],oX=,%4.4f,oY=,%4.4f,oZ=,%4.4f,%d\n\r",x,y,z,millis());
  }
  else if (event->type == SENSOR_TYPE_MAGNETIC_FIELD) {
    //SerialBT.print("Mag:");
    x = event->magnetic.x;
    y = event->magnetic.y;
    z = event->magnetic.z;
      //
    SerialBT.printf("mX=,%4.4f,mY=,%4.4f,mZ=,%4.4f,%d\n\r",x,y,z,millis());
  }
  else if (event->type == SENSOR_TYPE_GYROSCOPE) {
    //SerialBT.print("Gyro:");
   
    gx_1=gx;
    gy_1=gy;
    gz_1=gz;
    tg_1=tg;
    tg=millis();
    gx = event->gyro.x;
    gy = event->gyro.y;
    gz = event->gyro.z;
    gxsum=gxsum+(gx+gx_1)/2*(tg-tg_1)/1000;
    gysum=gysum+(gy+gy_1)/2*(tg-tg_1)/1000;
    gzsum=gzsum+(gz+gz_1)/2*(tg-tg_1)/1000;
    
      //
    //SerialBT.printf("g=,%4.4f,,%4.4f,,%4.4f,gsum=,%4.4f,%4.4f,%4.4f,%d\n\r",gx,gy,gz,gxsum*degrad,gysum*degrad,gzsum*degrad,tg);
  }
  else if (event->type == SENSOR_TYPE_ROTATION_VECTOR) {
   // SerialBT.print("Rot:");
    x = event->gyro.x;
    y = event->gyro.y;
    z = event->gyro.z;
     //
     
  }
  else if (event->type == SENSOR_TYPE_LINEAR_ACCELERATION) {
   // SerialBT.print("Linear:");
    lx_1=lx;
    ly_1=ly;
    lz_1=lz;
    tl_1=tl;
    tl=millis();
    //acc
    lx = event->acceleration.x;
    ly = event->acceleration.y;
    lz = event->acceleration.z;
    // 
    //加速度m/s2を積分して平均速度m/s sx,sy,sz
    sx=(lx+lx_1)/2*(tl-tl_1)/1000;
    sy=(ly+ly_1)/2*(tl-tl_1)/1000;
    sz=(lz+lz_1)/2*(tl-tl_1)/1000;
    //distance m
    sxsum=sxsum+sx*(tl-tl_1)/1000;
    sysum=sysum+sy*(tl-tl_1)/1000;
    szsum=szsum+sz*(tl-tl_1)/1000;
    d=sqrt(sxsum*sxsum+sysum*sysum+szsum*szsum);
    //SerialBT.printf("l=,%4.4f,%4.4f,%4.4f,s=,%4.4f,%4.4f,%4.4f,d=,%4.4f,%d\n\r",lx,ly,lz,sx,sy,sz,d,tl);
  }
  else if (event->type == SENSOR_TYPE_GRAVITY) {
   // SerialBT.print("Gravity:");
    tgr=millis();
    grx = event->acceleration.x;
    gry = event->acceleration.y;
    grz = event->acceleration.z;
     //
    //SerialBT.printf("grX=,%4.4f,grY=,%4.4f,grZ=,%4.4f,%d\n\r",x,y,z,millis());
  }
  else {
    SerialBT.print("Unk:");
  }
  /*
  SerialBT.print("\tx= ");
  SerialBT.print(x);
  SerialBT.print(" |\ty= ");
  SerialBT.print(y);
  SerialBT.print(" |\tz= ");
  SerialBT.println(z);
  */
}