Merge pull request #226 from pkendall64/head-tracker-fixes

Head tracker fixes

Author: pkendall64

lib/HeadTracker/ICMSeries.cpp

@@ -33,8 +33,12 @@ bool ICMSeries::initialize() {
 
     writeRegister(0x21, 9);         // 16G, 100Hz
     writeRegister(0x20, 9);         // 2000dps, 100Hz
+    writeRegister(0x23, 0x05);      // GYRO_CONFIG1, 34Hz UI LPF
+    writeRegister(0x24, 0x45);      // ACCEL_CONFIG1, 34Hz UI LPF
     writeRegister(0x1F, 0x0F);      // Low noise mode for Accel/Gyro
 
+    deltaTime = 0.01f;
+
     gyroRange = 2000.0;
     gRes = GRES;
 

lib/HeadTracker/IMUBase.h

@@ -19,12 +19,12 @@ class IMUBase {
 
     const float *getCalibration();
 
-    int getSampleRate() const { return sampleRate; }
+    float getDeltaTime() const { return deltaTime; }
     float getGyroRange() const { return gyroRange; }
 
 protected:
     int address;
-    int sampleRate = 0;
+    float deltaTime = 0.0f;
     float gyroRange;
     float gRes = 0;
 

lib/HeadTracker/MPU6050.cpp

@@ -4,6 +4,8 @@
 #define WHO_AM_I 0x75
 #define ACCEL_X_H 0x3B
 
+#define MPU6050_DELTA_TIME 0.01f
+
 #define ARES (16.0 / 32768)
 #define GRES (2000.0 / 32768)
 
@@ -16,12 +18,14 @@ bool MPU6050::initialize() {
     writeRegister(0x6B, 0x01);  // PWR_MGMT_1, use X axis gyro as clock reference
     writeRegister(0x1B, 0x18);  // GYRO_CONFIG, 2000dps
     writeRegister(0x1C, 0x18);  // ACCEL_CONFIG, +-16g
-    writeRegister(0x1A, 0x01);  // CONFIG, 184/188Hz DLPF
+    writeRegister(0x1A, 0x03);  // CONFIG, 42Hz gyro / 44Hz accel DLPF
     writeRegister(0x19, 0x09);  // SMPRT_DIV, 1kHz / (1 + 9) = 100Hz
     writeRegister(0x37, 0x02);  // INT_PIN_CFG, I2C_BYPASS_EN
     writeRegister(0x38, 0x01);  // INT_ENABLE, DATA_RDY_EN
     writeRegister(0x23, 0x00);  // FIFO_EN, disabled
 
+    deltaTime = MPU6050_DELTA_TIME;
+
     gyroRange = 2000.0;
     gRes = GRES;
 
@@ -31,13 +35,13 @@ bool MPU6050::initialize() {
 bool MPU6050::getDataFromRegisters(FusionVector &accel, FusionVector &gyro) {
     uint8_t values[14];
 
-    // Read Accel, temp & gyro data (ignore the temp)
+    // Read accel, temp, and gyro data. Temperature is ignored.
     readBuffer(ACCEL_X_H, values, 14);
-    accel.axis.x =  (int16_t)((values[0] << 8) | values[1]) * ARES;
-    accel.axis.y =  (int16_t)((values[2] << 8) | values[3]) * ARES;
-    accel.axis.z =  (int16_t)((values[4] << 8) | values[5]) * ARES;
-    gyro.axis.x =  (int16_t)((values[8] << 8) | values[9]) * GRES;
-    gyro.axis.y =  (int16_t)((values[10] << 8) | values[11]) * GRES;
-    gyro.axis.z =  (int16_t)((values[12] << 8) | values[13]) * GRES;
+    accel.axis.x =  (int16_t) ((values[0] << 8) | values[1]) * ARES;
+    accel.axis.y =  (int16_t) ((values[2] << 8) | values[3]) * ARES;
+    accel.axis.z =  (int16_t) ((values[4] << 8) | values[5]) * ARES;
+    gyro.axis.x =  (int16_t) ((values[8] << 8) | values[9]) * GRES;
+    gyro.axis.y =  (int16_t) ((values[10] << 8) | values[11]) * GRES;
+    gyro.axis.z =  (int16_t) ((values[12] << 8) | values[13]) * GRES;
     return true;
 }

lib/HeadTracker/MPU6050.h

@@ -9,4 +9,4 @@ class MPU6050 : public IMUBase {
 
 protected:
     bool getDataFromRegisters(FusionVector &accel, FusionVector &gyro);
-};
+};

lib/HeadTracker/QMI8658C.cpp

@@ -7,6 +7,8 @@
 #define REVISION_ID 0x01
 #define ACCEL_X_L 0x35
 
+#define QMI8658_DELTA_TIME (1.0f / 112.1f)
+
 #define ARES (16.0 / 32768)
 #define GRES (1024.0 / 32768)
 
@@ -54,12 +56,12 @@ bool QMI8658C::initialize() {
         return false;
     }
 
-    writeRegister(0x03, 0b00110011);    // 16G, 896.8 ODR
-    writeRegister(0x06, 0b00000111);    // both LPF enabled @ 13.37% ODR
-    writeRegister(0x03, 0b10110011);    // 16G, 896.8 ODR + self test
-    writeRegister(0x04, 0b01100011);    // 1024dps, 896.8 ODR
-    writeRegister(0x06, 0b01110111);    // both LPF enabled @ 13.37% ODR
-    writeRegister(0x04, 0b11100011);    // 1024dps, 896.8 ODR + self test
+    writeRegister(0x03, 0b00110110);    // 16G, 112.1Hz ODR
+    writeRegister(0x06, 0b00000110);    // both LPF enabled @ 27.5% ODR
+    writeRegister(0x03, 0b10110110);    // 16G, 112.1Hz ODR + self test
+    writeRegister(0x04, 0b01100110);    // 1024dps, 112.1Hz ODR
+    writeRegister(0x06, 0b01110110);    // both LPF enabled @ 27.5% ODR
+    writeRegister(0x04, 0b11100110);    // 1024dps, 112.1Hz ODR + self test
     writeRegister(0x08, 0b10000011);    // SyncSample, G+A enabled
 
     // disable AHB clock gating
@@ -73,6 +75,7 @@ bool QMI8658C::initialize() {
         return false;
     }
 
+    deltaTime = QMI8658_DELTA_TIME;
     gyroRange = 1024.0;
     gRes = GRES;
 

lib/HeadTracker/QMI8658C.h

@@ -11,4 +11,4 @@ class QMI8658C : public IMUBase {
 
 private:
     int writeCommand(uint8_t cmd);
-};
+};

lib/HeadTracker/devHeadTracker.cpp

@@ -58,16 +58,23 @@ static void initialize()
         }
     }
     imu->setInterruptHandler(PIN_INT);
+    if (imu->getDeltaTime() <= 0.0f) {
+        delete imu;
+        imu = nullptr;
+        ht_state = STATE_ERROR;
+        return;
+    }
 
     FusionAhrsInitialise(&ahrs);
     // Set AHRS algorithm settings
+    const unsigned int recoveryTriggerPeriod = (unsigned int) ((5.0f / imu->getDeltaTime()) + 0.5f);
     const FusionAhrsSettings settings = {
             .convention = FusionConventionNwu,
             .gain = 0.5f,
             .gyroscopeRange = imu->getGyroRange(), /* replace this with actual gyroscope range in degrees/s */
             .accelerationRejection = 10.0f,
             .magneticRejection = 10.0f,
-            .recoveryTriggerPeriod = 5U * imu->getSampleRate(), /* 5 seconds */
+            .recoveryTriggerPeriod = recoveryTriggerPeriod,
     };
     FusionAhrsSetSettings(&ahrs, &settings);
     DBGLN("starting head tracker");
@@ -138,21 +145,17 @@ static int timeout()
 {
     FusionVector a;
     FusionVector g;
+    const float deltaTime = imu->getDeltaTime();
 
-    if (!imu->readIMUData(a, g))
+    if (!imu->readIMUData(a, g)) {
         return DURATION_IMMEDIATELY;
+    }
 
     switch (ht_state) {
         case STATE_RUNNING: {
             rotate(a.array, orientation);
             rotate(g.array, orientation);
 
-            // Calculate delta time (in seconds) to account for gyroscope sample clock error
-            const clock_t timestamp = micros();
-            static clock_t previousTimestamp;
-            const float deltaTime = (float) (timestamp - previousTimestamp) / (float) 1000000;
-            previousTimestamp = timestamp;
-
             FusionAhrsUpdate(&ahrs, g, a, FUSION_VECTOR_ZERO, deltaTime);
 
             euler = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs));
@@ -171,6 +174,7 @@ static int timeout()
             break;
         }
     }
+
     return DURATION_IMMEDIATELY;
 }
 
@@ -265,4 +269,4 @@ void getEuler(float *yaw, float *pitch, float *roll)
     *pitch = fmap(ptrChannelData[2], CRSF_CHANNEL_VALUE_1000, CRSF_CHANNEL_VALUE_2000, -180.0, 180.0);
     *roll = fmap(ptrChannelData[1], CRSF_CHANNEL_VALUE_1000, CRSF_CHANNEL_VALUE_2000, -180.0, 180.0);
 }
-#endif
+#endif

targets/debug.ini

@@ -100,6 +100,27 @@ extends = env:DEBUG_TIMER_ESP32_Backpack_via_UART
 # Head Tracker VRX backpack targets
 # ********************************
 
+[env:DEBUG_ESP_HT_Backpack_via_UART]
+extends = env_common_esp8285, rapidfire_vrx_backpack_common
+upload_resetmethod = nodemcu
+build_flags =
+	${env_common_esp8285.build_flags}
+	${rapidfire_vrx_backpack_common.build_flags}
+	-D DEBUG_LOG
+	-D HAS_HEADTRACKING
+	-D PIN_BUTTON=0
+	-D PIN_LED=16
+	-D PIN_MOSI=13
+	-D PIN_CLK=14
+	-D PIN_CS=15
+	-D PIN_SDA=2
+	-D PIN_SCL=4
+	-D PIN_INT=9
+reset_method=nodemcu
+
+[env:DEBUG_ESP_HT_Backpack_via_WIFI]
+extends = env:DEBUG_ESP_HT_Backpack_via_UART
+
 [env:DEBUG_C3_HT_Backpack_via_UART]
 extends = env_common_esp32c3, rapidfire_vrx_backpack_common
 build_flags =