finish feature

SlimeVR · Jul 9, 2024 · 3da4427 · 3da4427
1 parent d2ca08a
commit 3da4427
Show file tree

Hide file tree

Showing 5 changed files with 40 additions and 32 deletions.
diff --git a/platformio.ini b/platformio.ini
@@ -50,10 +50,10 @@ build_unflags = -Os -std=gnu++11
 ; If you want to enable OTA Updates, uncomment and set OTA password here and in credentials.h
 ; You can set upload_port to device's ip after it's set up for the first time
 ; Use the same password in SlimeVR Server to let it OTA Update the device
-;upload_protocol = espota
-;upload_port = 192.168.1.49
-;upload_flags =
-;  --auth=SlimeVR-OTA
+upload_protocol = espota
+upload_port = 192.168.1.79
+upload_flags =
+  --auth=SlimeVR-OTA
 
 ; Settings for different boards
 

diff --git a/src/network/connection.cpp b/src/network/connection.cpp
@@ -298,6 +298,7 @@ void Connection::sendSensorInfo(Sensor* sensor) {
 	MUST(sendByte(sensor->getSensorId()));
 	MUST(sendByte((uint8_t)sensor->getSensorState()));
 	MUST(sendByte(sensor->getSensorType()));
+	MUST(sendByte((uint8_t)sensor->getMagStatus()));
 
 	MUST(endPacket());
 }
@@ -738,8 +739,11 @@ void Connection::update() {
 			uint8_t sensorId = m_Packet[12];
 			uint16_t flagId = m_Packet[13] << 8 | m_Packet[14];
 			bool newState = m_Packet[15] > 0;
-			if(sensorId == 255) {
-				// Apply the flag to the whole device
+			if(sensorId == UINT8_MAX) {
+				std::vector<Sensor *> & sensors = sensorManager.getSensors();
+				for (Sensor * sensor : sensors) {
+					sensor->setFlag(flagId, newState);
+				}
 			} else {
 				std::vector<Sensor *> & sensors = sensorManager.getSensors();
 				if(sensorId < sensors.size()) {
@@ -748,6 +752,10 @@ void Connection::update() {
 				}
 			}
 			sendAcknowledgeConfigChange(sensorId, flagId);
+			configuration.save();
+			// Should not be done always, but for magnetometer we probably prefer restarting
+			// the whole ESP
+			EspClass::restart();
 			break;
 		}
 	}

diff --git a/src/sensors/bno080sensor.cpp b/src/sensors/bno080sensor.cpp
@@ -58,14 +58,15 @@ void BNO080Sensor::motionSetup()
     switch (sensorConfig.type) {
     case SlimeVR::Configuration::SensorConfigType::BNO0XX:
         m_Config = sensorConfig.data.bno0XX;
-        m_magEnabled = m_Config.magEnabled;
+        magStatus = m_Config.magEnabled ? MagnetometerStatus::ENABLED : MagnetometerStatus::DISABLED;
         break;
     default:
         // Ignore lack of config for BNO, byt default use from FW build
+		magStatus = USE_6_AXIS ? MagnetometerStatus::DISABLED : MagnetometerStatus::ENABLED;
         break;
     }
 
-    if(!m_magEnabled) {
+    if(!isMagEnabled()) {
         if ((sensorType == IMU_BNO085 || sensorType == IMU_BNO086) && BNO_USE_ARVR_STABILIZATION) {
             imu.enableARVRStabilizedGameRotationVector(10);
         } else {
@@ -125,7 +126,7 @@ void BNO080Sensor::motionLoop()
         lastReset = 0;
         lastData = millis();
 
-        if(!m_magEnabled) {
+        if(!isMagEnabled()) {
                 if (imu.hasNewGameQuat()) // New quaternion if context
                 {
                     Quat nRotation;
@@ -233,11 +234,11 @@ void BNO080Sensor::sendData()
 #endif
     }
 
-    if(m_magEnabled) {
+    if(isMagEnabled()) {
         networkConnection.sendMagnetometerAccuracy(sensorId, magneticAccuracyEstimate);
     }
 
-    if(BNO_USE_MAGNETOMETER_CORRECTION && !m_magEnabled) {
+    if(BNO_USE_MAGNETOMETER_CORRECTION && !isMagEnabled()) {
         if (newMagData)
         {
             newMagData = false;
@@ -257,26 +258,13 @@ void BNO080Sensor::setFlag(uint16_t flagId, bool state)
 {
     if(flagId == FLAG_SENSOR_BNO0XX_MAG_ENABLED) {
         m_Config.magEnabled = state;
-        m_magEnabled = state;
-        if(state) {
-            if ((sensorType == IMU_BNO085 || sensorType == IMU_BNO086) && BNO_USE_ARVR_STABILIZATION) {
-                imu.enableARVRStabilizedRotationVector(10);
-            } else {
-                imu.enableRotationVector(10);
-            }
-        } else {
-            if ((sensorType == IMU_BNO085 || sensorType == IMU_BNO086) && BNO_USE_ARVR_STABILIZATION) {
-                imu.enableARVRStabilizedGameRotationVector(10);
-            } else {
-                imu.enableGameRotationVector(10);
-            }
-
-            #if BNO_USE_MAGNETOMETER_CORRECTION
-            imu.enableRotationVector(1000);
-            #endif
-        }
-        imu.softReset();
-    }
+        magStatus = state ? MagnetometerStatus::ENABLED : MagnetometerStatus::DISABLED;
+
+		SlimeVR::Configuration::SensorConfig config;
+		config.type = SlimeVR::Configuration::SensorConfigType::BNO0XX;
+		config.data.bno0XX = m_Config;
+		configuration.setSensor(sensorId, config);
+	}
 }
 
 void BNO080Sensor::startCalibration(int calibrationType)

diff --git a/src/sensors/bno080sensor.h b/src/sensors/bno080sensor.h
@@ -54,7 +54,6 @@ class BNO080Sensor : public Sensor
     unsigned long lastData = 0;
     uint8_t lastReset = 0;
     BNO080Error lastError{};
-    bool m_magEnabled = !USE_6_AXIS;
     SlimeVR::Configuration::BNO0XXSensorConfig m_Config = {};
 
     // Magnetometer specific members

diff --git a/src/sensors/sensor.h b/src/sensors/sensor.h
@@ -41,6 +41,12 @@ enum class SensorStatus : uint8_t {
     SENSOR_ERROR = 2
 };
 
+enum class MagnetometerStatus : uint8_t {
+	NOT_SUPPORTED = 0,
+	DISABLED = 1,
+	ENABLED = 2,
+};
+
 class Sensor
 {
 public:
@@ -73,12 +79,18 @@ class Sensor
     bool isValid() {
         return sclPin != sdaPin;
     };
+	bool isMagEnabled() {
+		return magStatus == MagnetometerStatus::ENABLED;
+	}
     uint8_t getSensorId() {
         return sensorId;
     };
     uint8_t getSensorType() {
         return sensorType;
     };
+	MagnetometerStatus getMagStatus() {
+		return magStatus;
+	};
     const Vector3& getAcceleration() {
         return acceleration;
     };
@@ -97,6 +109,7 @@ class Sensor
     bool configured = false;
     bool working = false;
     uint8_t calibrationAccuracy = 0;
+	MagnetometerStatus magStatus = MagnetometerStatus::NOT_SUPPORTED;
     Quat sensorOffset;
 
     bool newFusedRotation = false;