70 lines
2.3 KiB
C++
70 lines
2.3 KiB
C++
#include <Adafruit_BNO08x.h>
|
|
#include <Wire.h>
|
|
#include <math.h>
|
|
#include "imu.h"
|
|
#include "sensor_node_config.h"
|
|
|
|
namespace {
|
|
Adafruit_BNO08x bno086(-1);
|
|
sh2_SensorValue_t value;
|
|
bool initialized = false;
|
|
float radiansToDegrees(float r) { return r * 180.0f / PI; }
|
|
bool addressResponds(uint8_t address) {
|
|
Wire.beginTransmission(address);
|
|
return Wire.endTransmission() == 0;
|
|
}
|
|
|
|
uint8_t findBnoAddress() {
|
|
Serial.println("Scanning I2C bus on D7 SDA / D8 SCL...");
|
|
uint8_t bnoAddress = 0;
|
|
for (uint8_t address = 1; address < 127; address++) {
|
|
if (!addressResponds(address)) continue;
|
|
Serial.print("I2C device found at 0x");
|
|
if (address < 0x10) Serial.print('0');
|
|
Serial.println(address, HEX);
|
|
if (address == BNO086_I2C_ADDRESS || address == BNO086_I2C_ADDRESS_ALTERNATE) {
|
|
bnoAddress = address;
|
|
}
|
|
}
|
|
return bnoAddress;
|
|
}
|
|
|
|
void updateEuler(ImuState& s) {
|
|
float sinr = 2 * (s.real * s.i + s.j * s.k);
|
|
float cosr = 1 - 2 * (s.i * s.i + s.j * s.j);
|
|
s.rollDegrees = radiansToDegrees(atan2f(sinr, cosr));
|
|
float sinp = 2 * (s.real * s.j - s.k * s.i);
|
|
s.pitchDegrees = radiansToDegrees(fabsf(sinp) >= 1 ? copysignf(PI / 2, sinp) : asinf(sinp));
|
|
float siny = 2 * (s.real * s.k + s.i * s.j);
|
|
float cosy = 1 - 2 * (s.j * s.j + s.k * s.k);
|
|
s.yawDegrees = radiansToDegrees(atan2f(siny, cosy));
|
|
}
|
|
}
|
|
|
|
bool initImu() {
|
|
Wire.begin(BNO086_SDA_PIN, BNO086_SCL_PIN);
|
|
Wire.setClock(BNO086_I2C_CLOCK_HZ);
|
|
delay(100);
|
|
|
|
uint8_t address = findBnoAddress();
|
|
if (address == 0) {
|
|
Serial.println("No BNO086 found at 0x4A or 0x4B");
|
|
return false;
|
|
}
|
|
|
|
Serial.print("Starting BNO086 at 0x");
|
|
Serial.println(address, HEX);
|
|
initialized = bno086.begin_I2C(address, &Wire);
|
|
if (initialized) initialized = bno086.enableReport(SH2_ROTATION_VECTOR, IMU_REPORT_INTERVAL_US);
|
|
return initialized;
|
|
}
|
|
|
|
void updateImu(ImuState& s) {
|
|
s.online = initialized;
|
|
if (!initialized || !bno086.getSensorEvent(&value) || value.sensorId != SH2_ROTATION_VECTOR) return;
|
|
s.i = value.un.rotationVector.i; s.j = value.un.rotationVector.j;
|
|
s.k = value.un.rotationVector.k; s.real = value.un.rotationVector.real;
|
|
s.accuracy = value.status; s.valid = true; s.lastUpdateMs = millis();
|
|
updateEuler(s);
|
|
}
|