I am selecting data from BNO08x via the library
The code is as follows
#include <Adafruit_BNO08x.h>
#define SERIAL_PORT_SPEED 230400
#define I2C_SPEED_SET (400000) // kHz
#define BNO08X_RESET -1
#define sq(x) ((x)*(x))
const unsigned int BNO_Period = 5;
unsigned long BNO_Timestamp = 0;
long current_timestamp = millis();
long report_interval_euler = 1000;
// long report_interval_euler = 2000;
struct euler_t {
float yaw;
float pitch;
float roll;
} ypr_float;
Adafruit_BNO08x bno08x(BNO08X_RESET);
sh2_SensorValue_t sensorValue;
void quaternionToEuler(float qr, float qi, float qj, float qk, euler_t* ypr, bool degrees);
void quaternionToEulerRV(sh2_RotationVectorWAcc_t* rotational_vector, euler_t* ypr, bool degrees);
void setReports(void);
void setup(void) {
Serial.begin(SERIAL_PORT_SPEED); // PA9, PA10
Serial.println("Adafruit BNO08x test!");
Wire.begin();
// Try to initialize!
if (!bno08x.begin_I2C()) {
Serial.println("Failed to find BNO08x chip");
while (1) {
delay(120);
NVIC_SystemReset();
}
}
Wire.setClock(I2C_SPEED_SET); //Set clock speed to 400kHz
Serial.println("BNO08x Found!");
Serial.println("Reading events");
delay(100);
// default orient
// Y - N, X - E, Z - U
// sh2_Quaternion_t sh_orientation = { 0, 0, sqrt(2) / 2, 0}; // Qx, Qy, Qz, Qw
// Y - S, X - E, Z - D
sh2_Quaternion_t sh_orientation = { -1, 0, 0, 0}; // Qx, Qy, Qz, Qw
}
setReports();
} // SETUP
void loop() {
if (bno08x.wasReset()) {
Serial.println("sensor was reset ");
}
if (millis() - BNO_Timestamp >= BNO_Period) {
bno08x.getSensorEvent(&sensorValue);
telemetry_bno08x_min();
BNO_Timestamp = millis();
}
switch (sensorValue.sensorId) {
// https://forums.adafruit.com/viewtopic.php?p=935347#p935347
case SH2_ROTATION_VECTOR:
quaternionToEulerRV(&sensorValue.un.rotationVector, &ypr_float, true);
break;
}
} // END MODE BNO08X
} //LOOP
void quaternionToEuler(float qr, float qi, float qj, float qk, euler_t* ypr, bool degrees = false) {
float sqr = sq(qr);
float sqi = sq(qi);
float sqj = sq(qj);
float sqk = sq(qk);
ypr->yaw = atan2(2.0 * (qi * qj + qk * qr), (sqi - sqj - sqk + sqr));
ypr->pitch = asin(-2.0 * (qi * qk - qj * qr) / (sqi + sqj + sqk + sqr));
ypr->roll = atan2(2.0 * (qj * qk + qi * qr), (-sqi - sqj + sqk + sqr));
if (degrees) {
ypr->yaw *= RAD_TO_DEG;
ypr->pitch *= -RAD_TO_DEG;
ypr->roll *= RAD_TO_DEG;
}
}
void quaternionToEulerRV(sh2_RotationVectorWAcc_t* rotational_vector, euler_t* ypr, bool degrees = false) {
quaternionToEuler(rotational_vector->real, rotational_vector->i, rotational_vector->j, rotational_vector->k, ypr, degrees);
}
void setReports(void) {
Serial.println("Setting desired reports");
if (!bno08x.enableReport(SH2_ROTATION_VECTOR, report_interval_euler)) {
Serial.println("Could not enable rotation vector");
}
}
void telemetry_bno08x_min()
{
int textLength = 5 * 30;
char text[textLength];
char time[10];
char time_elapse[10];
char ex[10];
char ey[10];
char ez[10];
long timestamp = millis(); // Get the current time
long elapsed_time = timestamp - current_timestamp; // Calculate the time difference
dtostrf(timestamp, 10, 0, time);
dtostrf(elapsed_time, 10, 0, time_elapse);
dtostrf(ypr_float.roll, 10, 2, ex);
dtostrf(ypr_float.pitch, 10, 2, ey);
dtostrf(ypr_float.yaw, 10, 2, ez);
snprintf(text, textLength, "%10s,%4s,%8s,%8s,%8s", time, time_elapse, ex, ey, ez);
Serial.println(text);
// Updating the current time
current_timestamp = timestamp;
}
This code collects data from the sensor and transmits it to the serial port:
timestamp,elapsed,eul_x,eul_y,eul_z
1227, 1227, 0.00, 0.00, 0.00
1238, 11, 0.00, 0.00, 0.00
1244, 6, 0.00, 0.00, 0.00
...
1315, 9, -33.17, -14.75, -46.33
1323, 8, -34.56, -14.56, -46.40
1331, 8, -35.82, -14.35, -46.46
1338, 7, -36.98, -14.18, -46.50
1347, 9, -38.07, -14.01, -46.56
1355, 8, -39.46, -13.64, -46.79
1363, 8, -40.43, -13.39, -46.89
1371, 8, -41.28, -13.14, -47.00
1379, 8, -42.27, -12.81, -47.12
1387, 8, -42.99, -12.54, -47.25
...
1495, 9, -47.76, -10.88, -46.61
1503, 8, -47.16, -10.69, -46.74
1511, 8, -46.55, -10.56, -46.79
1519, 8, -46.11, -10.52, -46.86
1527, 8, -44.69, -10.41, -46.88
1541, 14, -43.70, -10.36, -46.76
1550, 9, -42.47, -10.25, -46.57
...
1566, 7, -36.65, -9.58, -45.92
1575, 9, -34.20, -9.29, -45.70
1582, 7, -30.86, -8.95, -45.49
1591, 9, -29.30, -8.75, -45.46
1600, 9, -26.34, -8.29, -45.47
1608, 8, -23.87, -7.88, -45.43
1616, 8, -21.74, -7.54, -45.41
1624, 8, -19.56, -7.19, -45.42
1632, 8, -17.86, -5.68, -45.44
1640, 8, -15.87, -5.12, -45.47
1648, 8, -13.50, -4.51, -45.54
1656, 8, -11.75, -4.14, -45.60
The sampling time is 8-10 ms, which is about 100 Hz, but there are some strange things with sampling data from the sensor.
report_interval_euler = 1000;
But for some reason, when you rotate the sensor slowly in roll or pitch angles, the data in the serial port goes every 8 ms, every 1 degree.
When you rotate the sensor quickly in roll or pitch angles, the angles in the serial port are output with gaps from 2 to 10 degrees.
What am I doing wrong?
I know about the INT pin and understand how to use it, but will this solve my problem and is there another way to deal with data gaps from the sensor - collecting data in a buffer, organizing a ring buffer, or something else?
I would be grateful for your help.