Merge branch 'SlimeVR:main' into main

CyanTabby · Mar 21, 2024 · 396c152 · 396c152
2 parents b5c93fe + 993d35a
commit 396c152
Show file tree

Hide file tree

Showing 9 changed files with 53 additions and 29 deletions.
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/src/consts.h b/src/consts.h
@@ -54,6 +54,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
@@ -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/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()

diff --git a/src/sensors/icm20948sensor.h b/src/sensors/icm20948sensor.h
@@ -45,7 +45,6 @@ class ICM20948Sensor : public Sensor
 private:
     void calculateAccelerationWithoutGravity(Quat *quaternion);
     unsigned long lastData = 0;
-    unsigned long lastDataSent = 0;
     int bias_save_counter = 0;
     bool hasdata = false;
 // Performance test