Merge branch 'SlimeVR:main' into main

SlimeVR · Mar 8, 2024 · 81a652b · 81a652b
2 parents f574956 + 993d35a
commit 81a652b
Show file tree

Hide file tree

Showing 14 changed files with 85 additions and 56 deletions.
diff --git a/README.md b/README.md
@@ -121,13 +121,6 @@ Firmware can work with both ESP8266 and ESP32. Please edit `defines.h` and set y
 
   - Calibration data is written to the flash of your MCU and is unique for each BMI160, keep that in mind if you have detachable aux trackers.
 
-## Infos about ESP32-C3 with direct connection to USB
-
-The ESP32-C3 has two ways to connect the serial port. One is directly via the onboard USB CDC or via the onboard UART.
-When the chip is connected to the USB CDC, the serial port shows as `USB Serial Port` in Device Manager. The SlimeVR server will currently not connect to this port.
-If you want to set your WiFi credentials, you can use the PlatformIO serial console.
-There you have to enter the following: `SET WIFI "SSID" "PASSWORD"`
-
 ## Uploading On Linux
 
 Follow the instructions in this link [PlatformIO](https://docs.platformio.org/en/latest//faq.html#platformio-udev-rules), this should solve any permission denied errors

diff --git a/platformio-tools.ini b/platformio-tools.ini
@@ -64,3 +64,10 @@ build_flags =
   ${env.build_flags}
   -DESP32C3
 board = esp32-c3-devkitm-1
+
+[env:BOARD_XIAO_ESP32C3]
+platform = espressif32 @ 6.1.0
+build_flags = 
+  ${env.build_flags}
+  -DESP32C3
+board = seeed_xiao_esp32c3
diff --git a/platformio.ini b/platformio.ini
@@ -35,6 +35,9 @@ build_flags =
 ;  -DWIFI_STATIC_GATEWAY=192,168,XXX,XXX
 ;  -DWIFI_STATIC_SUBNET=255,255,255,0
 
+;  -DSERVER_IP='"192.168.XXX.XXX"'
+;  -DSERVER_PORT=6969
+
 ; Uncomment below if your board are using 40MHz crystal instead of 26MHz for ESP8266
 ;  -DF_CRYSTAL=40000000
 
@@ -93,7 +96,7 @@ upload_speed = 921600
 
 ;[env:esp32c3]
 ;platform = espressif32 @ 6.1.0
-;build_flags = 
+;build_flags =
 ;  ${env.build_flags}
 ;  -DESP32C3
 ;board = lolin_c3_mini
diff --git a/src/batterymonitor.cpp b/src/batterymonitor.cpp
@@ -77,8 +77,11 @@ void BatteryMonitor::Loop()
                     }
                 }
             #endif
-            #if BATTERY_MONITOR == BAT_EXTERNAL
-                voltage = ((float)analogRead(PIN_BATTERY_LEVEL)) * batteryADCMultiplier;
+            #if ESP8266 && BATTERY_MONITOR == BAT_EXTERNAL
+                voltage = ((float)analogRead(PIN_BATTERY_LEVEL)) * ADCVoltageMax / ADCResolution * ADCMultiplier;
+            #endif
+            #if ESP32 && BATTERY_MONITOR == BAT_EXTERNAL
+                voltage = ((float)analogReadMilliVolts(PIN_BATTERY_LEVEL)) / 1000 * ADCMultiplier;
             #endif
             #if BATTERY_MONITOR == BAT_MCP3021 || BATTERY_MONITOR == BAT_INTERNAL_MCP3021
                 if (address > 0)
@@ -91,7 +94,7 @@ void BatteryMonitor::Loop()
                     if (status == 0)
                     {
                         float v = (((uint16_t)(MSB & 0x0F) << 6) | (uint16_t)(LSB >> 2));
-                        v *= batteryADCMultiplier;
+                        v *= ADCMultiplier;
                         voltage = (voltage > 0) ? min(voltage, v) : v;
                     }
                 }

diff --git a/src/batterymonitor.h b/src/batterymonitor.h
@@ -30,15 +30,11 @@
 #include "logging/Logger.h"
 
 #if ESP8266
-    #define ADCResulution 1023.0  // ESP8266 has 12bit ADC
+    #define ADCResolution 1023.0  // ESP8266 has 10bit ADC
     #define ADCVoltageMax 1.0     // ESP8266 input is 1.0 V = 1023.0
 #endif
-#if ESP32
-    #define ADCResulution 4095.0  // ESP32 has 12bit ADC
-    #define ADCVoltageMax 3.3     // ESP32 input is 3.3 V = 4095.0
-#endif
-#ifndef ADCResulution
-    #define ADCResulution 1023.0
+#ifndef ADCResolution
+    #define ADCResolution 1023.0
 #endif
 #ifndef ADCVoltageMax
     #define ADCVoltageMax 1.0
@@ -64,10 +60,10 @@
     // Diagramm:
     //   (Battery)--- [BATTERY_SHIELD_RESISTANCE] ---(INPUT_BOARD)---  [BATTERY_SHIELD_R2] ---(ESP_INPUT)--- [BATTERY_SHIELD_R1] --- (GND)
     // SlimeVR Board can handle max 5V > so analogRead of 5.0V input will result in 1023.0
-  #define batteryADCMultiplier ADCVoltageMax / ADCResulution * (BATTERY_SHIELD_R1 + BATTERY_SHIELD_R2 + BATTERY_SHIELD_RESISTANCE) / BATTERY_SHIELD_R1
+  #define ADCMultiplier (BATTERY_SHIELD_R1 + BATTERY_SHIELD_R2 + BATTERY_SHIELD_RESISTANCE) / BATTERY_SHIELD_R1
 #elif BATTERY_MONITOR == BAT_MCP3021 || BATTERY_MONITOR == BAT_INTERNAL_MCP3021
   // Default recommended resistors are 9.1k and 5.1k
-  #define batteryADCMultiplier 3.3 / 1023.0 * 14.2 / 9.1
+  #define ADCMultiplier 3.3 / 1023.0 * 14.2 / 9.1
 #endif
 
 class BatteryMonitor

diff --git a/src/consts.h b/src/consts.h
@@ -55,6 +55,7 @@
 #define BOARD_WRANGLER 14 // Only used by wrangler app
 #define BOARD_MOCOPI 15 // Used by mocopi/moslime
 #define BOARD_WEMOSWROOM02 16
+#define BOARD_XIAO_ESP32C3 17
 #define BOARD_DEV_RESERVED 250 // Reserved, should not be used in any release firmware
 
 #define BAT_EXTERNAL 1

diff --git a/src/defines.h b/src/defines.h
@@ -43,7 +43,7 @@
 #define BMI160_QMC_REMAP AXIS_REMAP_BUILD(AXIS_REMAP_USE_Y, AXIS_REMAP_USE_XN, AXIS_REMAP_USE_Z, \
                                        AXIS_REMAP_USE_YN, AXIS_REMAP_USE_X, AXIS_REMAP_USE_Z)
 
-IMU_DESC_ENTRY(IMU_BMP160, PRIMARY_IMU_ADDRESS_ONE, IMU_ROTATION, PIN_IMU_SCL, PIN_IMU_SDA, BMI160_QMC_REMAP) \
+IMU_DESC_ENTRY(IMU_BMP160, PRIMARY_IMU_ADDRESS_ONE, IMU_ROTATION, PIN_IMU_SCL, PIN_IMU_SDA, PRIMARY_IMU_OPTIONAL, BMI160_QMC_REMAP) \
 */
 
 #ifndef IMU_DESC_LIST
@@ -53,8 +53,8 @@ IMU_DESC_ENTRY(IMU_BMP160, PRIMARY_IMU_ADDRESS_ONE, IMU_ROTATION, PIN_IMU_SCL, P
 #endif
 
 // Battery monitoring options (comment to disable):
-//   BAT_EXTERNAL for ADC pin, 
-//   BAT_INTERNAL for internal - can detect only low battery, 
+//   BAT_EXTERNAL for ADC pin,
+//   BAT_INTERNAL for internal - can detect only low battery,
 //   BAT_MCP3021 for external ADC connected over I2C
 #define BATTERY_MONITOR BAT_EXTERNAL
 
@@ -75,7 +75,7 @@ IMU_DESC_ENTRY(IMU_BMP160, PRIMARY_IMU_ADDRESS_ONE, IMU_ROTATION, PIN_IMU_SCL, P
 //   LED_PIN
 //     - Number or Symbol (D1,..) of the Output
 //     - To turn off the LED, set LED_PIN to LED_OFF
-//   LED_INVERTED 
+//   LED_INVERTED
 //     - false for output 3.3V on high
 //     - true for pull down to GND on high
 
@@ -91,7 +91,7 @@ IMU_DESC_ENTRY(IMU_BMP160, PRIMARY_IMU_ADDRESS_ONE, IMU_ROTATION, PIN_IMU_SCL, P
   #ifndef BATTERY_SHIELD_RESISTANCE
     #define BATTERY_SHIELD_RESISTANCE 0
   #endif
-  #ifndef BATTERY_SHIELD_R1 
+  #ifndef BATTERY_SHIELD_R1
     #define BATTERY_SHIELD_R1 10
   #endif
   #ifndef BATTERY_SHIELD_R2
@@ -108,7 +108,7 @@ IMU_DESC_ENTRY(IMU_BMP160, PRIMARY_IMU_ADDRESS_ONE, IMU_ROTATION, PIN_IMU_SCL, P
   #ifndef BATTERY_SHIELD_RESISTANCE
     #define BATTERY_SHIELD_RESISTANCE 0
   #endif
-  #ifndef BATTERY_SHIELD_R1 
+  #ifndef BATTERY_SHIELD_R1
     #define BATTERY_SHIELD_R1 10
   #endif
   #ifndef BATTERY_SHIELD_R2
@@ -125,7 +125,7 @@ IMU_DESC_ENTRY(IMU_BMP160, PRIMARY_IMU_ADDRESS_ONE, IMU_ROTATION, PIN_IMU_SCL, P
   #ifndef BATTERY_SHIELD_RESISTANCE
     #define BATTERY_SHIELD_RESISTANCE 180
   #endif
-  #ifndef BATTERY_SHIELD_R1 
+  #ifndef BATTERY_SHIELD_R1
     #define BATTERY_SHIELD_R1 100
   #endif
   #ifndef BATTERY_SHIELD_R2
@@ -189,4 +189,21 @@ IMU_DESC_ENTRY(IMU_BMP160, PRIMARY_IMU_ADDRESS_ONE, IMU_ROTATION, PIN_IMU_SCL, P
   #define PIN_BATTERY_LEVEL A0
   #define LED_PIN 16
   #define LED_INVERTED true
+#elif BOARD == BOARD_XIAO_ESP32C3
+  #define PIN_IMU_SDA 6 // D4
+  #define PIN_IMU_SCL 7 // D5
+  #define PIN_IMU_INT 5 // D3
+  #define PIN_IMU_INT_2 10 // D10
+  #define LED_PIN 4 // D2
+  #define LED_INVERTED false
+  #define PIN_BATTERY_LEVEL 2 // D0 / A0
+  #ifndef BATTERY_SHIELD_RESISTANCE
+    #define BATTERY_SHIELD_RESISTANCE 0
+  #endif
+  #ifndef BATTERY_SHIELD_R1
+    #define BATTERY_SHIELD_R1 100
+  #endif
+  #ifndef BATTERY_SHIELD_R2
+    #define BATTERY_SHIELD_R2 100
+  #endif
 #endif
diff --git a/src/motionprocessing/RestDetection.h b/src/motionprocessing/RestDetection.h
@@ -20,14 +20,14 @@
 struct RestDetectionParams {
     sensor_real_t biasClip;
     sensor_real_t biasSigmaRest;
-    uint32_t restMinTimeMicros;
+    sensor_real_t restMinTime;
     sensor_real_t restFilterTau;
     sensor_real_t restThGyr;
     sensor_real_t restThAcc;
     RestDetectionParams():
         biasClip(2.0f),
         biasSigmaRest(0.03f),
-        restMinTimeMicros(1.5 * 1e6),
+        restMinTime(1.5),
         restFilterTau(0.5f),
         restThGyr(2.0f),
         restThAcc(0.5f)
@@ -103,7 +103,7 @@ class RestDetection {
     }
 #endif
 
-    void updateGyr(uint32_t dtMicros, sensor_real_t gyr[3]) {
+    void updateGyr(sensor_real_t gyr[3]) {
 #ifdef REST_DETECTION_DISABLE_LPF
         gyrLastSquaredDeviation =
             square(gyr[0] - lastSample.gyr[0]) +
@@ -114,7 +114,7 @@ class RestDetection {
         if (gyrLastSquaredDeviation >= square(params.restThGyr*sensor_real_t(M_PI/180.0))
                 || fabs(lastSample.gyr[0]) > biasClip || fabs(lastSample.gyr[1]) > biasClip
                 || fabs(lastSample.gyr[2]) > biasClip) {
-            restTimeMicros = 0;
+            restTime = 0;
             restDetected = false;
         }
 
@@ -134,13 +134,13 @@ class RestDetection {
         if (gyrLastSquaredDeviation >= square(params.restThGyr*sensor_real_t(M_PI/180.0))
                 || fabs(restLastGyrLp[0]) > biasClip || fabs(restLastGyrLp[1]) > biasClip
                 || fabs(restLastGyrLp[2]) > biasClip) {
-            restTimeMicros = 0;
+            restTime = 0;
             restDetected = false;
         }
 #endif
     }
 
-    void updateAcc(uint32_t dtMicros, sensor_real_t acc[3]) {
+    void updateAcc(sensor_real_t dt, sensor_real_t acc[3]) {
         if (acc[0] == sensor_real_t(0.0) && acc[1] == sensor_real_t(0.0) && acc[2] == sensor_real_t(0.0)) {
             return;
         }
@@ -152,11 +152,11 @@ class RestDetection {
             square(acc[2] - lastSample.acc[2]);
 
         if (accLastSquaredDeviation >= square(params.restThAcc)) {
-            restTimeMicros = 0;
+            restTime = 0;
             restDetected = false;
         } else {
-            restTimeMicros += dtMicros;
-            if (restTimeMicros >= params.restMinTimeMicros) {
+            restTime += dt;
+            if (restTime >= params.restMinTime) {
                 restDetected = true;
             }
         }
@@ -174,11 +174,11 @@ class RestDetection {
             square(acc[2] - restLastAccLp[2]);
 
         if (accLastSquaredDeviation >= square(params.restThAcc)) {
-            restTimeMicros = 0;
+            restTime = 0;
             restDetected = false;
         } else {
-            restTimeMicros += dtMicros;
-            if (restTimeMicros >= params.restMinTimeMicros) {
+            restTime += dt;
+            if (restTime >= params.restMinTime) {
                 restDetected = true;
             }
         }
@@ -195,7 +195,7 @@ class RestDetection {
 
         gyrLastSquaredDeviation = 0.0;
         accLastSquaredDeviation = 0.0;
-        restTimeMicros = 0.0;
+        restTime = 0.0;
         std::fill(restLastGyrLp, restLastGyrLp + 3, 0.0);
         std::fill(restGyrLpState, restGyrLpState + 3*2, NaN);
         std::fill(restLastAccLp, restLastAccLp + 3, 0.0);
@@ -235,7 +235,7 @@ class RestDetection {
 private:
     RestDetectionParams params;
     bool restDetected;
-    uint32_t restTimeMicros;
+    sensor_real_t restTime;
     sensor_real_t gyrLastSquaredDeviation = 0;
     sensor_real_t accLastSquaredDeviation = 0;
 

diff --git a/src/network/connection.h b/src/network/connection.h
@@ -37,6 +37,16 @@ namespace Network {
 
 class Connection {
 public:
+	Connection() {
+#ifdef SERVER_IP
+		m_ServerHost.fromString(SERVER_IP);
+#endif
+
+#ifdef SERVER_PORT
+		m_ServerPort = SERVER_PORT;
+#endif
+	}
+
 	void searchForServer();
 	void update();
 	void reset();

diff --git a/src/sensors/SensorFusionRestDetect.cpp b/src/sensors/SensorFusionRestDetect.cpp
@@ -15,7 +15,7 @@ namespace SlimeVR
         void SensorFusionRestDetect::updateGyro(sensor_real_t Gxyz[3], sensor_real_t deltat)
         {
             if (deltat < 0) deltat = gyrTs;
-            restDetection.updateGyr(deltat, Gxyz);
+            restDetection.updateGyr(Gxyz);
             SensorFusion::updateGyro(Gxyz, deltat);
         }
         #endif

diff --git a/src/sensors/SensorFusionRestDetect.h b/src/sensors/SensorFusionRestDetect.h
@@ -18,7 +18,7 @@ namespace SlimeVR
         #if !SENSOR_FUSION_WITH_RESTDETECT
         struct SensorRestDetectionParams: RestDetectionParams {
             SensorRestDetectionParams() : RestDetectionParams() {
-                restMinTimeMicros = 2.0f * 1e6;
+                restMinTime = 2.0f;
                 restThGyr = 0.6f; // 400 norm
                 restThAcc = 0.06f; // 100 norm
             }

diff --git a/src/sensors/bmi160sensor.cpp b/src/sensors/bmi160sensor.cpp
@@ -530,9 +530,9 @@ void BMI160Sensor::onGyroRawSample(uint32_t dtMicros, int16_t x, int16_t y, int1
     }
     remapGyroAccel(&Gxyz[0], &Gxyz[1], &Gxyz[2]);
 
-    sfusion.updateGyro(Gxyz, (double)dtMicros * 1.0e-6);
+	sfusion.updateGyro(Gxyz, (sensor_real_t)dtMicros * 1.0e-6);
 
-    optimistic_yield(100);
+	optimistic_yield(100);
 }
 void BMI160Sensor::onAccelRawSample(uint32_t dtMicros, int16_t x, int16_t y, int16_t z) {
     #if BMI160_DEBUG
@@ -548,9 +548,9 @@ void BMI160Sensor::onAccelRawSample(uint32_t dtMicros, int16_t x, int16_t y, int
     lastAxyz[1] = Axyz[1];
     lastAxyz[2] = Axyz[2];
 
-    sfusion.updateAcc(Axyz, dtMicros);
+	sfusion.updateAcc(Axyz, (sensor_real_t)dtMicros * 1.0e-6);
 
-    optimistic_yield(100);
+	optimistic_yield(100);
 }
 void BMI160Sensor::onMagRawSample(uint32_t dtMicros, int16_t x, int16_t y, int16_t z) {
     #if BMI160_DEBUG
@@ -825,12 +825,12 @@ void BMI160Sensor::maybeCalibrateAccel() {
         m_Logger.debug("Calculating accelerometer calibration data...");
     #elif BMI160_ACCEL_CALIBRATION_METHOD == ACCEL_CALIBRATION_METHOD_6POINT
         RestDetectionParams calibrationRestDetectionParams;
-        calibrationRestDetectionParams.restMinTimeMicros = 3 * 1e6;
+        calibrationRestDetectionParams.restMinTime = 3;
         calibrationRestDetectionParams.restThAcc = 0.25f;
         RestDetection calibrationRestDetection(
             calibrationRestDetectionParams,
-            BMI160_ODR_GYR_MICROS / 1e6f,
-            BMI160_ODR_ACC_MICROS / 1e6f
+            BMI160_ODR_GYR_MICROS * 1.0e-6,
+            BMI160_ODR_ACC_MICROS * 1.0e-6
         );
 
         constexpr uint16_t expectedPositions = 6;
@@ -853,9 +853,9 @@ void BMI160Sensor::maybeCalibrateAccel() {
             scaled[1] = ay * BMI160_ASCALE;
             scaled[2] = az * BMI160_ASCALE;
 
-            calibrationRestDetection.updateAcc(BMI160_ODR_ACC_MICROS, scaled);
+			calibrationRestDetection.updateAcc(BMI160_ODR_ACC_MICROS * 1.0e-6, scaled);
 
-            if (waitForMotion) {
+			if (waitForMotion) {
                 if (!calibrationRestDetection.getRestDetected()) {
                     waitForMotion = false;
                 }

diff --git a/src/sensors/icm20948sensor.cpp b/src/sensors/icm20948sensor.cpp
@@ -79,7 +79,7 @@ void ICM20948Sensor::motionLoop()
         cntbuf = 0;
         cntrounds = 0;
     }
-*/  
+*/
 }
 
 void ICM20948Sensor::readFIFOToEnd()
@@ -104,9 +104,8 @@ void ICM20948Sensor::readFIFOToEnd()
 
 void ICM20948Sensor::sendData()
 {
-    if(newFusedRotation && lastDataSent + 7 < millis())
+    if(newFusedRotation)
     {
-        lastDataSent = millis();
         newFusedRotation = false;
 
         #if(USE_6_AXIS)
@@ -320,6 +319,7 @@ void ICM20948Sensor::startMotionLoop()
 {
     lastData = millis();
     working = true;
+	hadData = true;
 }
 
 void ICM20948Sensor::checkSensorTimeout()