// Copyright (c) 2026 ЦПМК по информатике
// Licensed under the MIT License.
// https://robot.mipt.ru/
#include <Wire.h>
#define BNO_ADDR 0x29 // I2C адрес BNO055 (0x28 или 0x29)
#define BNO_CHIP_ID_REG 0x00 // Регистр идентификации (должен вернуть 0xA0)
// --- Структура для хранения данных ---
struct BNOData {
float euler[3]; // [0]-Курс(H), [1]-Крен(R), [2]-Тангаж(P). Эйлеровы углы в градусах.
float quat[4]; // [0]-W, [1]-X, [2]-Y, [3]-Z. Кватернионы Нормированные значения.
float linAcc[3]; // Линейное ускорение без гравитации (X, Y, Z) в m/s^2.
float grav[3]; // Вектор силы тяжести (X, Y, Z) в m/s^2.
float acc[3]; // Общее ускорение (X, Y, Z) в m/s^2.
float gyro[3]; // Угловая скорость (X, Y, Z) в Dps (град/сек).
float mag[3]; // Напряженность магн. поля (X, Y, Z) в uT (микротесла).
int8_t temp; // Температура чипа в градусах Цельсия.
uint8_t cal[4]; // Статус калибровки [0]-Система, [1]-Гироскоп, [2]-Акселерометр, [3]-Магнитометр (0..3).
};
// Функция записи в регистр
void writeReg(uint8_t reg, uint8_t val) {
Wire.beginTransmission(BNO_ADDR);
Wire.write(reg);
Wire.write(val);
Wire.endTransmission();
delay(30);
}
// Функция чтения нескольких регистров подряд
void readRegs(uint8_t reg, uint8_t *buf, uint8_t len) {
Wire.beginTransmission(BNO_ADDR);
Wire.write(reg);
Wire.endTransmission(false);
Wire.requestFrom((uint8_t)BNO_ADDR, (uint8_t)len);
for (uint8_t i = 0; i < len; i++) {
buf[i] = Wire.read();
}
}
// --- Функция чтения всех данных ---
void readBNOData(BNOData &data) {
uint8_t buffer[8]; // Буфер для приема данных (макс 8 байт для кватернионов)
// Регистр 0x08: Акселерометр (X,Y,Z). 1 LSB = 1/100 m/s^2
readRegs(0x08, buffer, 6);
for (int i = 0; i < 3; i++) data.acc[i] = (int16_t)((buffer[i * 2 + 1] << 8) | buffer[i * 2]) / 100.0;
// Регистр 0x0E: Магнитометр (X,Y,Z). 1 LSB = 1/16 uT
readRegs(0x0E, buffer, 6);
for (int i = 0; i < 3; i++) data.mag[i] = (int16_t)((buffer[i * 2 + 1] << 8) | buffer[i * 2]) / 16.0;
// Регистр 0x14: Гироскоп (X,Y,Z). 1 LSB = 1/16 Dps
readRegs(0x14, buffer, 6);
for (int i = 0; i < 3; i++) data.gyro[i] = (int16_t)((buffer[i * 2 + 1] << 8) | buffer[i * 2]) / 16.0;
// Регистр 0x1A: Эйлеровы углы (H,R,P). 1 LSB = 1/16 Degree
readRegs(0x1A, buffer, 6);
for (int i = 0; i < 3; i++) data.euler[i] = (int16_t)((buffer[i * 2 + 1] << 8) | buffer[i * 2]) / 16.0;
// Регистр 0x20: Кватернионы (W,X,Y,Z). 1 LSB = 1/2^14 (16384)
readRegs(0x20, buffer, 8);
for (int i = 0; i < 4; i++) data.quat[i] = (int16_t)((buffer[i * 2 + 1] << 8) | buffer[i * 2]) / 16384.0;
// Регистр 0x28: Линейное ускорение (X,Y,Z). 1 LSB = 1/100 m/s^2
readRegs(0x28, buffer, 6);
for (int i = 0; i < 3; i++) data.linAcc[i] = (int16_t)((buffer[i * 2 + 1] << 8) | buffer[i * 2]) / 100.0;
// Регистр 0x2E: Гравитация (X,Y,Z). 1 LSB = 1/100 m/s^2
readRegs(0x2E, buffer, 6);
for (int i = 0; i < 3; i++) data.grav[i] = (int16_t)((buffer[i * 2 + 1] << 8) | buffer[i * 2]) / 100.0;
// Регистр 0x34: Температура. 1 LSB = 1°C
readRegs(0x34, buffer, 1);
data.temp = (int8_t)buffer[0];
// Регистр 0x35: Статус калибровки (битовое поле)
readRegs(0x35, buffer, 1);
data.cal[0] = (buffer[0] >> 6) & 0x03; // Система
data.cal[1] = (buffer[0] >> 4) & 0x03; // Гироскоп
data.cal[2] = (buffer[0] >> 2) & 0x03; // Акселерометр
data.cal[3] = buffer[0] & 0x03; // Магнитометр
}
void printOutput(BNOData &data) { // Функция для вывода в Serial всех измеряемых значений
Serial.println("--- BNO055 FULL REPORT ---");
Serial.print("Euler: ");
Serial.print(data.euler[0]);
Serial.print(" ");
Serial.print(data.euler[1]);
Serial.print(" ");
Serial.println(data.euler[2]);
Serial.print("Quat: ");
Serial.print(data.quat[0]);
Serial.print(" ");
Serial.print(data.quat[1]);
Serial.print(" ");
Serial.print(data.quat[2]);
Serial.print(" ");
Serial.println(data.quat[3]);
Serial.print("LinAcc:");
Serial.print(data.linAcc[0]);
Serial.print(" ");
Serial.print(data.linAcc[1]);
Serial.print(" ");
Serial.println(data.linAcc[2]);
Serial.print("Grav: ");
Serial.print(data.grav[0]);
Serial.print(" ");
Serial.print(data.grav[1]);
Serial.print(" ");
Serial.println(data.grav[2]);
Serial.print("TotalAcc:");
Serial.print(data.acc[0]);
Serial.print(" ");
Serial.print(data.acc[1]);
Serial.print(" ");
Serial.println(data.acc[2]);
Serial.print("Gyro: ");
Serial.print(data.gyro[0]);
Serial.print(" ");
Serial.print(data.gyro[1]);
Serial.print(" ");
Serial.println(data.gyro[2]);
Serial.print("Mag: ");
Serial.print(data.mag[0]);
Serial.print(" ");
Serial.print(data.mag[1]);
Serial.print(" ");
Serial.println(data.mag[2]);
Serial.print("Temp: ");
Serial.print(data.temp);
Serial.println(" C");
Serial.print("Calib: [S:");
Serial.print(data.cal[0]);
Serial.print(" G:");
Serial.print(data.cal[1]);
Serial.print(" A:");
Serial.print(data.cal[2]);
Serial.print(" M:");
Serial.print(data.cal[3]);
Serial.println("]");
Serial.println();
}
void plotEuler(const BNOData &data) { // Функция для вывода в Serial Plotter углов Эйлера
// Формат для Serial Plotter: "Название1:Значение1,Название2:Значение2..."
Serial.print("Heading:"); // [0]-Курс(H)
Serial.print(data.euler[0]);
Serial.print(",");
Serial.print("Roll:"); // [1]-Крен(R)
Serial.print(data.euler[1]);
Serial.print(",");
Serial.print("Pitch:"); // [2]-Тангаж(P)
Serial.println(data.euler[2]); // В конце обязательно println
}
void setup() {
Serial.begin(115200);
Wire.begin();
// Проверка связи с датчиком, остановит программу если BNO055 не найден
uint8_t id;
readRegs(BNO_CHIP_ID_REG, &id, 1);
if (id != 0xA0) {
Serial.println("Ошибка: BNO055 не найден!");
while (1);
}
// Настройка датчика
writeReg(0x3D, 0x00); // OPR_MODE: CONFIGMODE (нужен для настройки)
writeReg(0x3F, 0x20); // SYS_TRIGGER: Сброс системы
delay(700); // Время на перезагрузку
writeReg(0x3B, 0x00); // UNIT_SEL: m/s2, Dps, Degrees, Celsius (выбор единиц измерения)
writeReg(0x3D, 0x0C); // OPR_MODE: NDOF (Sensor Fusion включен)
delay(100);
}
void loop() {
BNOData currentData;
readBNOData(currentData); // Вызов функции чтения
plotEuler(currentData); // Функция для вывода в Serial Plotter углов Эйлера
// printOutput(currentData); // Вывод в Serial всех данных
delay(100);
}
