Merge branch 'main' into ads111x-implementation

.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

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)

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

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();
     }
 }

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_

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_

src/network/connection.cpp

@@ -514,9 +514,11 @@ void Connection::maybeRequestFeatureFlags() {
 }
 
 bool Connection::isSensorStateUpdated(int i, std::unique_ptr<Sensor>& 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() {

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

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 {

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<uint8_t, FullFifoEntrySize * MaxReadings> read_buffer;
 
-		std::array<uint8_t, FullFifoEntrySize * 8> read_buffer;  // max 8 readings
-		const auto bytes_to_read = std::min(
-									   static_cast<size_t>(read_buffer.size()),
-									   static_cast<size_t>(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<int32_t>(entry.gyro[0]) << 4 | (entry.lsb[0] & 0xf),
-				static_cast<int32_t>(entry.gyro[1]) << 4 | (entry.lsb[1] & 0xf),
-				static_cast<int32_t>(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<int32_t>(entry.gyro[0]) << 4 | (entry.lsb[0] & 0xf),
+					static_cast<int32_t>(entry.gyro[1]) << 4 | (entry.lsb[1] & 0xf),
+					static_cast<int32_t>(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<int32_t>(entry.accel[0]) << 4
 						| (static_cast<int32_t>((entry.lsb[0]) & 0xf0) >> 4),