github/SlimeVR-Tracker-ESP
1
1
Fork 0
mirror of https://github.com/SlimeVR/SlimeVR-Tracker-ESP.git synced 2026-04-06 02:01:57 +02:00
Code Issues 76 Packages Projects Releases Wiki Activity

Refactor BNO08x conditions (#166)

Browse Source
This commit is contained in:
Yury
2022-06-28 19:11:37 +02:00
committed by GitHub
parent aac00d813d
commit 37d95bf749
3 changed files with 77 additions and 68 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
src/debug.h
View File

@@ -26,8 +26,9 @@
#include "logging/Level.h"
#define IMU_MPU6050_RUNTIME_CALIBRATION // Comment to revert to startup/traditional-calibration
#define BNO_USE_ARVR_STABILIZATION // Comment to not use stabilization for BNO085+ IMUs
#define USE_6_AXIS true // uses 9 (with mag) if false (only for ICM-20948 currently)
#define BNO_USE_ARVR_STABILIZATION true // Set to false to disable stabilization for BNO085+ IMUs
#define BNO_USE_MAGNETOMETER_CORRECTION false // Set to true to enable magnetometer correction for BNO08x IMUs. Only works with USE_6_AXIS set to true.
#define USE_6_AXIS true // uses 9 DoF (with mag) if false (only for ICM-20948 and BNO08x currently)
#define LOAD_BIAS 1 // Loads the bias values from NVS on start (ESP32 Only)
#define SAVE_BIAS 1 // Periodically saves bias calibration data to NVS (ESP32 Only)
#define BIAS_DEBUG false // Printing BIAS Variables to serial (ICM20948 only)

138
src/sensors/bno080sensor.cpp
View File

@@ -52,27 +52,24 @@ void BNO080Sensor::motionSetup()
imu.swVersionPatch
);
bool useStabilization = false;
#ifdef BNO_USE_ARVR_STABILIZATION
useStabilization = true;
#if USE_6_AXIS
#if (IMU == IMU_BNO085 || IMU == IMU_BNO086) && BNO_USE_ARVR_STABILIZATION
imu.enableARVRStabilizedGameRotationVector(10);
#else
imu.enableGameRotationVector(10);
#endif
#if BNO_USE_MAGNETOMETER_CORRECTION
imu.enableRotationVector(1000);
#endif
#else
#if (IMU == IMU_BNO085 || IMU == IMU_BNO086) && BNO_USE_ARVR_STABILIZATION
imu.enableARVRStabilizedRotationVector(10);
#else
imu.enableRotationVector(10);
#endif
#endif
if(useStabilization && (sensorType == IMU_BNO085 || sensorType == IMU_BNO086)) {
if(useMagnetometerAllTheTime) {
imu.enableARVRStabilizedRotationVector(10);
} else {
imu.enableARVRStabilizedGameRotationVector(10);
if (useMagnetometerCorrection)
imu.enableRotationVector(1000);
}
} else {
if(useMagnetometerAllTheTime) {
imu.enableRotationVector(10);
} else {
imu.enableGameRotationVector(10);
if(useMagnetometerCorrection)
imu.enableRotationVector(1000);
}
}
imu.enableTapDetector(100);
#if ENABLE_INSPECTION
@@ -115,55 +112,62 @@ void BNO080Sensor::motionLoop()
lastReset = 0;
lastData = millis();
if (useMagnetometerAllTheTime || !useMagnetometerCorrection)
#if USE_6_AXIS
if (imu.hasNewGameQuat())
{
if (imu.hasNewQuat())
imu.getGameQuat(quaternion.x, quaternion.y, quaternion.z, quaternion.w, calibrationAccuracy);
quaternion *= sensorOffset;
#if ENABLE_INSPECTION
{
imu.getQuat(quaternion.x, quaternion.y, quaternion.z, quaternion.w, magneticAccuracyEstimate, calibrationAccuracy);
quaternion *= sensorOffset;
#if ENABLE_INSPECTION
{
Network::sendInspectionFusedIMUData(sensorId, quaternion);
}
#endif
if (!OPTIMIZE_UPDATES || !lastQuatSent.equalsWithEpsilon(quaternion))
{
newData = true;
lastQuatSent = quaternion;
}
Network::sendInspectionFusedIMUData(sensorId, quaternion);
}
}
else
{
if (imu.hasNewGameQuat())
#endif // ENABLE_INSPECTION
if (!OPTIMIZE_UPDATES || !lastQuatSent.equalsWithEpsilon(quaternion))
{
imu.getGameQuat(quaternion.x, quaternion.y, quaternion.z, quaternion.w, calibrationAccuracy);
quaternion *= sensorOffset;
#if ENABLE_INSPECTION
{
Network::sendInspectionFusedIMUData(sensorId, quaternion);
}
#endif
newData = true;
}
if (imu.hasNewMagQuat())
{
imu.getMagQuat(magQuaternion.x, magQuaternion.y, magQuaternion.z, magQuaternion.w, magneticAccuracyEstimate, magCalibrationAccuracy);
magQuaternion *= sensorOffset;
#if ENABLE_INSPECTION
{
Network::sendInspectionCorrectionData(sensorId, quaternion);
}
#endif
newMagData = true;
lastQuatSent = quaternion;
}
}
#if BNO_USE_MAGNETOMETER_CORRECTION
if (imu.hasNewMagQuat())
{
imu.getMagQuat(magQuaternion.x, magQuaternion.y, magQuaternion.z, magQuaternion.w, magneticAccuracyEstimate, magCalibrationAccuracy);
magQuaternion *= sensorOffset;
#if ENABLE_INSPECTION
{
Network::sendInspectionCorrectionData(sensorId, quaternion);
}
#endif // ENABLE_INSPECTION
newMagData = true;
}
#endif // BNO_USE_MAGNETOMETER_CORRECTION
#else // USE_6_AXIS
if (imu.hasNewQuat())
{
imu.getQuat(quaternion.x, quaternion.y, quaternion.z, quaternion.w, magneticAccuracyEstimate, calibrationAccuracy);
quaternion *= sensorOffset;
#if ENABLE_INSPECTION
{
Network::sendInspectionFusedIMUData(sensorId, quaternion);
}
#endif // ENABLE_INSPECTION
if (!OPTIMIZE_UPDATES || !lastQuatSent.equalsWithEpsilon(quaternion))
{
newData = true;
lastQuatSent = quaternion;
}
}
#endif // USE_6_AXIS
if (imu.getTapDetected())
{
tap = imu.getTapDetector();
@@ -213,19 +217,25 @@ void BNO080Sensor::sendData()
{
newData = false;
Network::sendRotationData(&quaternion, DATA_TYPE_NORMAL, calibrationAccuracy, sensorId);
if (useMagnetometerAllTheTime)
Network::sendMagnetometerAccuracy(magneticAccuracyEstimate, sensorId);
#if !USE_6_AXIS
Network::sendMagnetometerAccuracy(magneticAccuracyEstimate, sensorId);
#endif
#ifdef DEBUG_SENSOR
m_Logger.trace("Quaternion: %f, %f, %f, %f", UNPACK_QUATERNION(quaternion));
#endif
}
#if USE_6_AXIS && BNO_USE_MAGNETOMETER_CORRECTION
if (newMagData)
{
newMagData = false;
Network::sendRotationData(&magQuaternion, DATA_TYPE_CORRECTION, magCalibrationAccuracy, sensorId);
Network::sendMagnetometerAccuracy(magneticAccuracyEstimate, sensorId);
}
#endif
if (tap != 0)
{
Network::sendTap(tap, sensorId);

2
src/sensors/bno080sensor.h
View File

@@ -57,8 +57,6 @@ private:
Quat magQuaternion{};
uint8_t magCalibrationAccuracy = 0;
float magneticAccuracyEstimate = 999;
bool useMagnetometerAllTheTime = false;
bool useMagnetometerCorrection = false;
bool newMagData = false;
};