Skip to content

Instantly share code, notes, and snippets.

@dj1711572002

dj1711572002/BNO085vs055_ESP32_Kaiten_RVC_NDOF_BT_rev33.ino Secret

Created November 23, 2023 03:40
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save dj1711572002/ecd6f1c11f9b52f38688d08af5a4a437 to your computer and use it in GitHub Desktop.
Save dj1711572002/ecd6f1c11f9b52f38688d08af5a4a437 to your computer and use it in GitHub Desktop.
Download ZIP
ESP32 Arduino BNO055 vs BNO085 Rotation Accuracy comparison　
Raw
BNO085vs055_ESP32_Kaiten_RVC_NDOF_BT_rev33.ino
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include "BluetoothSerial.h"
#include "Adafruit_BNO08x_RVC.h"
Adafruit_BNO08x_RVC rvc = Adafruit_BNO08x_RVC();
/* 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)
*/
#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];
int t,t0_1,t1_1,n;
/* Set the delay between fresh samples */
uint16_t BNO055_SAMPLERATE_DELAY_MS = 10;
volatile static long btime[150]={};//BNO Sampling itow
volatile int no;
volatile int no_1;
static int tpr,tpr_1;
static float uX[150]={};//yaw
static float uY[150]={};//pitch
static float uZ[150]={};//roll
static float eX[150]={};//yaw
static float eY[150]={};//pitch
static float eZ[150]={};//roll
static double roll,pitch,yaw;
static float qW[150]={};
static float qX[150]={};
static float qY[150]={};
static float qZ[150]={};
//acc
static float acX[150]={};
static float acY[150]={};
static float acZ[150]={};
//GYRO
volatile int tmag,magflag,tg,tg_1,tl,tl_1,tgread,gycount;//gycountはtimepulseでinterrupttimerと同時にリセット
volatile double gxsum,gysum,gzsum,gx,gx_1,gy,gy_1,gz,gz_1;
volatile double gyX[150]={};//3000msec分
volatile double gyY[150]={};
volatile double gyZ[150]={};
volatile double gsX[150];
volatile double gsY[150];
volatile double gsZ[150];
//Linear Acc
static double laX[150]={};
static double laY[150]={};
static double laZ[150]={};
static double lvX[150]={};//速度積分値
static double lvY[150]={};
static double lvZ[150]={};
static double ldX[150]={};//距離要素積分値
static double ldY[150]={};
static double ldZ[150]={};
static double ldsX[150]={};//距離累積積分値
static double ldsY[150]={};
static double ldsZ[150]={};
//Gravity
static double grX[150]={};
static double grY[150]={};
static double grZ[150]={};
static double sX[150]={};
static double sY[150]={};
static double sZ[150]={};
static double sroll;
// 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);
//MAG PIN intgerrupt
void magin()
{
tmag=millis();
magflag=1;
gxsum=0;
gysum=0;
gzsum=0;
}
//---------------------------------------------------------------------------------------
void bnoreadGYRO(){
int no=gycount%150;//BNOのタイミングは、最初のtimepulseでスタートして3秒毎にカウンタがリセットされる 120msecとtimepulseが一致する周期3秒
int no_1=(gycount-1)%150;//
sensors_event_t orientationData , angVelocityData , linearAccelData, magnetometerData, accelerometerData, gravityData;//event構造体定義
//-------------------get Gyro---------------------------------
bno.getEvent(&angVelocityData, Adafruit_BNO055::VECTOR_GYROSCOPE);
//BNO時間をitowに変換
btime[no]=millis();
gyX[no]=angVelocityData.gyro.x;
gyY[no]=angVelocityData.gyro.y;
gyZ[no]=angVelocityData.gyro.z;
gxsum=gxsum+(gyX[no]+gyX[no_1])/2*(double)(btime[no]-btime[no_1])/1000;//(btime[no]-btime[no_1)/1000;
gysum=gysum+(gyY[no]+gyY[no_1])/2*(double)(btime[no]-btime[no_1])/1000;//
gzsum=gzsum+(gyZ[no]+gyZ[no_1])/2*(double)(btime[no]-btime[no_1])/1000;//
int gxi=(int)(gxsum*57.29577);
double gxd=gxsum*57.29577-gxi;
gsX[no]=(double)(gxi%360)+gxd;
int gyi=(int)(gysum*57.29577);
double gyd=gysum*57.29577-gyi;
gsY[no]=(double)(gyi%360)+gyd;
int gzi=(int)(gzsum*57.29577);
double gzd=gzsum*57.29577-gzi;
gsZ[no]=(double)(gzi%360)+gzd;
tgread=millis();
gycount++;
//-------------get Acc--------------------------------
bno.getEvent(&accelerometerData, Adafruit_BNO055::VECTOR_ACCELEROMETER);
acX[no] =accelerometerData.acceleration.x;
acY[no] =accelerometerData.acceleration.y;
acZ[no] =accelerometerData.acceleration.z;
//printEvent(&angVelocityData,no);
//--------------get Linear Acc----------------------
bno.getEvent(&linearAccelData, Adafruit_BNO055::VECTOR_LINEARACCEL);
//printEvent(&linearAccelData,no);
laX[no] = linearAccelData.acceleration.x;
laY[no] = linearAccelData.acceleration.y;
laZ[no] = linearAccelData.acceleration.z;
tpr_1=tpr;
tpr=millis();
//
lvX[no]=(laX[no]+laX[no_1])/2*(tpr-tpr_1);//mm/sec
lvY[no]=(laY[no]+laY[no_1])/2*(tpr-tpr_1);
lvZ[no]=(laZ[no]+laZ[no_1])/2*(tpr-tpr_1);
//
ldX[no]=(lvX[no]+lvX[no_1])/2*(tpr-tpr_1)/1000;
ldY[no]=(lvY[no]+lvY[no_1])/2*(tpr-tpr_1)/1000;
ldZ[no]=(lvZ[no]+lvZ[no_1])/2*(tpr-tpr_1)/1000;
//
ldsX[no]=ldsX[no_1]+(lvX[no]+lvX[no_1])/2*(tpr-tpr_1)/1000;
ldsY[no]=ldsY[no_1]+(lvY[no]+lvY[no_1])/2*(tpr-tpr_1)/1000;
ldsZ[no]=ldsZ[no_1]+(lvZ[no]+lvZ[no_1])/2*(tpr-tpr_1)/1000;
//------------get Gravity------------------------
bno.getEvent(&gravityData, Adafruit_BNO055::VECTOR_GRAVITY);
grX[no] = gravityData.acceleration.x;
grY[no] = gravityData.acceleration.y;
grZ[no] = gravityData.acceleration.z;
//-----GravityからeY[no]=pitch eZ[no]=roll 絶対座標変換-------
//https://www.analog.com/media/jp/technical-documentation/app-notes/an-1057_jp.pdf
//
sX[no]=(atan(grX[no]/sqrt(grY[no]*grY[no]+grZ[no]*grZ[no])))*57.29577;//
sY[no]=(atan(grY[no]/sqrt(grX[no]*grX[no]+grZ[no]*grZ[no])))*57.29577;//
sZ[no]=(atan(sqrt(grX[no]*grX[no]+grY[no]*grY[no])/grZ[no]))*57.29577;//x roll
//sXはpitch rollは　atan(grZ/grY) https://watako-lab.com/2019/02/15/3axis_acc/
sroll=atan(grY[no]/grZ[no])*57.29577;
/*
double srolldbl=(atan2(grZ[no],grY[no])*57.29577*10000);
int srollint=(int)srolldbl;
Serial.printf("sroldbl,int=%4.4f,%d\n\r",srolldbl,srollint);
sroll=(double)(srollint%900000)/10000;
*/
//-----------Orienttation からpitch roll
bno.getEvent(&orientationData, Adafruit_BNO055::VECTOR_EULER);
eX[no] = orientationData.orientation.x;
eY[no] = orientationData.orientation.y;
eZ[no] = orientationData.orientation.z;
//Quartenion Copy from https://nobita-rx7.hatenablog.com/entry/27642606
imu::Quaternion quat = bno.getQuat();
qW[no]=(double)quat.w();
qX[no]=(double)quat.x();
qY[no]=(double)quat.y();
qZ[no]=(double)quat.z();
//Quarternion 　Euler変換
double w=qW[no];
double x=qX[no];
double y=qY[no];
double z=qZ[no];
double ysqr = y * y;
// roll (x-axis rotation)
double t0 = +2.0 * (w * x + y * z);
double t1 = +1.0 - 2.0 * (x * x + ysqr);
roll = atan2(t0, t1);
// pitch (y-axis rotation)
double t2 = +2.0 * (w * y - z * x);
t2 = t2 > 1.0 ? 1.0 : t2;
//if(t2>1){t2=1;}else{t2=t2;}
t2 = t2 < -1.0 ? -1.0 : t2;
//if(t2<-1.0){t2=-1.0}else{t2=t2)
pitch = asin(t2);
// yaw (z-axis rotation)
double t3 = +2.0 * (w * z + x * y);
double t4 = +1.0 - 2.0 * (ysqr + z * z);
//rad->deg
yaw = atan2(t3, t4);
roll *= 57.2957795131;
pitch *= 57.2957795131;
yaw *= 57.2957795131;
if(yaw<0){//yaw 0-> -180deg=
yaw=360+yaw;;
}
yaw=360-yaw;
//------------------------------
//btime[no]=millis();
//eX[no]=yaw;
//eX[no]=millis();//DEBUG yaw;
//eY[no]=pitch;
//eZ[no]=roll;
//Kaiten NDOF SVM
//Serial.printf("ms=%d,gyX=%4.3f,gyY=%4.3f,gyZ=%4.3f, acX=%4.3f,acY=%4.3f,acZ=%4.3f,,grX=%4.3f,grY=%4.3f,grZ=%4.3f,eX=%4.3f,eY=[,%4.3f,],eZ=[,%4.3f,],qW=,%4.3f,qX=%4.3f,qY=%4.3f,qZ=%4.3f\n\r",millis(),gyX[no],gyY[no],gyZ[no],acX[no],acY[no],acZ[no],grX[no],grY[no],grZ[no],eX[no],eY[no],eZ[no],qW[no],qX[no],qY[no],qZ[no]);
// Serial.printf("%d,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f\n\r",millis(),gyX[no],gyY[no],gyZ[no],acX[no],acY[no],acZ[no],grX[no],grY[no],grZ[no],eX[no],eY[no],eZ[no],qW[no],qX[no],qY[no],qZ[no]);
//SerialBT.printf("%d,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f\n\r",millis(),gyX[no],gyY[no],gyZ[no],acX[no],acY[no],acZ[no],grX[no],grY[no],grZ[no],eX[no],eY[no],eZ[no],qW[no],qX[no],qY[no],qZ[no]);
//CPLT観察用
//SerialBT.printf("%d,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f\n\r",millis(),gyX[no],gyY[no],gyZ[no],eX[no],eY[no],eZ[no]);
//hako観察用
Serial.printf("bnoread:%d,%4.3f,%4.3f,%4.3f\n\r",millis(),yaw,pitch,roll);
// SerialBT.printf("ms=%d,roll=%4.1f,pitch=%4.1f,yaw=%4.1f,eX=%4.1f,eY=%4.1f,eZ=%4.1f,grx=%4.3f,gry=%4.3f,grz=%4.3f,qW=%4.3f,qX=%4.3f,qY=%4.3f,qZ=%4.3f\n\r",millis(),roll,pitch,yaw,eX[no],eY[no],eZ[no],grX[no],grY[no],grZ[no],qW[no],qX[no],qY[no],qZ[no]);
//SerialBT.printf("Gyro gsX=%4.1f,gsY=%4.1f,gsZ=%4.1f,Gravity pitch_eY=%4.1f,roll_eZ=%4.1f,grX=%4.1f,grY=%4.1f,grZ=%4.1f\n\r",gsX[no],gsY[no],gsZ[no],eY[no],eZ[no],grX[no],grY[no],grZ[no]);
//Serial.printf("Gyro gsX=%4.1f,gsY=%4.1f,gsZ=%4.1f,eX=%4.1f,eY=%4.1f,eZ=%4.1f,grX=%4.1f,grY=%4.1f,grZ=%4.1f\n\r",gsX[no],gsY[no],gsZ[no],eX[no],eY[no],eZ[no],grX[no],grY[no],grZ[no]);
//Serial.printf("Gyro gsX=%4.1f,gsY=%4.1f,gsZ=%4.1f,sX=%4.1f,sY=%4.1f,sZ=%4.1f,grX=%4.1f,grY=%4.1f,grZ=%4.1f,eX=%4.1f,eY=%4.1f,eZ=%4.1f\n\r",gsX[no],gsY[no],gsZ[no],sX[no],sY[no],sZ[no],grX[no],grY[no],grZ[no],eX[no],eY[no],eZ[no]);
// Serial.printf("ms=%d,gyX=%4.3f,gyY=%4.3f,gyZ=%4.3f, gsX=%4.3f,gsY=%4.3f,gsZ=%4.3f,,grX=%4.3f,grY=%4.3f,grZ=%4.3f,eX=%4.3f,eY=[,%4.3f,],eZ=[,%4.3f,],qW=,%4.3f,qX=%4.3f,qY=%4.3f,qZ=%4.3f\n\r",millis(),gyX[no],gyY[no],gyZ[no],gsX[no],gsY[no],gsZ[no],grX[no],grY[no],grZ[no],eX[no],eY[no],eZ[no],qW[no],qX[no],qY[no],qZ[no]);
// SerialBT.printf("ms=%d,roll=%4.1f,pitch=%4.1f,yaw=%4.1f,eX=%4.1f,eY=%4.1f,eZ=%4.1f,grx=%4.3f,gry=%4.3f,grz=%4.3f,qW=%4.3f,qX=%4.3f,qY=%4.3f,qZ=%4.3f\n\r",millis(),roll,pitch,yaw,eX[no],eY[no],eZ[no],grX[no],grY[no],grZ[no],qW[no],qX[no],qY[no],qZ[no]);
//Acc
// SerialBT.printf("%d,%4.3f,%4.3f,%4.3f,%d\n\r",gycount,acX[no],acY[no],acZ[no],millis());
// Serial.printf("%d,%4.3f,%4.3f,%4.3f,%d\n\r",gycount,acX[no],acY[no],acZ[no],millis());
//gyro
//SerialBT.printf("%d,%4.3f,%4.3f,%4.3f,\n\r",gycount,gyX[no],gyY[no],gyZ[no]);
//Serial.printf("%d,%4.3f,%4.3f,%4.3f,\n\r",gycount,gyX[no],gyY[no],gyZ[no]);
//linear acc
// SerialBT.printf("%d,%4.3f,%4.3f,%4.3f,%d\n\r",gycount,laX[no],laY[no],laZ[no],tpr);
//Serial.printf("%d,%4.3f,%4.3f,%4.3f,%d\n\r",gycount,laX[no],laY[no],laZ[no],tpr);
// linear Acc+gyro
//SerialBT.printf("%d,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%d\n\r",gycount,laX[no],laY[no],laZ[no],gyX[no],gyY[no],gyZ[no],tpr);
//Serial.printf("%d,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%d\n\r",gycount,laX[no],laY[no],laZ[no],gyX[no],gyY[no],gyZ[no],tpr);
//速度Vectorを出力
//double vx=((laX[no]+laX[no_1])/2)/(tpr-tpr_1);//mm/sec
// double vy=((laY[no]+laY[no_1])/2)/(tpr-tpr_1);
// double vz=((laZ[no]+laZ[no_1])/2)/(tpr-tpr_1);
// SerialBT.printf("%d,%4.3f,%4.3f,%4.3f,%d\n\r",gycount,vx,vy,vz,tpr-tpr_1);
// Serial.printf("%d,%4.3f,%4.3f,%4.3f,%d\n\r",gycount,vx,vy,vz,tpr-tpr_1);
// Gravity+Linear出力
// SerialBT.printf("%d,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%d\n\r",gycount,grX[no],grY[no],grZ[no],laX[no],laY[no],laZ[no],tpr);
// Serial.printf("%d,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%d\n\r",gycount,grX[no],grY[no],grZ[no],laX[no],laY[no],laZ[no],tpr);
//Distance要素出力
//SerialBT.printf("%d,%4.3f,%4.3f,%4.3f,%d\n\r",gycount,ldX[no],ldY[no],ldZ[no],tpr);
//Serial.printf("%d,%4.3f,%4.3f,%4.3f,%d\n\r",gycount,ldX[no],ldY[no],ldZ[no],tpr);
//Distance+Gravity
// SerialBT.printf("%d,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%d\n\r",gycount,ldX[no],ldY[no],ldZ[no],ldsX[no],ldsY[no],ldsZ[no],tpr);
// Serial.printf("%d,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%d\n\r",gycount,ldX[no],ldY[no],ldZ[no],ldsX[no],ldsY[no],ldsZ[no],tpr);
}
void setup(void)
{
Serial.begin(115200);
pinMode(21, INPUT_PULLUP); //SDA 21番ピンのプルアップ(念のため)
pinMode(22, INPUT_PULLUP); //SDA 22番ピンのプルアップ(念のため)
while (!Serial) delay(10); // wait for serial port to open!
//MagEncoder Interrupt
pinMode(33, INPUT_PULLUP);
//attachInterrupt(33, magin, FALLING);
attachInterrupt(33, magin, CHANGE);
Serial.println("Orientation Sensor Test"); Serial.println("");
//--BNO085 RVC SET------------------------------------------------
Serial.println("Adafruit BNO08x IMU - UART-RVC mode");
Serial1.begin(115200,SERIAL_8N1,16,17); // RX16,TX17 This is the baud rate specified by the datasheet
while (!Serial1)
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!");
//----BNO055 SET-------------------------------------------
/* 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);
Serial.print("BT chipname:");
Serial.println(chipname);
}
void loop(void)
{
t=millis();
//======BNO085 reading=====================================================
BNO08x_RVC_Data heading;
// delay(20);
//Serial.printf("%4.2f,%4.2f,%4.2f\n\r",heading.yaw,heading.pitch,heading.roll);
//SerialBT.printf("%4.2f,%4.2f,%4.2f\n\r",heading.yaw,heading.pitch,heading.roll);
//===========================================================================
//Serial.printf("t-t1=%d\n\r",t-t1_1);
if(t-t1_1>50)
{
if (!rvc.read(&heading)) {
return;
}
//Serial.printf("INN:t-t1=%d\n\r",t-t1_1);
t1_1=t;
// bnoread(0);
bnoreadGYRO();
Serial.printf("%4.2f,%4.2f,%4.2f,%4.2f,%4.2f,%4.2f\n\r",heading.yaw,heading.pitch,heading.roll,yaw,pitch,roll);
SerialBT.printf("%4.2f,%4.2f,%4.2f,%4.2f,%4.2f,%4.2f\n\r",heading.yaw,heading.pitch,heading.roll,yaw,pitch,roll);
if(magflag==1)
{
SerialBT.println();
SerialBT.printf("magflag=1,time=%d\n\r",tmag);
magflag=0;
}
}
// delay(BNO055_SAMPLERATE_DELAY_MS);
}
void agread()
{
sensors_event_t event;
bno.getEvent(&event);
//sensors_event_t orientationData , angVelocityData , linearAccelData, magnetometerData, accelerometerData, gravityData;
//--------------get Gyro----------------------------
// bno.getEvent(&angVelocityData, Adafruit_BNO055::VECTOR_GYROSCOPE);
//parameter in
double gx = event.gyro.x;
double gy = event.gyro.y;
double gz = event.gyro.z;
//--------------get Linear Acc----------------------
//bno.getEvent(&linearAccelData, Adafruit_BNO055::VECTOR_LINEARACCEL);
//------------get Gravity------------------------
//bno.getEvent(&gravityData, Adafruit_BNO055::VECTOR_GRAVITY);
//
SerialBT.printf("gx=,%4.4f,gy=,%4.4f,gz=,%4.4f,t=,%d\n\r",gx,gy,gz,millis());
}
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) {
Serial.print("Accl:");
x = event->acceleration.x;
y = event->acceleration.y;
z = event->acceleration.z;
}
else if (event->type == SENSOR_TYPE_ORIENTATION) {
Serial.print("Orient:");
x = event->orientation.x;
y = event->orientation.y;
z = event->orientation.z;
}
else if (event->type == SENSOR_TYPE_MAGNETIC_FIELD) {
Serial.print("Mag:");
x = event->magnetic.x;
y = event->magnetic.y;
z = event->magnetic.z;
}
else if (event->type == SENSOR_TYPE_GYROSCOPE) {
Serial.print("Gyro:");
x = event->gyro.x;
y = event->gyro.y;
z = event->gyro.z;
}
else if (event->type == SENSOR_TYPE_ROTATION_VECTOR) {
Serial.print("Rot:");
x = event->gyro.x;
y = event->gyro.y;
z = event->gyro.z;
}
else if (event->type == SENSOR_TYPE_LINEAR_ACCELERATION) {
Serial.print("Linear:");
x = event->acceleration.x;
y = event->acceleration.y;
z = event->acceleration.z;
}
else if (event->type == SENSOR_TYPE_GRAVITY) {
Serial.print("Gravity:");
x = event->acceleration.x;
y = event->acceleration.y;
z = event->acceleration.z;
}
else {
Serial.print("Unk:");
}
Serial.print("\tx= ");
Serial.print(x);
Serial.print(" |\ty= ");
Serial.print(y);
Serial.print(" |\tz= ");
Serial.println(z);
}
void bnoread(int n){
//delay(20);//15+5=20msec半周期ずらす
sensors_event_t event;
sensors_event_t orientationData , angVelocityData , linearAccelData, magnetometerData, accelerometerData, gravityData;
bno.getEvent(&event);
//-------------------get Gyro---------------------------------
int no=gycount%150;//BNOのタイミングは、最初のtimepulseでスタートして3秒毎にカウンタがリセットされる 120msecとtimepulseが一致する周期3秒
int no_1=(gycount-1)%150;//
bno.getEvent(&angVelocityData, Adafruit_BNO055::VECTOR_GYROSCOPE);
//gx_1=gx;
//gy_1=gy;
//gz_1=gz;
//tg_1=tg;
// tg=millis();
//BNO時間をitowに変換
btime[no]=millis();//-tp03k)+itow03k;
gyX[no]=angVelocityData.gyro.x;
gyY[no]=angVelocityData.gyro.y;
gyZ[no]=angVelocityData.gyro.z;
gxsum=gxsum+(gyX[no]+gyX[no_1])/2*(double)(btime[no]-btime[no_1])/1000;//(btime[no]-btime[no_1)/1000;
gysum=gysum+(gyY[no]+gyY[no_1])/2*(double)(btime[no]-btime[no_1])/1000;//
gzsum=gzsum+(gyZ[no]+gyZ[no_1])/2*(double)(btime[no]-btime[no_1])/1000;//
gsX[no]=gxsum*57.29577;
gsY[no]=gysum*57.29577;
gsZ[no]=gzsum*57.29577;
tgread=millis();
gycount++;
//Euler angle
// uX[no]=(float)event.orientation.x;
// uY[no]=(float)event.orientation.y;
// uZ[no]=(float)event.orientation.z;
//Quartenion Copy from https://nobita-rx7.hatenablog.com/entry/27642606
imu::Quaternion quat = bno.getQuat();
qW[no]=(double)quat.w();
qX[no]=(double)quat.x();
qY[no]=(double)quat.y();
qZ[no]=(double)quat.z();
//Calc Euler from Quaternion
double w=qW[no];
double x=qX[no];
double y=qY[no];
double z=qZ[no];
double ysqr = y * y;
// roll (x-axis rotation)
double t0 = +2.0 * (w * x + y * z);
double t1 = +1.0 - 2.0 * (x * x + ysqr);
double roll = atan2(t0, t1);
// pitch (y-axis rotation)
double t2 = +2.0 * (w * y - z * x);
t2 = t2 > 1.0 ? 1.0 : t2;
t2 = t2 < -1.0 ? -1.0 : t2;
double pitch = asin(t2);
// yaw (z-axis rotation)
double t3 = +2.0 * (w * z + x * y);
double t4 = +1.0 - 2.0 * (ysqr + z * z);
//rad->deg
double yaw = atan2(t3, t4);
roll *= 57.2957795131;
pitch *= 57.2957795131;
yaw *= 57.2957795131;
if(yaw<0){//yaw 0-> -180deg=
yaw=360+yaw;;
}
yaw=360-yaw;
//------------------------------
//btime[no]=millis();
eX[no]=yaw;
eY[no]=pitch;
eZ[no]=roll;
//--------------get Linear Acc----------------------
bno.getEvent(&linearAccelData, Adafruit_BNO055::VECTOR_LINEARACCEL);
laX[no] = linearAccelData.acceleration.x;
laY[no] = linearAccelData.acceleration.y;
laZ[no] = linearAccelData.acceleration.z;
//-------------get Acc--------------------------------
bno.getEvent(&accelerometerData, Adafruit_BNO055::VECTOR_ACCELEROMETER);
acX[no] =accelerometerData.acceleration.x;
acY[no] =accelerometerData.acceleration.y;
acZ[no] =accelerometerData.acceleration.z;
// printEvent(&linearAccelData,no);
//------------get Gravity------------------------
bno.getEvent(&gravityData, Adafruit_BNO055::VECTOR_GRAVITY);
grX[no] = gravityData.acceleration.x;
grY[no] = gravityData.acceleration.y;
grZ[no] = gravityData.acceleration.z;
// printEvent(&gravityData,no);
//Serial.printf("bnoread:%d:%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3,%4.3f,%4.3f,%4.3\n\r",mills(),gX[no],gY[no],gz[no],gsX[no]gsY[no],gsZ[no],eX[no],eY[no],eZ[no],qW[no],qX[no],qY[no],qZ[no]);
// SerialBT.printf("bnoread:,%d,:[gy],%4.3f,%4.3f,%4.3f,[gs],%4.1f,%4.1f,%4.1f,[e],%4.1f,%4.1f,%4.1f,[qurt],%4.3f,%4.3f,%4.3f,%4.3f\n\r",millis(),gyX[no],gyY[no],gyZ[no],gsX[no],gsY[no],gsZ[no],eX[no],eY[no],eZ[no],qW[no],qX[no],qY[no],qZ[no]);
//SerialBT.printf("eX=,%4.2f,eY=%4.2f,eZ=%4.2f,t=,%d\n\r",eX,eY,eZ,millis());
SerialBT.printf("bn:,%d,[gs],%4.1f,%4.1f,%4.1f,[gr],%4.3f,%4.3f,%4.3f,[la],%4.3f,%4.3f,%4.3f,[qt],%4.5f,%4.5f,%4.5f,%4.5f,[ac],%4.3f,%4.3f,%4.3f,[gy],%4.3f,%4.3f,%4.3f\n\r",millis(),gsX[no],gsY[no],gsZ[no],grX[no],grY[no],grZ[no],laX[no],laY[no],laZ[no],qW[no],qX[no],qY[no],qZ[no],acX[no],acY[no],acZ[no],gyX[no],gyY[no],gyZ[no]);
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment