Created
November 23, 2023 03:40
-
-
Save dj1711572002/ecd6f1c11f9b52f38688d08af5a4a437 to your computer and use it in GitHub Desktop.
ESP32 Arduino BNO055 vs BNO085 Rotation Accuracy comparison
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#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