From 39545d7ae7c100b91a94222ee8dcd8758176aee4 Mon Sep 17 00:00:00 2001 From: Meia <71262281+kounocom@users.noreply.github.com> Date: Thu, 17 Apr 2025 15:23:36 +0200 Subject: [PATCH 1/5] Remove OTA timeout (#419) * Remove OTA timeout * Make it toggleable from debug.h --- lib/ota/ota.cpp | 2 ++ src/debug.h | 4 ++++ 2 files changed, 6 insertions(+) diff --git a/lib/ota/ota.cpp b/lib/ota/ota.cpp index 7ebc9aa..667251e 100644 --- a/lib/ota/ota.cpp +++ b/lib/ota/ota.cpp @@ -64,12 +64,14 @@ void OTA::otaSetup(const char * const otaPassword) { void OTA::otaUpdate() { if(enabled) { + #if USE_OTA_TIMEOUT if(bootTime + 60000 < millis()) { // Disable OTA 60 seconds after boot as protection measure enabled = false; Serial.println("[NOTICE] OTA updates disabled by timeout, this is not an error"); return; } + #endif ArduinoOTA.handle(); } } diff --git a/src/debug.h b/src/debug.h index 72b1037..cf49ea4 100644 --- a/src/debug.h +++ b/src/debug.h @@ -110,4 +110,8 @@ #define DEBUG_MEASURE_SENSOR_TIME_TAKEN false #endif +#ifndef USE_OTA_TIMEOUT +#define USE_OTA_TIMEOUT false +#endif + #endif // SLIMEVR_DEBUG_H_ From 913a330601db8d0700e6a1eb495f08ead4619344 Mon Sep 17 00:00:00 2001 From: Meia <71262281+kounocom@users.noreply.github.com> Date: Thu, 17 Apr 2025 15:24:01 +0200 Subject: [PATCH 2/5] Use ubuntu-latest for build job (#421) Use ubuntu-latest for build action --- .github/workflows/actions.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/actions.yml b/.github/workflows/actions.yml index 06b0ae7..37510c2 100644 --- a/.github/workflows/actions.yml +++ b/.github/workflows/actions.yml @@ -30,7 +30,7 @@ jobs: # cmake_args: '-G Ninja -DCMAKE_C_COMPILER=clang -DCMAKE_CXX_COMPILER=clang++' build: - runs-on: ubuntu-20.04 + runs-on: ubuntu-latest steps: - uses: actions/checkout@v4 From 4c2377f2f33230ea4e745534912e31646c6324d2 Mon Sep 17 00:00:00 2001 From: unlogisch04 <98281608+unlogisch04@users.noreply.github.com> Date: Thu, 17 Apr 2025 17:57:33 +0200 Subject: [PATCH 3/5] fix SensorStateUpdate (#418) After a reboot of the tracker the optional sensor did show up on the server too. This is because a sensor packet was sent from all sensors including empty and unknown. This should hopefully fix the issue. It is a fast fix. It probably would be better to move all this check to sensor, and if the sensor has a chang set the flag, and reset the flag if the function gets called. --- src/network/connection.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/network/connection.cpp b/src/network/connection.cpp index 8168dee..dd906df 100644 --- a/src/network/connection.cpp +++ b/src/network/connection.cpp @@ -514,9 +514,11 @@ void Connection::maybeRequestFeatureFlags() { } bool Connection::isSensorStateUpdated(int i, std::unique_ptr& sensor) { - return m_AckedSensorState[i] != sensor->getSensorState() - || m_AckedSensorCalibration[i] != sensor->hasCompletedRestCalibration() - || m_AckedSensorConfigData[i] != sensor->getSensorConfigData(); + return (m_AckedSensorState[i] != sensor->getSensorState() + || m_AckedSensorCalibration[i] != sensor->hasCompletedRestCalibration() + || m_AckedSensorConfigData[i] != sensor->getSensorConfigData()) + && sensor->getSensorType() != SensorTypeID::Unknown + && sensor->getSensorType() != SensorTypeID::Empty; } void Connection::searchForServer() { From ee48341c303e97a2d35eb978311bb682dc1cce9b Mon Sep 17 00:00:00 2001 From: unlogisch04 <98281608+unlogisch04@users.noreply.github.com> Date: Thu, 17 Apr 2025 18:15:47 +0200 Subject: [PATCH 4/5] Feat BNO085 temp (#417) * BNO085 add more Features and Readouts to the lib * BNO085 add - Experimental compiler flags for disable Calibration - Temp Readout (all 1 sec) - Inspection only send when updated - added conginue as 1 imu.dataAvailable() only reads out 1 packet of data * BNO085 add source of info --- lib/bno080/BNO080.cpp | 172 +++++++++++++++++++++++++++++++++-- lib/bno080/BNO080.h | 28 +++++- src/globals.h | 5 + src/sensors/bno080sensor.cpp | 93 ++++++++++++------- src/sensors/bno080sensor.h | 6 ++ 5 files changed, 263 insertions(+), 41 deletions(-) diff --git a/lib/bno080/BNO080.cpp b/lib/bno080/BNO080.cpp index 99e1ebf..e97e2a2 100644 --- a/lib/bno080/BNO080.cpp +++ b/lib/bno080/BNO080.cpp @@ -285,12 +285,13 @@ uint16_t BNO080::parseCommandReport(void) //shtpData[5 + 0]: Then a feature report ID (0x01 for Accel, 0x05 for Rotation Vector) //shtpData[5 + 1]: Sequence number (See 6.5.18.2) //shtpData[5 + 2]: Status -//shtpData[3]: Delay -//shtpData[4:5]: i/accel x/gyro x/etc -//shtpData[6:7]: j/accel y/gyro y/etc -//shtpData[8:9]: k/accel z/gyro z/etc -//shtpData[10:11]: real/gyro temp/etc -//shtpData[12:13]: Accuracy estimate +//shtpData[5 + 3]: Delay +//shtpData[5 + 4:5]: i/accel x/gyro x/etc +//shtpData[5 + 6:7]: j/accel y/gyro y/etc +//shtpData[5 + 8:9]: k/accel z/gyro z/etc +//shtpData[5 + 10:11]: real/gyro temp/etc +//shtpData[5 + 12:13]: Accuracy estimate: Raw Accel/Gyro/Mag Timestap part1 +//shtpData[5 + 14:15]: Raw Accel/Gyro/Mag Timestap part2 uint16_t BNO080::parseInputReport(void) { //Calculate the number of data bytes in this packet @@ -311,7 +312,7 @@ uint16_t BNO080::parseInputReport(void) rawFastGyroX = (uint16_t)shtpData[9] << 8 | shtpData[8]; rawFastGyroY = (uint16_t)shtpData[11] << 8 | shtpData[10]; rawFastGyroZ = (uint16_t)shtpData[13] << 8 | shtpData[12]; - + hasNewFastGyro_ = true; return SENSOR_REPORTID_GYRO_INTEGRATED_ROTATION_VECTOR; } @@ -321,6 +322,7 @@ uint16_t BNO080::parseInputReport(void) uint16_t data3 = (uint16_t)shtpData[5 + 9] << 8 | shtpData[5 + 8]; uint16_t data4 = 0; uint16_t data5 = 0; //We would need to change this to uin32_t to capture time stamp value on Raw Accel/Gyro/Mag reports + uint32_t memstimeStamp = 0; //Timestamp of MEMS sensor reading if (dataLength - 5 > 9) { @@ -330,6 +332,11 @@ uint16_t BNO080::parseInputReport(void) { data5 = (uint16_t)shtpData[5 + 13] << 8 | shtpData[5 + 12]; } + //only for Raw Reports 0x14, 0x15, 0x16 + if (dataLength - 5 >= 15) + { + memstimeStamp = ((uint32_t)shtpData[5 + 15] << (8 * 3)) | ((uint32_t)shtpData[5 + 14] << (8 * 2)) | ((uint32_t)shtpData[5 + 13] << (8 * 1)) | ((uint32_t)shtpData[5 + 12] << (8 * 0)); + } //Store these generic values to their proper global variable if (shtpData[5] == SENSOR_REPORTID_ACCELEROMETER || shtpData[5] == SENSOR_REPORTID_GRAVITY) @@ -342,6 +349,7 @@ uint16_t BNO080::parseInputReport(void) } else if (shtpData[5] == SENSOR_REPORTID_LINEAR_ACCELERATION) { + hasNewLinAccel_ = true; accelLinAccuracy = status; rawLinAccelX = data1; rawLinAccelY = data2; @@ -349,6 +357,7 @@ uint16_t BNO080::parseInputReport(void) } else if (shtpData[5] == SENSOR_REPORTID_GYROSCOPE) { + hasNewGyro_ = true; gyroAccuracy = status; rawGyroX = data1; rawGyroY = data2; @@ -356,6 +365,7 @@ uint16_t BNO080::parseInputReport(void) } else if (shtpData[5] == SENSOR_REPORTID_MAGNETIC_FIELD) { + hasNewMag_ = true; magAccuracy = status; rawMagX = data1; rawMagY = data2; @@ -418,21 +428,28 @@ uint16_t BNO080::parseInputReport(void) } else if (shtpData[5] == SENSOR_REPORTID_RAW_ACCELEROMETER) { + hasNewRawAccel_ = true; memsRawAccelX = data1; memsRawAccelY = data2; memsRawAccelZ = data3; + memsAccelTimeStamp = memstimeStamp; } else if (shtpData[5] == SENSOR_REPORTID_RAW_GYROSCOPE) { + hasNewRawGyro_ = true; memsRawGyroX = data1; memsRawGyroY = data2; memsRawGyroZ = data3; + memsRawGyroTemp = data4; + memsGyroTimeStamp = memstimeStamp; } else if (shtpData[5] == SENSOR_REPORTID_RAW_MAGNETOMETER) { + hasNewRawMag_ = true; memsRawMagX = data1; memsRawMagY = data2; memsRawMagZ = data3; + memsMagTimeStamp = memstimeStamp; } else if (shtpData[5] == SHTP_REPORT_COMMAND_RESPONSE) { @@ -552,6 +569,16 @@ void BNO080::getQuat(float &i, float &j, float &k, float &real, float &radAccura hasNewQuaternion = false; } +bool BNO080::getNewQuat(float &i, float &j, float &k, float &real, float &radAccuracy, uint8_t &accuracy) +{ + if (hasNewQuaternion) + { + getQuat(i, j, k, real, radAccuracy, accuracy); + return true; + } + return false; +} + void BNO080::getGameQuat(float &i, float &j, float &k, float &real, uint8_t &accuracy) { i = qToFloat(rawGameQuatI, rotationVector_Q1); @@ -562,6 +589,16 @@ void BNO080::getGameQuat(float &i, float &j, float &k, float &real, uint8_t &acc hasNewGameQuaternion = false; } +bool BNO080::getNewGameQuat(float &i, float &j, float &k, float &real, uint8_t &accuracy) +{ + if (hasNewGameQuaternion) + { + getGameQuat(i, j, k, real, accuracy); + return true; + } + return false; +} + void BNO080::getMagQuat(float &i, float &j, float &k, float &real, float &radAccuracy, uint8_t &accuracy) { i = qToFloat(rawMagQuatI, rotationVector_Q1); @@ -573,6 +610,16 @@ void BNO080::getMagQuat(float &i, float &j, float &k, float &real, float &radAcc hasNewMagQuaternion = false; } +bool BNO080::getNewMagQuat(float &i, float &j, float &k, float &real, float &radAccuracy, uint8_t &accuracy) +{ + if (hasNewMagQuaternion) + { + getMagQuat(i, j, k, real, radAccuracy, accuracy); + return true; + } + return false; +} + bool BNO080::hasNewQuat() { return hasNewQuaternion; } @@ -678,6 +725,21 @@ void BNO080::getLinAccel(float &x, float &y, float &z, uint8_t &accuracy) y = qToFloat(rawLinAccelY, linear_accelerometer_Q1); z = qToFloat(rawLinAccelZ, linear_accelerometer_Q1); accuracy = accelLinAccuracy; + hasNewLinAccel_ = false; +} + +bool BNO080::getNewLinAccel(float &x, float &y, float &z, uint8_t &accuracy) +{ + if (hasNewLinAccel_) + { + getLinAccel(x, y, z, accuracy); + return true; + } + return false; +} + +bool BNO080::hasNewLinAccel() { + return hasNewLinAccel_; } //Return the acceleration component @@ -715,6 +777,7 @@ void BNO080::getGyro(float &x, float &y, float &z, uint8_t &accuracy) y = qToFloat(rawGyroY, gyro_Q1); z = qToFloat(rawGyroZ, gyro_Q1); accuracy = gyroAccuracy; + hasNewGyro_ = false; } //Return the gyro component @@ -744,6 +807,10 @@ uint8_t BNO080::getGyroAccuracy() return (gyroAccuracy); } +bool BNO080::hasNewGyro() { + return hasNewGyro_; +} + //Gets the full mag vector //x,y,z output floats void BNO080::getMag(float &x, float &y, float &z, uint8_t &accuracy) @@ -752,6 +819,7 @@ void BNO080::getMag(float &x, float &y, float &z, uint8_t &accuracy) y = qToFloat(rawMagY, magnetometer_Q1); z = qToFloat(rawMagZ, magnetometer_Q1); accuracy = magAccuracy; + hasNewMag_=false; } //Return the magnetometer component @@ -781,6 +849,10 @@ uint8_t BNO080::getMagAccuracy() return (magAccuracy); } +bool BNO080::hasNewMag() { + return hasNewMag_; +} + //Gets the full high rate gyro vector //x,y,z output floats void BNO080::getFastGyro(float &x, float &y, float &z) @@ -811,6 +883,10 @@ float BNO080::getFastGyroZ() return (gyro); } +bool BNO080::hasNewFastGyro() { + return hasNewFastGyro_; +} + //Return the tap detector uint8_t BNO080::getTapDetector() { @@ -864,6 +940,29 @@ int16_t BNO080::getRawAccelZ() return (memsRawAccelZ); } +void BNO080::getRawAccel(int16_t &x, int16_t &y, int16_t &z, uint32_t &timeStamp) +{ + x = memsRawAccelX; + y = memsRawAccelY; + z = memsRawAccelZ; + timeStamp = memsAccelTimeStamp; + hasNewRawAccel_ = false; +} + +bool BNO080::getNewRawAccel(int16_t &x, int16_t &y, int16_t &z, uint32_t &timeStamp) +{ + if (hasNewRawAccel_) + { + getRawAccel(x, y, z, timeStamp); + return true; + } + return false; +} + +bool BNO080::hasNewRawAccel() { + return hasNewRawAccel_; +} + //Return raw mems value for the gyro int16_t BNO080::getRawGyroX() { @@ -878,6 +977,42 @@ int16_t BNO080::getRawGyroZ() return (memsRawGyroZ); } +// From https://github.com/ceva-dsp/sh2/issues/15 +// Raw gyro temperature for BNO085 uses BMI055 gyro +// memsRawGyroTemp is in 23°C + 0.5°C/LSB +float BNO080::getGyroTemp() +{ + return (23.0 + (0.5f * memsRawGyroTemp)); +} + +void BNO080::resetNewRawGyro() +{ + hasNewRawGyro_ = false; +} + +void BNO080::getRawGyro(int16_t &x, int16_t &y, int16_t &z, uint32_t &timeStamp) +{ + x = memsRawGyroX; + y = memsRawGyroY; + z = memsRawGyroZ; + timeStamp = memsGyroTimeStamp; + hasNewRawGyro_ = false; +} + +bool BNO080::getNewRawGyro(int16_t &x, int16_t &y, int16_t &z, uint32_t &timeStamp) +{ + if (hasNewRawGyro_) + { + getRawGyro(x, y, z, timeStamp); + return true; + } + return false; +} + +bool BNO080::hasNewRawGyro() { + return hasNewRawGyro_; +} + //Return raw mems value for the mag int16_t BNO080::getRawMagX() { @@ -892,6 +1027,29 @@ int16_t BNO080::getRawMagZ() return (memsRawMagZ); } +void BNO080::getRawMag(int16_t &x, int16_t &y, int16_t &z, uint32_t &timeStamp) +{ + x = memsRawMagX; + y = memsRawMagY; + z = memsRawMagZ; + timeStamp = memsMagTimeStamp; + hasNewRawMag_ = false; +} + +bool BNO080::getNewRawMag(int16_t &x, int16_t &y, int16_t &z, uint32_t &timeStamp) +{ + if (hasNewRawMag_) + { + getRawMag(x, y, z, timeStamp); + return true; + } + return false; +} + +bool BNO080::hasNewRawMag() { + return hasNewRawMag_; +} + //Given a record ID, read the Q1 value from the metaData record in the FRS (ya, it's complicated) //Q1 is used for all sensor data calculations int16_t BNO080::getQ1(uint16_t recordID) diff --git a/lib/bno080/BNO080.h b/lib/bno080/BNO080.h index e7f318c..eb31a99 100644 --- a/lib/bno080/BNO080.h +++ b/lib/bno080/BNO080.h @@ -201,8 +201,11 @@ public: bool hasNewGameQuat(); bool hasNewMagQuat(); void getQuat(float &i, float &j, float &k, float &real, float &radAccuracy, uint8_t &accuracy); + 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); 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(); float getQuatJ(); float getQuatK(); @@ -218,27 +221,32 @@ public: bool hasNewAccel(); void getLinAccel(float &x, float &y, float &z, uint8_t &accuracy); + bool getNewLinAccel(float &x, float &y, float &z, uint8_t &accuracy); float getLinAccelX(); float getLinAccelY(); float getLinAccelZ(); uint8_t getLinAccelAccuracy(); + bool hasNewLinAccel(); void getGyro(float &x, float &y, float &z, uint8_t &accuracy); float getGyroX(); float getGyroY(); float getGyroZ(); uint8_t getGyroAccuracy(); + bool hasNewGyro(); void getFastGyro(float &x, float &y, float &z); float getFastGyroX(); float getFastGyroY(); float getFastGyroZ(); + bool hasNewFastGyro(); void getMag(float &x, float &y, float &z, uint8_t &accuracy); float getMagX(); float getMagY(); float getMagZ(); uint8_t getMagAccuracy(); + bool hasNewMag(); void endCalibration(); void saveCalibration(); @@ -257,14 +265,25 @@ public: int16_t getRawAccelX(); int16_t getRawAccelY(); int16_t getRawAccelZ(); + void getRawAccel(int16_t &x, int16_t &y, int16_t &z, uint32_t &timeStamp); + bool getNewRawAccel(int16_t &x, int16_t &y, int16_t &z, uint32_t &timeStamp); + bool hasNewRawAccel(); int16_t getRawGyroX(); int16_t getRawGyroY(); int16_t getRawGyroZ(); + float getGyroTemp(); + void getRawGyro(int16_t &x, int16_t &y, int16_t &z, uint32_t &timeStamp); + bool getNewRawGyro(int16_t &x, int16_t &y, int16_t &z, uint32_t &timeStamp); + bool hasNewRawGyro(); + void resetNewRawGyro(); int16_t getRawMagX(); int16_t getRawMagY(); int16_t getRawMagZ(); + void getRawMag(int16_t &x, int16_t &y, int16_t &z, uint32_t &timeStamp); + bool getNewRawMag(int16_t &x, int16_t &y, int16_t &z, uint32_t &timeStamp); + bool hasNewRawMag(); float getRoll(); float getPitch(); @@ -326,7 +345,8 @@ private: uint16_t rawQuatI, rawQuatJ, rawQuatK, rawQuatReal, rawQuatRadianAccuracy, quatAccuracy; uint16_t rawGameQuatI, rawGameQuatJ, rawGameQuatK, rawGameQuatReal, quatGameAccuracy; uint16_t rawMagQuatI, rawMagQuatJ, rawMagQuatK, rawMagQuatReal, rawMagQuatRadianAccuracy, quatMagAccuracy; - bool hasNewQuaternion, hasNewGameQuaternion, hasNewMagQuaternion, hasNewAccel_; + bool hasNewQuaternion, hasNewGameQuaternion, hasNewMagQuaternion, hasNewAccel_, hasNewLinAccel_, hasNewFastGyro_; + bool hasNewMag_, hasNewGyro_; uint16_t rawFastGyroX, rawFastGyroY, rawFastGyroZ; uint8_t tapDetector; bool hasNewTap; @@ -337,8 +357,12 @@ private: uint8_t *_activityConfidences; //Array that store the confidences of the 9 possible activities uint8_t calibrationStatus; //Byte R0 of ME Calibration Response uint16_t memsRawAccelX, memsRawAccelY, memsRawAccelZ; //Raw readings from MEMS sensor - uint16_t memsRawGyroX, memsRawGyroY, memsRawGyroZ; //Raw readings from MEMS sensor + uint16_t memsRawGyroX, memsRawGyroY, memsRawGyroZ, memsRawGyroTemp; //Raw readings from MEMS sensor uint16_t memsRawMagX, memsRawMagY, memsRawMagZ; //Raw readings from MEMS sensor + uint32_t memsAccelTimeStamp, memsGyroTimeStamp, memsMagTimeStamp; //Timestamp of MEMS sensor reading + bool hasNewRawAccel_ = false; + bool hasNewRawGyro_= false; + bool hasNewRawMag_ = false; //These Q values are defined in the datasheet but can also be obtained by querying the meta data records //See the read metadata example for more info diff --git a/src/globals.h b/src/globals.h index 447e1f9..1a17abd 100644 --- a/src/globals.h +++ b/src/globals.h @@ -80,4 +80,9 @@ #define SENSOR_INFO_LIST #endif +// Experimental features +#ifndef EXPERIMENTAL_BNO_DISABLE_ACCEL_CALIBRATION +#define EXPERIMENTAL_BNO_DISABLE_ACCEL_CALIBRATION true +#endif + #endif // SLIMEVR_GLOBALS_H_ diff --git a/src/sensors/bno080sensor.cpp b/src/sensors/bno080sensor.cpp index 50d5bde..b34292c 100644 --- a/src/sensors/bno080sensor.cpp +++ b/src/sensors/bno080sensor.cpp @@ -85,6 +85,8 @@ void BNO080Sensor::motionSetup() { // EXPERIMENTAL Enable periodic calibration save to permanent memory imu.saveCalibrationPeriodically(true); imu.requestCalibrationStatus(); + +#if EXPERIMENTAL_BNO_DISABLE_ACCEL_CALIBRATION // EXPERIMENTAL Disable accelerometer calibration after 1 minute to prevent // "stomping" bug WARNING : Executing IMU commands outside of the update loop is not // allowed since the address might have changed when the timer is executed! @@ -119,10 +121,13 @@ void BNO080Sensor::motionSetup() { &imu ); } +#endif // imu.sendCalibrateCommand(SH2_CAL_ACCEL | SH2_CAL_GYRO_IN_HAND | SH2_CAL_MAG | // SH2_CAL_ON_TABLE | SH2_CAL_PLANAR); imu.enableStabilityClassifier(500); + // enableRawGyro only for reading the Temperature every 1 second (0.5°C steps) + imu.enableRawGyro(1000); lastReset = 0; lastData = millis(); @@ -137,43 +142,46 @@ void BNO080Sensor::motionLoop() { // Look for reports from the IMU while (imu.dataAvailable()) { hadData = true; + lastReset = 0; + lastData = millis(); + #if ENABLE_INSPECTION { - int16_t rX = imu.getRawGyroX(); - int16_t rY = imu.getRawGyroY(); - int16_t rZ = imu.getRawGyroZ(); - uint8_t rA = imu.getGyroAccuracy(); + if (imu.hasNewRawAccel() && imu.hasNewRawGyro() && imu.hasNewRawMag()) { + int16_t aX, aY, aZ, rX, rY, rZ, mX, mY, mZ; + uint32_t aTs, gTs, mTs; + // Accuracy estimates are not send by RAW Values + uint8_t rA = 0, aA = 0, mA = 0; - int16_t aX = imu.getRawAccelX(); - int16_t aY = imu.getRawAccelY(); - int16_t aZ = imu.getRawAccelZ(); - uint8_t aA = imu.getAccelAccuracy(); + imu.getRawAccel(aX, aY, aZ, aTs); + imu.getRawGyro(rX, rY, rZ, gTs); + imu.getRawMag(mX, mY, mZ, mTs); - int16_t mX = imu.getRawMagX(); - int16_t mY = imu.getRawMagY(); - int16_t mZ = imu.getRawMagZ(); - uint8_t mA = imu.getMagAccuracy(); + // only send Data when we have a set of new data - networkConnection.sendInspectionRawIMUData( - sensorId, - rX, - rY, - rZ, - rA, - aX, - aY, - aZ, - aA, - mX, - mY, - mZ, - mA - ); + networkConnection.sendInspectionRawIMUData( + sensorId, + rX, + rY, + rZ, + rA, + aX, + aY, + aZ, + aA, + mX, + mY, + mZ, + mA + ); + } } #endif - lastReset = 0; - lastData = millis(); + if (imu.hasNewRawGyro()) { + lastReadTemperature = imu.getGyroTemp(); + imu.resetNewRawGyro(); + } if (!toggles.getToggle(SensorToggles::MagEnabled)) { if (imu.hasNewGameQuat()) // New quaternion if context @@ -188,6 +196,7 @@ void BNO080Sensor::motionLoop() { ); setFusedRotation(nRotation); + continue; // Leave new quaternion if context open, it's closed later } } else { @@ -204,7 +213,7 @@ void BNO080Sensor::motionLoop() { ); setFusedRotation(nRotation); - + continue; // Leave new quaternion if context open, it's closed later } // Closing new quaternion if context } @@ -214,14 +223,19 @@ void BNO080Sensor::motionLoop() { { uint8_t acc; Vector3 nAccel; - imu.getLinAccel(nAccel.x, nAccel.y, nAccel.z, acc); - setAcceleration(nAccel); + // only send Accel if we have new data + if (imu.getNewLinAccel(nAccel.x, nAccel.y, nAccel.z, acc)) { + setAcceleration(nAccel); + continue; + } } #endif // SEND_ACCELERATION if (imu.getTapDetected()) { tap = imu.getTapDetector(); + continue; } + if (imu.hasNewCalibrationStatus()) { uint8_t calibrationResponseStatus; uint8_t accelCalEnabled; @@ -250,7 +264,9 @@ void BNO080Sensor::motionLoop() { // Default calibration flags for BNO085: // Accel: 1, Gyro: 0, Mag: 1, Planar: 0, OnTable: 0 (OnTable can't be // disabled) + continue; } + if (m_IntPin == nullptr || imu.I2CTimedOut()) { break; } @@ -334,12 +350,25 @@ void BNO080Sensor::sendData() { #endif } + sendTempIfNeeded(); + if (tap != 0) { networkConnection.sendSensorTap(sensorId, tap); tap = 0; } } +void BNO080Sensor::sendTempIfNeeded() { + uint32_t now = micros(); + constexpr float maxSendRateHz = 2.0f; + constexpr uint32_t sendInterval = 1.0f / maxSendRateHz * 1e6; + uint32_t elapsed = now - m_lastTemperaturePacketSent; + if (elapsed >= sendInterval) { + m_lastTemperaturePacketSent = now - (elapsed - sendInterval); + networkConnection.sendTemperature(sensorId, lastReadTemperature); + } +} + void BNO080Sensor::startCalibration(int calibrationType) { // BNO does automatic calibration, // it's always enabled except accelerometer diff --git a/src/sensors/bno080sensor.h b/src/sensors/bno080sensor.h index 1d864db..df1c6b9 100644 --- a/src/sensors/bno080sensor.h +++ b/src/sensors/bno080sensor.h @@ -61,6 +61,7 @@ public: void startCalibration(int calibrationType) override final; SensorStatus getSensorState() override final; bool isFlagSupported(SensorToggles toggle) const final; + void sendTempIfNeeded(); protected: // forwarding constructor @@ -94,6 +95,11 @@ private: float magneticAccuracyEstimate = 999; bool newMagData = false; bool configured = false; + + // Temperature reading + float lastReadTemperature = 0; + uint32_t lastTempPollTime = micros(); + uint32_t m_lastTemperaturePacketSent = 0; }; class BNO085Sensor : public BNO080Sensor { From bbba63e06cbe862c0e15637df84216cf0b2a71e6 Mon Sep 17 00:00:00 2001 From: jabberrock <130935387+jabberrock@users.noreply.github.com> Date: Sat, 19 Apr 2025 09:21:03 -0700 Subject: [PATCH 5/5] [ICM45*] Fix processing bad samples and stack overflow panics (#423) * [ICM45*] Fix processing bad samples and stack overflow panics 1. At startup, we were receiving invalid samples from the IMU and passing it to VQF. This causes huge initial rotations and accelerations that takes time for VQF to eliminate. Fix is to check the header to see if the data is present. Also had to add a check for invalid gyro samples even though it is present. 2. During play, the tracker would rarely go haywire and jump into a random position. I suspect this is caused by a stack overflow, which causes the tracker to panic. Combined with problem (1), whenever the tracker restarts, it would send huge rotations and accelerations to the server. This causes the jump. Fix is to make the read_buffer static, so that it's reserved on the BSS segment instead of the stack. I noticed this because I was getting panics when I tried to increase the size of the read buffer, or declared some new stack variables. After implementing these two fixes, rotation and acceleration are stable at startup, and I am able to make modifications to the icm45base code without randomly encountering panics. * Always read one fewer packet to prevent AN-000364 2.16 errata --- src/sensors/softfusion/drivers/icm45base.h | 66 +++++++++++++++++----- 1 file changed, 51 insertions(+), 15 deletions(-) diff --git a/src/sensors/softfusion/drivers/icm45base.h b/src/sensors/softfusion/drivers/icm45base.h index 106e079..907daf8 100644 --- a/src/sensors/softfusion/drivers/icm45base.h +++ b/src/sensors/softfusion/drivers/icm45base.h @@ -141,31 +141,67 @@ struct ICM45Base { GyroCall&& processGyroSample, TempCall&& processTemperatureSample ) { - const auto fifo_packets = i2c.readReg16(BaseRegs::FifoCount); - const auto fifo_bytes = (fifo_packets - 1) * FullFifoEntrySize; + // Allocate statically so that it does not take up stack space, which + // can result in stack overflow and panic + constexpr size_t MaxReadings = 8; + static std::array read_buffer; - std::array read_buffer; // max 8 readings - const auto bytes_to_read = std::min( - static_cast(read_buffer.size()), - static_cast(fifo_bytes) - ) - / FullFifoEntrySize * FullFifoEntrySize; + constexpr int16_t InvalidReading = -32768; + + size_t fifo_packets = i2c.readReg16(BaseRegs::FifoCount); + + if (fifo_packets >= 1) { + // + // AN-000364 + // 2.16 FIFO EMPTY EVENT IN STREAMING MODE CAN CORRUPT FIFO DATA + // + // Description: When in FIFO streaming mode, a FIFO empty event + // (caused by host reading the last byte of the last FIFO frame) can + // cause FIFO data corruption in the first FIFO frame that arrives + // after the FIFO empty condition. Once the issue is triggered, the + // FIFO state is compromised and cannot recover. FIFO must be set in + // bypass mode to flush out the wrong state + // + // When operating in FIFO streaming mode, if FIFO threshold + // interrupt is triggered with M number of FIFO frames accumulated + // in the FIFO buffer, the host should only read the first M-1 + // number of FIFO frames. This prevents the FIFO empty event, that + // can cause FIFO data corruption, from happening. + // + --fifo_packets; + } + + if (fifo_packets == 0) { + return; + } + + fifo_packets = std::min(fifo_packets, MaxReadings); + + size_t bytes_to_read = fifo_packets * FullFifoEntrySize; i2c.readBytes(BaseRegs::FifoData, bytes_to_read, read_buffer.data()); + for (auto i = 0u; i < bytes_to_read; i += FullFifoEntrySize) { + uint8_t header = read_buffer[i]; + bool has_gyro = header & (1 << 5); + bool has_accel = header & (1 << 6); + FifoEntryAligned entry; memcpy( &entry, &read_buffer[i + 0x1], sizeof(FifoEntryAligned) ); // skip fifo header - const int32_t gyroData[3]{ - static_cast(entry.gyro[0]) << 4 | (entry.lsb[0] & 0xf), - static_cast(entry.gyro[1]) << 4 | (entry.lsb[1] & 0xf), - static_cast(entry.gyro[2]) << 4 | (entry.lsb[2] & 0xf), - }; - processGyroSample(gyroData, GyrTs); - if (entry.accel[0] != -32768) { + if (has_gyro && entry.gyro[0] != InvalidReading) { + const int32_t gyroData[3]{ + static_cast(entry.gyro[0]) << 4 | (entry.lsb[0] & 0xf), + static_cast(entry.gyro[1]) << 4 | (entry.lsb[1] & 0xf), + static_cast(entry.gyro[2]) << 4 | (entry.lsb[2] & 0xf), + }; + processGyroSample(gyroData, GyrTs); + } + + if (has_accel && entry.accel[0] != InvalidReading) { const int32_t accelData[3]{ static_cast(entry.accel[0]) << 4 | (static_cast((entry.lsb[0]) & 0xf0) >> 4),