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 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 816e920..488f8d7 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/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_ 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/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() { 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 { 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),