From 7ed1c14491b7927e4fb738924cd09b9b0cd150b6 Mon Sep 17 00:00:00 2001 From: poprox Date: Thu, 24 Sep 2026 18:04:39 +0200 Subject: [PATCH 1/2] Finish implementing GIRV --- lib/bno080/BNO080.cpp | 33 ++++++++++++++++++++++++++++----- lib/bno080/BNO080.h | 4 ++++ src/globals.h | 5 +++++ src/sensors/bno080sensor.cpp | 23 +++++++++++++++++++++++ 4 files changed, 60 insertions(+), 5 deletions(-) diff --git a/lib/bno080/BNO080.cpp b/lib/bno080/BNO080.cpp index 9a2373b7b..c16ff78c7 100644 --- a/lib/bno080/BNO080.cpp +++ b/lib/bno080/BNO080.cpp @@ -302,15 +302,18 @@ uint16_t BNO080::parseInputReport(void) timeStamp = ((uint32_t)shtpData[4] << (8 * 3)) | ((uint32_t)shtpData[3] << (8 * 2)) | ((uint32_t)shtpData[2] << (8 * 1)) | ((uint32_t)shtpData[1] << (8 * 0)); - // The gyro-integrated input reports are sent via the special gyro channel and do no include the usual ID, sequence, and status fields + // The gyro-integrated input reports are sent via the special gyro channel and do not include the usual ID, sequence, and status fields if(shtpHeader[2] == CHANNEL_GYRO) { - rawQuatI = (uint16_t)shtpData[1] << 8 | shtpData[0]; - rawQuatJ = (uint16_t)shtpData[3] << 8 | shtpData[2]; - rawQuatK = (uint16_t)shtpData[5] << 8 | shtpData[4]; - rawQuatReal = (uint16_t)shtpData[7] << 8 | shtpData[6]; + rawGyroIntegratedQuatI = (uint16_t)shtpData[1] << 8 | shtpData[0]; + rawGyroIntegratedQuatJ = (uint16_t)shtpData[3] << 8 | shtpData[2]; + rawGyroIntegratedQuatK = (uint16_t)shtpData[5] << 8 | shtpData[4]; + rawGyroIntegratedQuatReal = (uint16_t)shtpData[7] << 8 | shtpData[6]; + rawFastGyroX = (uint16_t)shtpData[9] << 8 | shtpData[8]; rawFastGyroY = (uint16_t)shtpData[11] << 8 | shtpData[10]; rawFastGyroZ = (uint16_t)shtpData[13] << 8 | shtpData[12]; + + hasNewGyroIntegratedQuaternion = true; hasNewFastGyro_ = true; return SENSOR_REPORTID_GYRO_INTEGRATED_ROTATION_VECTOR; } @@ -598,6 +601,26 @@ bool BNO080::getNewGameQuat(float &i, float &j, float &k, float &real, uint8_t & return false; } +void BNO080::getGyroIntegratedQuat( + float &i, + float &j, + float &k, + float &real +) +{ + i = qToFloat(rawGyroIntegratedQuatI, rotationVector_Q1); + j = qToFloat(rawGyroIntegratedQuatJ, rotationVector_Q1); + k = qToFloat(rawGyroIntegratedQuatK, rotationVector_Q1); + real = qToFloat(rawGyroIntegratedQuatReal, rotationVector_Q1); + + hasNewGyroIntegratedQuaternion = false; +} + +bool BNO080::hasNewGyroIntegratedQuat() +{ + return hasNewGyroIntegratedQuaternion; +} + void BNO080::getMagQuat(float &i, float &j, float &k, float &real, float &radAccuracy, uint8_t &accuracy) { i = qToFloat(rawMagQuatI, rotationVector_Q1); diff --git a/lib/bno080/BNO080.h b/lib/bno080/BNO080.h index 678714bdc..042e9ed10 100644 --- a/lib/bno080/BNO080.h +++ b/lib/bno080/BNO080.h @@ -204,6 +204,8 @@ class BNO080 bool getNewQuat(float &i, float &j, float &k, float &real, float &radAccuracy, uint8_t &accuracy); void getGameQuat(float &i, float &j, float &k, float &real, uint8_t &accuracy); bool getNewGameQuat(float &i, float &j, float &k, float &real, uint8_t &accuracy); + bool hasNewGyroIntegratedQuat(); + void getGyroIntegratedQuat(float &i, float &j, float &k, float &real); void getMagQuat(float &i, float &j, float &k, float &real, float &radAccuracy, uint8_t &accuracy); bool getNewMagQuat(float &i, float &j, float &k, float &real, float &radAccuracy, uint8_t &accuracy); float getQuatI(); @@ -345,6 +347,8 @@ class BNO080 uint16_t rawMagX, rawMagY, rawMagZ, magAccuracy; uint16_t rawQuatI, rawQuatJ, rawQuatK, rawQuatReal, rawQuatRadianAccuracy, quatAccuracy; uint16_t rawGameQuatI, rawGameQuatJ, rawGameQuatK, rawGameQuatReal, quatGameAccuracy; + uint16_t rawGyroIntegratedQuatI, rawGyroIntegratedQuatJ, rawGyroIntegratedQuatK, rawGyroIntegratedQuatReal; + bool hasNewGyroIntegratedQuaternion = false; uint16_t rawMagQuatI, rawMagQuatJ, rawMagQuatK, rawMagQuatReal, rawMagQuatRadianAccuracy, quatMagAccuracy; bool hasNewQuaternion, hasNewGameQuaternion, hasNewMagQuaternion, hasNewAccel_, hasNewLinAccel_, hasNewFastGyro_; bool hasNewMag_, hasNewGyro_; diff --git a/src/globals.h b/src/globals.h index db8103107..2ce070d2d 100644 --- a/src/globals.h +++ b/src/globals.h @@ -53,6 +53,11 @@ #define EXPERIMENTAL_BNO_DISABLE_ACCEL_CALIBRATION true #endif +#ifndef EXPERIMENTAL_BNO_USE_GIRV +#define EXPERIMENTAL_BNO_USE_GIRV true +// Use GIRV on BNO to report at 333Hz, stabilization still happens at 100Hz +#endif + #ifndef IMU_USE_EXTERNAL_CLOCK #define IMU_USE_EXTERNAL_CLOCK true // Use external clock for IMU (ICM-45686 only) #endif diff --git a/src/sensors/bno080sensor.cpp b/src/sensors/bno080sensor.cpp index c5cf59ff7..0d403eb13 100644 --- a/src/sensors/bno080sensor.cpp +++ b/src/sensors/bno080sensor.cpp @@ -60,6 +60,10 @@ void BNO080Sensor::motionSetup() { toggles = configuration.getSensorToggles(sensorId); +#if EXPERIMENTAL_BNO_USE_GIRV + imu.enableGyroIntegratedRotationVector(3); +#else + if (!toggles.getToggle(SensorToggles::MagEnabled)) { if ((sensorType == SensorTypeID::BNO085 || sensorType == SensorTypeID::BNO086) && BNO_USE_ARVR_STABILIZATION) { @@ -75,6 +79,7 @@ void BNO080Sensor::motionSetup() { imu.enableRotationVector(10); } } +#endif #if ENABLE_INSPECTION imu.enableRawGyro(10); @@ -190,6 +195,24 @@ void BNO080Sensor::motionLoop() { imu.resetNewRawGyro(); } +#if EXPERIMENTAL_BNO_USE_GIRV + + if (imu.hasNewGyroIntegratedQuat()) { + Quat nRotation; + + imu.getGyroIntegratedQuat( + nRotation.x, + nRotation.y, + nRotation.z, + nRotation.w + ); + + setFusedRotation(nRotation); + continue; + } + +#endif + if (!toggles.getToggle(SensorToggles::MagEnabled)) { if (imu.hasNewGameQuat()) // New quaternion if context { From 941552fcdd2a69a610558d3355a01ecced56fc83 Mon Sep 17 00:00:00 2001 From: poprox Date: Thu, 24 Sep 2026 18:19:04 +0200 Subject: [PATCH 2/2] Forgot to set it to false --- src/globals.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/globals.h b/src/globals.h index 2ce070d2d..e6a0d8344 100644 --- a/src/globals.h +++ b/src/globals.h @@ -54,7 +54,7 @@ #endif #ifndef EXPERIMENTAL_BNO_USE_GIRV -#define EXPERIMENTAL_BNO_USE_GIRV true +#define EXPERIMENTAL_BNO_USE_GIRV false // Use GIRV on BNO to report at 333Hz, stabilization still happens at 100Hz #endif