Report rest calibration

src/network/connection.cpp

@@ -297,6 +297,7 @@ void Connection::sendSensorInfo(Sensor& sensor) {
 	MUST(sendByte(static_cast<uint8_t>(sensor.getSensorState())));
 	MUST(sendByte(static_cast<uint8_t>(sensor.getSensorType())));
 	MUST(sendShort(sensor.getSensorConfigData()));
+	MUST(sendByte(sensor.completedRestCalibration()));
 
 	MUST(endPacket());
 }
@@ -533,7 +534,7 @@ void Connection::updateSensorState(std::vector<std::unique_ptr<Sensor>>& sensors
 	m_LastSensorInfoPacketTimestamp = millis();
 
 	for (int i = 0; i < (int)sensors.size(); i++) {
-		if (m_AckedSensorState[i] != sensors[i]->getSensorState()) {
+		if (isSensorStateUpdated(i, sensors[i])) {
 			sendSensorInfo(*sensors[i]);
 		}
 	}
@@ -553,6 +554,11 @@ void Connection::maybeRequestFeatureFlags() {
 	m_FeatureFlagsRequestAttempts++;
 }
 
+bool Connection::isSensorStateUpdated(int i, std::unique_ptr<Sensor>& sensor) {
+	return m_AckedSensorState[i] != sensor->getSensorState()
+		|| m_AckedSensorCalibration[i] != sensor->completedRestCalibration();
+}
+
 void Connection::searchForServer() {
 	while (true) {
 		int packetSize = m_UDP.parsePacket();
@@ -624,6 +630,11 @@ void Connection::reset() {
 		m_AckedSensorState + MAX_IMU_COUNT,
 		SensorStatus::SENSOR_OFFLINE
 	);
+	std::fill(
+		m_AckedSensorCalibration,
+		m_AckedSensorCalibration + MAX_IMU_COUNT,
+		false
+	);
 
 	m_UDP.begin(m_ServerPort);
 
@@ -650,6 +661,11 @@ void Connection::update() {
 			m_AckedSensorState + MAX_IMU_COUNT,
 			SensorStatus::SENSOR_OFFLINE
 		);
+		std::fill(
+			m_AckedSensorCalibration,
+			m_AckedSensorCalibration + MAX_IMU_COUNT,
+			false
+		);
 		m_Logger.warn("Connection to server timed out");
 
 		return;
@@ -707,6 +723,7 @@ void Connection::update() {
 			for (int i = 0; i < (int)sensors.size(); i++) {
 				if (m_Packet[4] == sensors[i]->getSensorId()) {
 					m_AckedSensorState[i] = (SensorStatus)m_Packet[5];
+					m_AckedSensorCalibration[i] = (bool)m_Packet[9];
 					break;
 				}
 			}

src/network/connection.h

@@ -125,6 +125,7 @@ class Connection {
 private:
 	void updateSensorState(std::vector<std::unique_ptr<Sensor>>& sensors);
 	void maybeRequestFeatureFlags();
+	bool isSensorStateUpdated(int i, std::unique_ptr<Sensor>& sensor);
 
 	bool beginPacket();
 	bool endPacket();
@@ -171,6 +172,7 @@ class Connection {
 	unsigned long m_LastPacketTimestamp;
 
 	SensorStatus m_AckedSensorState[MAX_IMU_COUNT] = {SensorStatus::SENSOR_OFFLINE};
+	bool m_AckedSensorCalibration[MAX_IMU_COUNT] = {false};
 	unsigned long m_LastSensorInfoPacketTimestamp = 0;
 
 	uint8_t m_FeatureFlagsRequestAttempts = 0;

src/sensors/RestCalibrationDetector.cpp

@@ -0,0 +1,53 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2025 Gorbit99 & SlimeVR Contributors
+
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
+
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
+
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
+*/
+
+#include "RestCalibrationDetector.h"
+
+namespace SlimeVR::Sensors {
+
+bool RestCalibrationDetector::update(SensorFusionRestDetect& fusion) {
+	if (state == CalibrationState::Done) {
+		return false;
+	}
+
+	if (!fusion.getRestDetected()) {
+		state = CalibrationState::NoRest;
+		return false;
+	}
+
+	if (state == CalibrationState::NoRest) {
+		state = CalibrationState::Calibrating;
+		lastRestStartedMillis = millis();
+		return false;
+	}
+
+	uint32_t elapsed = millis() - lastRestStartedMillis;
+	if (elapsed < static_cast<uint32_t>(restCalibrationSeconds * 1e3)) {
+		return false;
+	}
+
+	state = CalibrationState::Done;
+	return true;
+}
+
+}  // namespace SlimeVR::Sensors

src/sensors/RestCalibrationDetector.h

@@ -0,0 +1,49 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2025 Gorbit99 & SlimeVR Contributors
+
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
+
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
+
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
+*/
+
+#pragma once
+
+#include <cstdint>
+
+#include "SensorFusionRestDetect.h"
+
+namespace SlimeVR::Sensors {
+
+class RestCalibrationDetector {
+public:
+	bool update(SensorFusionRestDetect& fusion);
+
+private:
+	static constexpr float restCalibrationSeconds = 3.0f;
+
+	enum class CalibrationState {
+		NoRest,
+		Calibrating,
+		Done,
+	};
+
+	CalibrationState state = CalibrationState::NoRest;
+	uint32_t lastRestStartedMillis = 0;
+};
+
+}  // namespace SlimeVR::Sensors

src/sensors/bmi160sensor.cpp

@@ -307,6 +307,10 @@ void BMI160Sensor::motionLoop() {
 	}
 #endif
 
+	if (calibrationDetector.update(sfusion)) {
+		markRestCalibrationComplete();
+	}
+
 	{
 		uint32_t now = micros();
 		constexpr uint32_t BMI160_TARGET_SYNC_INTERVAL_MICROS = 25000;

src/sensors/bmi160sensor.h

@@ -29,6 +29,7 @@
 #include "../motionprocessing/GyroTemperatureCalibrator.h"
 #include "../motionprocessing/RestDetection.h"
 #include "../motionprocessing/types.h"
+#include "RestCalibrationDetector.h"
 #include "SensorFusionRestDetect.h"
 #include "magneto1.4.h"
 #include "sensor.h"
@@ -265,6 +266,8 @@ class BMI160Sensor : public Sensor {
 	bool isMagCalibrated = false;
 
 	SlimeVR::Configuration::BMI160SensorConfig m_Config = {};
+
+	SlimeVR::Sensors::RestCalibrationDetector calibrationDetector;
 };
 
 #endif

src/sensors/bno055sensor.cpp

@@ -22,6 +22,8 @@
 */
 #include "bno055sensor.h"
 
+#include <cstdint>
+
 #include "GlobalVars.h"
 #include "globals.h"
 
@@ -68,6 +70,12 @@ void BNO055Sensor::motionLoop() {
 	}
 #endif
 
+	uint8_t gyroCalibrationState;
+	imu.getCalibration(nullptr, &gyroCalibrationState, nullptr, nullptr);
+	if (gyroCalibrationState == 3) {
+		markRestCalibrationComplete();
+	}
+
 	// TODO Optimize a bit with setting rawQuat directly
 	setFusedRotation(imu.getQuat());
 	hadData = true;

src/sensors/bno080sensor.cpp

@@ -97,6 +97,8 @@ void BNO080Sensor::motionSetup() {
 	imu.enableRawMagnetometer(10);
 #endif
 
+	imu.enableStabilityClassifier(500);
+
 	lastReset = 0;
 	lastData = millis();
 	working = true;
@@ -238,6 +240,10 @@ void BNO080Sensor::motionLoop() {
 			lastError.error_code
 		);
 	}
+
+	if (imu.getStabilityClassifier() == 1) {
+		markRestCalibrationComplete();
+	}
 }
 
 SensorStatus BNO080Sensor::getSensorState() {

src/sensors/sensor.cpp

@@ -138,3 +138,9 @@ const char* getIMUNameByType(ImuID imuType) {
 	}
 	return "Unknown";
 }
+
+bool Sensor::completedRestCalibration() { return restCalibrationComplete; }
+
+void Sensor::markRestCalibrationComplete(bool completed) {
+	restCalibrationComplete = completed;
+}

src/sensors/sensor.h

@@ -96,6 +96,7 @@ class Sensor {
 	const Vector3& getAcceleration() { return acceleration; };
 	const Quat& getFusedRotation() { return fusedRotation; };
 	bool hasNewDataToSend() { return newFusedRotation || newAcceleration; };
+	bool completedRestCalibration();
 
 protected:
 	uint8_t addr = 0;
@@ -114,6 +115,8 @@ class Sensor {
 	bool newAcceleration = false;
 	Vector3 acceleration{};
 
+	void markRestCalibrationComplete(bool completed = true);
+
 	mutable SlimeVR::Logging::Logger m_Logger;
 
 public:
@@ -122,6 +125,8 @@ class Sensor {
 
 private:
 	void printTemperatureCalibrationUnsupported();
+
+	bool restCalibrationComplete;
 };
 
 const char* getIMUNameByType(ImuID imuType);

src/sensors/softfusion/softfusionsensor.h

@@ -23,6 +23,7 @@
 
 #pragma once
 
+#include "../RestCalibrationDetector.h"
 #include "../SensorFusionRestDetect.h"
 #include "../sensor.h"
 #include "GlobalVars.h"
@@ -236,6 +237,10 @@ class SoftFusionSensor : public Sensor {
 			setAcceleration(m_fusion.getLinearAccVec());
 			optimistic_yield(100);
 		}
+
+		if (calibrationDetector.update(m_fusion)) {
+			markRestCalibrationComplete();
+		}
 	}
 
 	void motionSetup() override final {
@@ -617,6 +622,8 @@ class SoftFusionSensor : public Sensor {
 	uint32_t m_lastPollTime = micros();
 	uint32_t m_lastRotationPacketSent = 0;
 	uint32_t m_lastTemperaturePacketSent = 0;
+
+	RestCalibrationDetector calibrationDetector;
 };
 
 }  // namespace SlimeVR::Sensors