diff --git a/README.md b/README.md index eb39b2f00..9f24e0c98 100644 --- a/README.md +++ b/README.md @@ -35,6 +35,12 @@ The following IMUs and their corresponding `IMU` values are supported by the fir * Using fusion in internal DMP for 6Dof or 9DoF, 9DoF mode requires good magnetic environment. * Comment out `USE_6DOF` in `debug.h` for 9DoF mode. * Experimental support! +* BMI270 (IMU_BMI270), ICM-42688 (IMU_ICM42688), LSM6DS3TR-C (IMU_LSM6DS3TRC), LSM6DSV (IMU_LSM6DSV), LSM6DSO (IMU_LSM6DSO), LSM6DSR (IMU_LSM6DSR), MPU-6050 (IMU_MPU6050_SF) + * Using common code: SoftFusionSensor for sensor fusion of Gyroscope and Accelerometer. + * Gyro&Accel sample rate, gyroscope offset and 6-side accelerometer calibration supported. + * In case of BMI270, gyroscope sensitivity auto-calibration (CRT) is additionally performed. + * Support for magnetometers is currently not implemented. + * VERY experimental support! Firmware can work with both ESP8266 and ESP32. Please edit `defines.h` and set your pinout properly according to how you connected the IMU. diff --git a/lib/ICM42688/ICM42688.h b/lib/ICM42688/ICM42688.h deleted file mode 100644 index 115879390..000000000 --- a/lib/ICM42688/ICM42688.h +++ /dev/null @@ -1,192 +0,0 @@ -/* 01/14/2022 Copyright Tlera Corporation - - Created by Kris Winer - - This sketch uses SDA/SCL on pins 21/20 (ladybug default), respectively, and it uses the Ladybug STM32L432 Breakout Board. - The ICM42688 is a combo sensor with embedded accel and gyro, here used as 6 DoF in a 9 DoF absolute orientation solution. - - Library may be used freely and without limit with attribution. - -*/ - -#ifndef ICM42688_h -#define ICM42688_h - -/* ICM42688 registers -https://media.digikey.com/pdf/Data%20Sheets/TDK%20PDFs/ICM-42688-P_DS_Rev1.2.pdf -*/ -// User Bank 0 -#define ICM42688_DEVICE_CONFIG 0x11 -#define ICM42688_DRIVE_CONFIG 0x13 -#define ICM42688_INT_CONFIG 0x14 -#define ICM42688_FIFO_CONFIG 0x16 -#define ICM42688_TEMP_DATA1 0x1D -#define ICM42688_TEMP_DATA0 0x1E -#define ICM42688_ACCEL_DATA_X1 0x1F -#define ICM42688_ACCEL_DATA_X0 0x20 -#define ICM42688_ACCEL_DATA_Y1 0x21 -#define ICM42688_ACCEL_DATA_Y0 0x22 -#define ICM42688_ACCEL_DATA_Z1 0x23 -#define ICM42688_ACCEL_DATA_Z0 0x24 -#define ICM42688_GYRO_DATA_X1 0x25 -#define ICM42688_GYRO_DATA_X0 0x26 -#define ICM42688_GYRO_DATA_Y1 0x27 -#define ICM42688_GYRO_DATA_Y0 0x28 -#define ICM42688_GYRO_DATA_Z1 0x29 -#define ICM42688_GYRO_DATA_Z0 0x2A -#define ICM42688_TMST_FSYNCH 0x2B -#define ICM42688_TMST_FSYNCL 0x2C -#define ICM42688_INT_STATUS 0x2D -#define ICM42688_FIFO_COUNTH 0x2E -#define ICM42688_FIFO_COUNTL 0x2F -#define ICM42688_FIFO_DATA 0x30 -#define ICM42688_APEX_DATA0 0x31 -#define ICM42688_APEX_DATA1 0x32 -#define ICM42688_APEX_DATA2 0x33 -#define ICM42688_APEX_DATA3 0x34 -#define ICM42688_APEX_DATA4 0x35 -#define ICM42688_APEX_DATA5 0x36 -#define ICM42688_INT_STATUS2 0x37 -#define ICM42688_INT_STATUS3 0x38 -#define ICM42688_SIGNAL_PATH_RESET 0x4B -#define ICM42688_INTF_CONFIG0 0x4C -#define ICM42688_INTF_CONFIG1 0x4D -#define ICM42688_PWR_MGMT0 0x4E -#define ICM42688_GYRO_CONFIG0 0x4F -#define ICM42688_ACCEL_CONFIG0 0x50 -#define ICM42688_GYRO_CONFIG1 0x51 -#define ICM42688_GYRO_ACCEL_CONFIG0 0x52 -#define ICM42688_ACCEL_CONFIG1 0x53 -#define ICM42688_TMST_CONFIG 0x54 -#define ICM42688_APEX_CONFIG0 0x56 -#define ICM42688_SMD_CONFIG 0x57 -#define ICM42688_FIFO_CONFIG1 0x5F -#define ICM42688_FIFO_CONFIG2 0x60 -#define ICM42688_FIFO_CONFIG3 0x61 -#define ICM42688_FSYNC_CONFIG 0x62 -#define ICM42688_INT_CONFIG0 0x63 -#define ICM42688_INT_CONFIG1 0x64 -#define ICM42688_INT_SOURCE0 0x65 -#define ICM42688_INT_SOURCE1 0x66 -#define ICM42688_INT_SOURCE3 0x68 -#define ICM42688_INT_SOURCE4 0x69 -#define ICM42688_FIFO_LOST_PKT0 0x6C -#define ICM42688_FIFO_LOST_PKT1 0x6D -#define ICM42688_SELF_TEST_CONFIG 0x70 -#define ICM42688_WHO_AM_I 0x75 // should return 0x47 -#define ICM42688_REG_BANK_SEL 0x76 - -// User Bank 1 -#define ICM42688_SENSOR_CONFIG0 0x03 -#define ICM42688_GYRO_CONFIG_STATIC2 0x0B -#define ICM42688_GYRO_CONFIG_STATIC3 0x0C -#define ICM42688_GYRO_CONFIG_STATIC4 0x0D -#define ICM42688_GYRO_CONFIG_STATIC5 0x0E -#define ICM42688_GYRO_CONFIG_STATIC6 0x0F -#define ICM42688_GYRO_CONFIG_STATIC7 0x10 -#define ICM42688_GYRO_CONFIG_STATIC8 0x11 -#define ICM42688_GYRO_CONFIG_STATIC9 0x12 -#define ICM42688_GYRO_CONFIG_STATIC10 0x13 -#define ICM42688_XG_ST_DATA 0x5F -#define ICM42688_YG_ST_DATA 0x60 -#define ICM42688_ZG_ST_DATA 0x61 -#define ICM42688_TMSTAL0 0x63 -#define ICM42688_TMSTAL1 0x64 -#define ICM42688_TMSTAL2 0x62 -#define ICM42688_INTF_CONFIG4 0x7A -#define ICM42688_INTF_CONFIG5 0x7B -#define ICM42688_INTF_CONFIG6 0x7C - -// User Bank 2 -#define ICM42688_ACCEL_CONFIG_STATIC2 0x03 -#define ICM42688_ACCEL_CONFIG_STATIC3 0x04 -#define ICM42688_ACCEL_CONFIG_STATIC4 0x05 -#define ICM42688_XA_ST_DATA 0x3B -#define ICM42688_YA_ST_DATA 0x3C -#define ICM42688_ZA_ST_DATA 0x3D - -// User Bank 4 -#define ICM42688_APEX_CONFIG1 0x40 -#define ICM42688_APEX_CONFIG2 0x41 -#define ICM42688_APEX_CONFIG3 0x42 -#define ICM42688_APEX_CONFIG4 0x43 -#define ICM42688_APEX_CONFIG5 0x44 -#define ICM42688_APEX_CONFIG6 0x45 -#define ICM42688_APEX_CONFIG7 0x46 -#define ICM42688_APEX_CONFIG8 0x47 -#define ICM42688_APEX_CONFIG9 0x48 -#define ICM42688_ACCEL_WOM_X_THR 0x4A -#define ICM42688_ACCEL_WOM_Y_THR 0x4B -#define ICM42688_ACCEL_WOM_Z_THR 0x4C -#define ICM42688_INT_SOURCE6 0x4D -#define ICM42688_INT_SOURCE7 0x4E -#define ICM42688_INT_SOURCE8 0x4F -#define ICM42688_INT_SOURCE9 0x50 -#define ICM42688_INT_SOURCE10 0x51 -#define ICM42688_OFFSET_USER0 0x77 -#define ICM42688_OFFSET_USER1 0x78 -#define ICM42688_OFFSET_USER2 0x79 -#define ICM42688_OFFSET_USER3 0x7A -#define ICM42688_OFFSET_USER4 0x7B -#define ICM42688_OFFSET_USER5 0x7C -#define ICM42688_OFFSET_USER6 0x7D -#define ICM42688_OFFSET_USER7 0x7E -#define ICM42688_OFFSET_USER8 0x7F - -#define ICM42688_ADDRESS 0x68 // Address of ICM42688 accel/gyro when ADO = 0 - -#define AFS_2G 0x03 -#define AFS_4G 0x02 -#define AFS_8G 0x01 -#define AFS_16G 0x00 // default - -#define GFS_2000DPS 0x00 // default -#define GFS_1000DPS 0x01 -#define GFS_500DPS 0x02 -#define GFS_250DPS 0x03 -#define GFS_125DPS 0x04 -#define GFS_62_50DPS 0x05 -#define GFS_31_25DPS 0x06 -#define GFS_15_625DPS 0x07 - -// Low Noise mode -#define AODR_32kHz 0x01 -#define AODR_16kHz 0x02 -#define AODR_8kHz 0x03 -#define AODR_4kHz 0x04 -#define AODR_2kHz 0x05 -#define AODR_1kHz 0x06 // default -//Low Noise or Low Power modes -#define AODR_500Hz 0x0F -#define AODR_200Hz 0x07 -#define AODR_100Hz 0x08 -#define AODR_50Hz 0x09 -#define AODR_25Hz 0x0A -#define AODR_12_5Hz 0x0B -// Low Power mode -#define AODR_6_25Hz 0x0C -#define AODR_3_125Hz 0x0D -#define AODR_1_5625Hz 0x0E - -#define GODR_32kHz 0x01 -#define GODR_16kHz 0x02 -#define GODR_8kHz 0x03 -#define GODR_4kHz 0x04 -#define GODR_2kHz 0x05 -#define GODR_1kHz 0x06 // default -#define GODR_500Hz 0x0F -#define GODR_200Hz 0x07 -#define GODR_100Hz 0x08 -#define GODR_50Hz 0x09 -#define GODR_25Hz 0x0A -#define GODR_12_5Hz 0x0B - -#define aMode_OFF 0x01 -#define aMode_LP 0x02 -#define aMode_LN 0x03 - -#define gMode_OFF 0x00 -#define gMode_SBY 0x01 -#define gMode_LN 0x03 - -#endif diff --git a/lib/ICM42688/MMC5983MA.h b/lib/ICM42688/MMC5983MA.h deleted file mode 100644 index bdec5b77e..000000000 --- a/lib/ICM42688/MMC5983MA.h +++ /dev/null @@ -1,63 +0,0 @@ -/* 06/14/2020 Copyright Tlera Corporation - - Created by Kris Winer - - This sketch uses SDA/SCL on pins 21/20 (Ladybug default), respectively, and it uses the Ladybug STM32L432 Breakout Board. - The MMC5983MA is a low power magnetometer, here used as 3 DoF in a 9 DoF absolute orientation solution. - - Library may be used freely and without limit with attribution. - -*/ - -#ifndef MMC5983MA_h -#define MMC5983MA_h - -//Register map for MMC5983MA' -//http://www.memsic.com/userfiles/files/DataSheets/Magnetic-Sensors-Datasheets/MMC5983MA_Datasheet.pdf -#define MMC5983MA_XOUT_0 0x00 -#define MMC5983MA_XOUT_1 0x01 -#define MMC5983MA_YOUT_0 0x02 -#define MMC5983MA_YOUT_1 0x03 -#define MMC5983MA_ZOUT_0 0x04 -#define MMC5983MA_ZOUT_1 0x05 -#define MMC5983MA_XYZOUT_2 0x06 -#define MMC5983MA_TOUT 0x07 -#define MMC5983MA_STATUS 0x08 -#define MMC5983MA_CONTROL_0 0x09 -#define MMC5983MA_CONTROL_1 0x0A -#define MMC5983MA_CONTROL_2 0x0B -#define MMC5983MA_CONTROL_3 0x0C -#define MMC5983MA_PRODUCT_ID 0x2F - -#define MMC5983MA_ADDRESS 0x30 - -// Sample rates -#define MODR_ONESHOT 0x00 -#define MODR_1Hz 0x01 -#define MODR_10Hz 0x02 -#define MODR_20Hz 0x03 -#define MODR_50Hz 0x04 -#define MODR_100Hz 0x05 -#define MODR_200Hz 0x06 // BW = 0x01 only -#define MODR_1000Hz 0x07 // BW = 0x11 only - -//Bandwidths -#define MBW_100Hz 0x00 // 8 ms measurement time -#define MBW_200Hz 0x01 // 4 ms -#define MBW_400Hz 0x02 // 2 ms -#define MBW_800Hz 0x03 // 0.5 ms - -// Set/Reset as a function of measurements -#define MSET_1 0x00 // Set/Reset each data measurement -#define MSET_25 0x01 // each 25 data measurements -#define MSET_75 0x02 -#define MSET_100 0x03 -#define MSET_250 0x04 -#define MSET_500 0x05 -#define MSET_1000 0x06 -#define MSET_2000 0x07 - -#define MMC5983MA_mRes (1.0f / 16384.0f) // mag sensitivity if using 18 bit data -#define MMC5983MA_offset 131072.0f // mag range unsigned to signed - -#endif diff --git a/platformio-tools.ini b/platformio-tools.ini index 8389d9cdd..7d5169029 100644 --- a/platformio-tools.ini +++ b/platformio-tools.ini @@ -7,59 +7,74 @@ framework = arduino build_flags = !python scripts/get_git_commit.py -O2 - -std=gnu++17 + -std=gnu++2a build_unflags = -Os - -std=gnu++11 + -std=gnu++11 -std=gnu++17 [env:BOARD_SLIMEVR] -platform = espressif8266 @ 4.2.0 +platform = espressif8266 @ 4.2.1 board = esp12e [env:BOARD_SLIMEVR_DEV] -platform = espressif8266 @ 4.2.0 +platform = espressif8266 @ 4.2.1 board = esp12e [env:BOARD_NODEMCU] -platform = espressif8266 @ 4.2.0 +platform = espressif8266 @ 4.2.1 board = esp12e [env:BOARD_WEMOSD1MINI] -platform = espressif8266 @ 4.2.0 +platform = espressif8266 @ 4.2.1 board = esp12e [env:BOARD_TTGO_TBASE] -platform = espressif8266 @ 4.2.0 +platform = espressif8266 @ 4.2.1 board = esp12e [env:BOARD_WEMOSWROOM02] -platform = espressif8266 @ 4.2.0 +platform = espressif8266 @ 4.2.1 board = esp12e [env:BOARD_WROOM32] -platform = espressif32 @ 6.1.0 +platform = espressif32 @ 6.7.0 +platform_packages = + framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32.git#3.0.1 + framework-arduinoespressif32-libs @ https://github.com/espressif/arduino-esp32/releases/download/3.0.1/esp32-arduino-libs-3.0.1.zip board = esp32dev [env:BOARD_ESP01] -platform = espressif32 @ 6.1.0 +platform = espressif32 @ 6.7.0 +platform_packages = + framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32.git#3.0.1 + framework-arduinoespressif32-libs @ https://github.com/espressif/arduino-esp32/releases/download/3.0.1/esp32-arduino-libs-3.0.1.zip board = esp32dev [env:BOARD_LOLIN_C3_MINI] -platform = espressif32 @ 6.1.0 +platform = espressif32 @ 6.7.0 +platform_packages = + framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32.git#3.0.1 + framework-arduinoespressif32-libs @ https://github.com/espressif/arduino-esp32/releases/download/3.0.1/esp32-arduino-libs-3.0.1.zip build_flags = ${env.build_flags} -DESP32C3 board = lolin_c3_mini [env:BOARD_BEETLE32C3] -platform = espressif32 @ 6.1.0 +platform = espressif32 @ 6.7.0 +platform_packages = + framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32.git#3.0.1 + framework-arduinoespressif32-libs @ https://github.com/espressif/arduino-esp32/releases/download/3.0.1/esp32-arduino-libs-3.0.1.zip build_flags = ${env.build_flags} -DESP32C3 board = dfrobot_beetle_esp32c3 [env:BOARD_ES32C3DEVKITM1] -platform = espressif32 @ 6.1.0 +platform = espressif32 @ 6.7.0 +platform_packages = + framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32.git#3.0.1 + framework-arduinoespressif32-libs @ https://github.com/espressif/arduino-esp32/releases/download/3.0.1/esp32-arduino-libs-3.0.1.zip build_flags = ${env.build_flags} -DESP32C3 @@ -73,7 +88,10 @@ build_flags = board = esp32-c6-devkitc-1 [env:BOARD_XIAO_ESP32C3] -platform = espressif32 @ 6.1.0 +platform = espressif32 @ 6.7.0 +platform_packages = + framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32.git#3.0.1 + framework-arduinoespressif32-libs @ https://github.com/espressif/arduino-esp32/releases/download/3.0.1/esp32-arduino-libs-3.0.1.zip build_flags = ${env.build_flags} -DESP32C3 diff --git a/platformio.ini b/platformio.ini index 3ba3cfa66..e12b70238 100644 --- a/platformio.ini +++ b/platformio.ini @@ -43,9 +43,9 @@ build_flags = ; Enable -O2 GCC optimization -O2 - -std=gnu++17 + -std=gnu++2a -build_unflags = -Os -std=gnu++11 +build_unflags = -Os -std=gnu++11 -std=gnu++17 ; 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 @@ -58,7 +58,7 @@ build_unflags = -Os -std=gnu++11 ; Settings for different boards [env:esp12e] -platform = espressif8266 @ 4.2.0 +platform = espressif8266 @ 4.2.1 board = esp12e ; Comment out this line below if you have any trouble uploading the firmware ; and if it has a CP2102 on it (a square chip next to the usb port): change to 3000000 (3 million) for even faster upload speed @@ -66,13 +66,13 @@ upload_speed = 921600 ; Uncomment below if you want to build for ESP-01 ;[env:esp01_1m] -;platform = espressif8266 @ 4.2.0 +;platform = espressif8266 @ 4.2.1 ;board = esp01_1m ;board_build.arduino.ldscript = "eagle.flash.1m64.ld" ; Uncomment below if you want to build for ESP8285 (ESP8266 with embedded Flash) ;[env:esp8285] -;platform = espressif8266 @ 4.2.0 +;platform = espressif8266 @ 4.2.1 ;board = esp8285 ;board_build.arduino.ldscript = "eagle.flash.1m64.ld" ;board_build.flash_mode = dout @@ -80,7 +80,10 @@ upload_speed = 921600 ; Uncomment below if you want to build for esp32 ; Check your board name at https://docs.platformio.org/en/latest/platforms/espressif32.html#boards ; [env:esp32] -; platform = espressif32 @ 6.1.0 +; platform = espressif32 @ 6.7.0 +; platform_packages = +; framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32.git#3.0.1 +; framework-arduinoespressif32-libs @ https://github.com/espressif/arduino-esp32/releases/download/3.0.1/esp32-arduino-libs-3.0.1.zip ; board = esp32dev ; Comment out this line below if you have any trouble uploading the firmware - and if it has a CP2102 on it (a square chip next to the usb port): change to 3000000 (3 million) for even faster upload speed ;upload_speed = 921600 @@ -95,7 +98,10 @@ upload_speed = 921600 ; -DARDUINO_USB_CDC_ON_BOOT=1 ;[env:esp32c3] -;platform = espressif32 @ 6.1.0 +;platform = espressif32 @ 6.7.0 +;platform_packages = +; framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32.git#3.0.1 +; framework-arduinoespressif32-libs @ https://github.com/espressif/arduino-esp32/releases/download/3.0.1/esp32-arduino-libs-3.0.1.zip ;build_flags = ; ${env.build_flags} ; -DESP32C3 diff --git a/src/configuration/CalibrationConfig.cpp b/src/configuration/CalibrationConfig.cpp index 9aeb3eec7..0c9458df1 100644 --- a/src/configuration/CalibrationConfig.cpp +++ b/src/configuration/CalibrationConfig.cpp @@ -37,8 +37,8 @@ namespace SlimeVR { return "MPU9250"; case ICM20948: return "ICM20948"; - case ICM42688: - return "ICM42688"; + case SFUSION: + return "SoftFusion (common)"; default: return "UNKNOWN"; } diff --git a/src/configuration/CalibrationConfig.h b/src/configuration/CalibrationConfig.h index 0fe4d91f8..f07e0dc2f 100644 --- a/src/configuration/CalibrationConfig.h +++ b/src/configuration/CalibrationConfig.h @@ -25,6 +25,7 @@ #define SLIMEVR_CONFIGURATION_CALIBRATIONCONFIG_H #include +#include "consts.h" namespace SlimeVR { namespace Configuration { @@ -44,6 +45,36 @@ namespace SlimeVR { float temperature; }; + struct SoftFusionCalibrationConfig { + ImuID ImuType; + uint16_t MotionlessDataLen; + + // accelerometer offsets and correction matrix + float A_B[3]; + float A_Ainv[3][3]; + + // magnetometer offsets and correction matrix + float M_B[3]; + float M_Ainv[3][3]; + + // raw offsets, determined from gyro at rest + float G_off[3]; + + // calibration temperature for dynamic compensation + float temperature; + + // real measured sensor sampling rate + float A_Ts; + float G_Ts; + float M_Ts; + + // gyro sensitivity multiplier + float G_Sens[3]; + + uint8_t MotionlessData[60]; + }; + + struct MPU6050CalibrationConfig { // accelerometer offsets and correction matrix float A_B[3]; @@ -89,7 +120,7 @@ namespace SlimeVR { float G_off[3]; }; - enum CalibrationConfigType { NONE, BMI160, MPU6050, MPU9250, ICM20948, ICM42688 }; + enum CalibrationConfigType { NONE, BMI160, MPU6050, MPU9250, ICM20948, SFUSION }; const char* calibrationConfigTypeToString(CalibrationConfigType type); @@ -98,10 +129,10 @@ namespace SlimeVR { union { BMI160CalibrationConfig bmi160; + SoftFusionCalibrationConfig sfusion; MPU6050CalibrationConfig mpu6050; MPU9250CalibrationConfig mpu9250; ICM20948CalibrationConfig icm20948; - ICM42688CalibrationConfig icm42688; } data; }; } diff --git a/src/configuration/Configuration.cpp b/src/configuration/Configuration.cpp index 03e2190d4..0114c30c0 100644 --- a/src/configuration/Configuration.cpp +++ b/src/configuration/Configuration.cpp @@ -272,6 +272,18 @@ namespace SlimeVR { break; + case CalibrationConfigType::SFUSION: + m_Logger.info(" A_B : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.sfusion.A_B)); + + m_Logger.info(" A_Ainv :"); + for (uint8_t i = 0; i < 3; i++) { + m_Logger.info(" %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.sfusion.A_Ainv[i])); + } + + m_Logger.info(" G_off : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.sfusion.G_off)); + m_Logger.info(" Temperature: %f", c.data.sfusion.temperature); + break; + case CalibrationConfigType::ICM20948: m_Logger.info(" G: %d, %d, %d", UNPACK_VECTOR_ARRAY(c.data.icm20948.G)); m_Logger.info(" A: %d, %d, %d", UNPACK_VECTOR_ARRAY(c.data.icm20948.A)); @@ -302,25 +314,6 @@ namespace SlimeVR { m_Logger.info(" A_B : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.mpu6050.A_B)); m_Logger.info(" G_off: %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.mpu6050.G_off)); - break; - - case CalibrationConfigType::ICM42688: - m_Logger.info(" A_B : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.icm42688.A_B)); - - m_Logger.info(" A_Ainv:"); - for (uint8_t i = 0; i < 3; i++) { - m_Logger.info(" %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.icm42688.A_Ainv[i])); - } - - m_Logger.info(" M_B : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.icm42688.M_B)); - - m_Logger.info(" M_Ainv:"); - for (uint8_t i = 0; i < 3; i++) { - m_Logger.info(" %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.icm42688.M_Ainv[i])); - } - - m_Logger.info(" G_off : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.icm42688.G_off)); - break; } } diff --git a/src/consts.h b/src/consts.h index 1e656dce6..b53a049d6 100644 --- a/src/consts.h +++ b/src/consts.h @@ -24,17 +24,45 @@ #define SLIMEVR_CONSTS_H_ // List of constants used in other places -#define IMU_UNKNOWN 0 -#define IMU_MPU9250 1 -#define IMU_MPU6500 2 -#define IMU_BNO080 3 -#define IMU_BNO085 4 -#define IMU_BNO055 5 -#define IMU_MPU6050 6 -#define IMU_BNO086 7 -#define IMU_BMI160 8 -#define IMU_ICM20948 9 -#define IMU_ICM42688 10 + +enum class ImuID { + Unknown = 0, + MPU9250, + MPU6500, + BNO080, + BNO085, + BNO055, + MPU6050, + BNO086, + BMI160, + ICM20948, + ICM42688, + BMI270, + LSM6DS3TRC, + LSM6DSV, + LSM6DSO, + LSM6DSR, + Empty = 255 +}; + +#define IMU_UNKNOWN ErroneousSensor +#define IMU_MPU9250 MPU9250Sensor +#define IMU_MPU6500 MPU6050Sensor +#define IMU_BNO080 BNO080Sensor +#define IMU_BNO085 BNO085Sensor +#define IMU_BNO055 BNO055Sensor +#define IMU_MPU6050 MPU6050Sensor +#define IMU_BNO086 BNO086Sensor +#define IMU_BMI160 BMI160Sensor +#define IMU_ICM20948 ICM20948Sensor +#define IMU_ICM42688 SoftFusionICM42688 +#define IMU_BMI270 SoftFusionBMI270 +#define IMU_LSM6DS3TRC SoftFusionLSM6DS3TRC +#define IMU_LSM6DSV SoftFusionLSM6DSV +#define IMU_LSM6DSO SoftFusionLSM6DSO +#define IMU_LSM6DSR SoftFusionLSM6DSR +#define IMU_MPU6050_SF SoftFusionMPU6050 + #define IMU_DEV_RESERVED 250 // Reserved, should not be used in any release firmware #define BOARD_UNKNOWN 0 diff --git a/src/main.cpp b/src/main.cpp index b1ca2c756..f79ca59e3 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -71,10 +71,10 @@ void setup() SerialCommands::setUp(); -#if IMU == IMU_MPU6500 || IMU == IMU_MPU6050 || IMU == IMU_MPU9250 || IMU == IMU_BNO055 || IMU == IMU_ICM20948 || IMU == IMU_BMI160|| IMU == IMU_ICM42688 I2CSCAN::clearBus(PIN_IMU_SDA, PIN_IMU_SCL); // Make sure the bus isn't stuck when resetting ESP without powering it down - // Fixes I2C issues for certain IMUs. Only has been tested on IMUs above. Testing advised when adding other IMUs. -#endif + // Fixes I2C issues for certain IMUs. Previously this feature was enabled for selected IMUs, now it's enabled for all. + // If some IMU turned out to be broken by this, check needs to be re-added. + // join I2C bus #if ESP32 diff --git a/src/motionprocessing/RestDetection.h b/src/motionprocessing/RestDetection.h index 81dac51e6..94d754532 100644 --- a/src/motionprocessing/RestDetection.h +++ b/src/motionprocessing/RestDetection.h @@ -103,7 +103,7 @@ class RestDetection { } #endif - void updateGyr(sensor_real_t gyr[3]) { + void updateGyr(const sensor_real_t gyr[3]) { #ifdef REST_DETECTION_DISABLE_LPF gyrLastSquaredDeviation = square(gyr[0] - lastSample.gyr[0]) + @@ -140,7 +140,7 @@ class RestDetection { #endif } - void updateAcc(sensor_real_t dt, sensor_real_t acc[3]) { + void updateAcc(sensor_real_t dt, const 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; } diff --git a/src/network/connection.cpp b/src/network/connection.cpp index 86b2b5998..d37f93d97 100644 --- a/src/network/connection.cpp +++ b/src/network/connection.cpp @@ -288,16 +288,16 @@ void Connection::sendSensorError(uint8_t sensorId, uint8_t error) { } // PACKET_SENSOR_INFO 15 -void Connection::sendSensorInfo(Sensor* sensor) { +void Connection::sendSensorInfo(Sensor& sensor) { MUST(m_Connected); MUST(beginPacket()); MUST(sendPacketType(PACKET_SENSOR_INFO)); MUST(sendPacketNumber()); - MUST(sendByte(sensor->getSensorId())); - MUST(sendByte((uint8_t)sensor->getSensorState())); - MUST(sendByte(sensor->getSensorType())); + MUST(sendByte(sensor.getSensorId())); + MUST(sendByte(static_cast(sensor.getSensorState()))); + MUST(sendByte(static_cast(sensor.getSensorType()))); MUST(endPacket()); } @@ -396,7 +396,7 @@ void Connection::sendTrackerDiscovery() { // This is kept for backwards compatibility, // but the latest SlimeVR server will not initialize trackers // with firmware build > 8 until it recieves a sensor info packet - MUST(sendInt(IMU)); + MUST(sendInt(static_cast(sensorManager.getSensorType(0)))); MUST(sendInt(HARDWARE_MCU)); MUST(sendInt(0)); MUST(sendInt(0)); @@ -511,7 +511,7 @@ void Connection::returnLastPacket(int len) { MUST(endPacket()); } -void Connection::updateSensorState(std::vector & sensors) { +void Connection::updateSensorState(std::vector> & sensors) { if (millis() - m_LastSensorInfoPacketTimestamp <= 1000) { return; } @@ -520,7 +520,7 @@ void Connection::updateSensorState(std::vector & sensors) { for (int i = 0; i < (int)sensors.size(); i++) { if (m_AckedSensorState[i] != sensors[i]->getSensorState()) { - sendSensorInfo(sensors[i]); + sendSensorInfo(*sensors[i]); } } } @@ -547,7 +547,7 @@ void Connection::searchForServer() { } // receive incoming UDP packets - int len __attribute__((unused)) = m_UDP.read(m_Packet, sizeof(m_Packet)); + [[maybe_unused]] int len = m_UDP.read(m_Packet, sizeof(m_Packet)); #ifdef DEBUG_NETWORK m_Logger.trace( @@ -611,7 +611,7 @@ void Connection::reset() { } void Connection::update() { - std::vector & sensors = sensorManager.getSensors(); + auto & sensors = sensorManager.getSensors(); updateSensorState(sensors); maybeRequestFeatureFlags(); diff --git a/src/network/connection.h b/src/network/connection.h index 7b8f7b6a2..b0aefc3e5 100644 --- a/src/network/connection.h +++ b/src/network/connection.h @@ -125,7 +125,7 @@ class Connection { bool endBundle(); private: - void updateSensorState(std::vector & sensors); + void updateSensorState(std::vector> & sensors); void maybeRequestFeatureFlags(); bool beginPacket(); @@ -156,7 +156,7 @@ class Connection { void sendTrackerDiscovery(); // PACKET_SENSOR_INFO 15 - void sendSensorInfo(Sensor* sensor); + void sendSensorInfo(Sensor& sensor); bool m_Connected = false; SlimeVR::Logging::Logger m_Logger = SlimeVR::Logging::Logger("UDPConnection"); diff --git a/src/sensors/EmptySensor.h b/src/sensors/EmptySensor.h index 1263178a9..c5e785f51 100644 --- a/src/sensors/EmptySensor.h +++ b/src/sensors/EmptySensor.h @@ -33,7 +33,7 @@ namespace SlimeVR class EmptySensor : public Sensor { public: - EmptySensor(uint8_t id) : Sensor("EmptySensor", 255, id, 0, 0.0){}; + EmptySensor(uint8_t id) : Sensor("EmptySensor", ImuID::Empty, id, 0, 0.0){}; ~EmptySensor(){}; void motionSetup() override final{}; diff --git a/src/sensors/ErroneousSensor.h b/src/sensors/ErroneousSensor.h index 4dc1d5310..7cd0fc413 100644 --- a/src/sensors/ErroneousSensor.h +++ b/src/sensors/ErroneousSensor.h @@ -33,7 +33,7 @@ namespace SlimeVR class ErroneousSensor : public Sensor { public: - ErroneousSensor(uint8_t id, uint8_t type) : Sensor("ErroneousSensor", type, id, 0, 0.0), m_ExpectedType(type){}; + ErroneousSensor(uint8_t id, ImuID type) : Sensor("ErroneousSensor", type, id, 0, 0.0), m_ExpectedType(type){}; ~ErroneousSensor(){}; void motionSetup() override; @@ -43,7 +43,7 @@ namespace SlimeVR SensorStatus getSensorState() override final; private: - uint8_t m_ExpectedType; + ImuID m_ExpectedType; }; } } diff --git a/src/sensors/SensorFusion.cpp b/src/sensors/SensorFusion.cpp index e36380a40..478b9abbf 100644 --- a/src/sensors/SensorFusion.cpp +++ b/src/sensors/SensorFusion.cpp @@ -18,7 +18,7 @@ namespace SlimeVR updateGyro(Gxyz, deltat); } - void SensorFusion::updateAcc(sensor_real_t Axyz[3], sensor_real_t deltat) + void SensorFusion::updateAcc(const sensor_real_t Axyz[3], sensor_real_t deltat) { if (deltat < 0) deltat = accTs; @@ -32,7 +32,7 @@ namespace SlimeVR #endif } - void SensorFusion::updateMag(sensor_real_t Mxyz[3], sensor_real_t deltat) + void SensorFusion::updateMag(const sensor_real_t Mxyz[3], sensor_real_t deltat) { if (deltat < 0) deltat = magTs; @@ -53,7 +53,7 @@ namespace SlimeVR #endif } - void SensorFusion::updateGyro(sensor_real_t Gxyz[3], sensor_real_t deltat) + void SensorFusion::updateGyro(const sensor_real_t Gxyz[3], sensor_real_t deltat) { if (deltat < 0) deltat = gyrTs; diff --git a/src/sensors/SensorFusion.h b/src/sensors/SensorFusion.h index a10261773..cc0116205 100644 --- a/src/sensors/SensorFusion.h +++ b/src/sensors/SensorFusion.h @@ -74,9 +74,9 @@ namespace SlimeVR void update6D(sensor_real_t Axyz[3], sensor_real_t Gxyz[3], sensor_real_t deltat=-1.0f); void update9D(sensor_real_t Axyz[3], sensor_real_t Gxyz[3], sensor_real_t Mxyz[3], sensor_real_t deltat=-1.0f); - void updateAcc(sensor_real_t Axyz[3], sensor_real_t deltat=-1.0f); - void updateMag(sensor_real_t Mxyz[3], sensor_real_t deltat=-1.0f); - void updateGyro(sensor_real_t Gxyz[3], sensor_real_t deltat=-1.0f); + void updateAcc(const sensor_real_t Axyz[3], sensor_real_t deltat=-1.0f); + void updateMag(const sensor_real_t Mxyz[3], sensor_real_t deltat=-1.0f); + void updateGyro(const sensor_real_t Gxyz[3], sensor_real_t deltat=-1.0f); bool isUpdated(); void clearUpdated(); diff --git a/src/sensors/SensorFusionRestDetect.cpp b/src/sensors/SensorFusionRestDetect.cpp index 1221810a4..a742c883a 100644 --- a/src/sensors/SensorFusionRestDetect.cpp +++ b/src/sensors/SensorFusionRestDetect.cpp @@ -5,14 +5,14 @@ namespace SlimeVR namespace Sensors { #if !SENSOR_FUSION_WITH_RESTDETECT - void SensorFusionRestDetect::updateAcc(sensor_real_t Axyz[3], sensor_real_t deltat) + void SensorFusionRestDetect::updateAcc(const sensor_real_t Axyz[3], sensor_real_t deltat) { if (deltat < 0) deltat = accTs; restDetection.updateAcc(deltat, Axyz); SensorFusion::updateAcc(Axyz, deltat); } - void SensorFusionRestDetect::updateGyro(sensor_real_t Gxyz[3], sensor_real_t deltat) + void SensorFusionRestDetect::updateGyro(const sensor_real_t Gxyz[3], sensor_real_t deltat) { if (deltat < 0) deltat = gyrTs; restDetection.updateGyr(Gxyz); diff --git a/src/sensors/SensorFusionRestDetect.h b/src/sensors/SensorFusionRestDetect.h index 025e8de5d..03e7354bc 100644 --- a/src/sensors/SensorFusionRestDetect.h +++ b/src/sensors/SensorFusionRestDetect.h @@ -39,8 +39,8 @@ namespace SlimeVR bool getRestDetected(); #if !SENSOR_FUSION_WITH_RESTDETECT - void updateAcc(sensor_real_t Axyz[3], sensor_real_t deltat); - void updateGyro(sensor_real_t Gxyz[3], sensor_real_t deltat); + void updateAcc(const sensor_real_t Axyz[3], const sensor_real_t deltat); + void updateGyro(const sensor_real_t Gxyz[3], const sensor_real_t deltat); #endif protected: #if !SENSOR_FUSION_WITH_RESTDETECT diff --git a/src/sensors/SensorManager.cpp b/src/sensors/SensorManager.cpp index 434f8a5f7..1c1dc9cc7 100644 --- a/src/sensors/SensorManager.cpp +++ b/src/sensors/SensorManager.cpp @@ -22,17 +22,24 @@ */ #include "SensorManager.h" -#include + #include "bno055sensor.h" #include "bno080sensor.h" #include "mpu9250sensor.h" #include "mpu6050sensor.h" #include "bmi160sensor.h" #include "icm20948sensor.h" -#include "icm42688sensor.h" -#include "ErroneousSensor.h" #include "sensoraddresses.h" -#include "GlobalVars.h" +#include "softfusion/softfusionsensor.h" +#include "softfusion/drivers/lsm6ds3trc.h" +#include "softfusion/drivers/icm42688.h" +#include "softfusion/drivers/bmi270.h" +#include "softfusion/drivers/lsm6dsv.h" +#include "softfusion/drivers/lsm6dso.h" +#include "softfusion/drivers/lsm6dsr.h" +#include "softfusion/drivers/mpu6050.h" + +#include "softfusion/i2cimpl.h" #if ESP32 #include "driver/i2c.h" @@ -42,80 +49,13 @@ namespace SlimeVR { namespace Sensors { - Sensor* SensorManager::buildSensor(uint8_t sensorID, uint8_t imuType, uint8_t address, float rotation, uint8_t sclPin, uint8_t sdaPin, bool optional, int extraParam) - { - m_Logger.trace("Building IMU with: id=%d,\n\ - imuType=0x%02X, address=0x%02X, rotation=%f,\n\ - sclPin=%d, sdaPin=%d, extraParam=%d, optional=%d", - sensorID, - imuType, address, rotation, - sclPin, sdaPin, extraParam, optional); - - // Now start detecting and building the IMU - Sensor* sensor = nullptr; - - // Clear and reset I2C bus for each sensor upon startup - I2CSCAN::clearBus(sdaPin, sclPin); - swapI2C(sclPin, sdaPin); - - if (I2CSCAN::hasDevOnBus(address)) { - m_Logger.trace("Sensor %d found at address 0x%02X", sensorID + 1, address); - } else { - if (!optional) { - m_Logger.error("Mandatory sensor %d not found at address 0x%02X", sensorID + 1, address); - sensor = new ErroneousSensor(sensorID, imuType); - } - else { - m_Logger.debug("Optional sensor %d not found at address 0x%02X", sensorID + 1, address); - sensor = new EmptySensor(sensorID); - } - return sensor; - } - - switch (imuType) { - case IMU_BNO080: case IMU_BNO085: case IMU_BNO086: - // Extra param used as interrupt pin - { - uint8_t intPin = extraParam; - sensor = new BNO080Sensor(sensorID, imuType, address, rotation, sclPin, sdaPin, intPin); - } - break; - case IMU_BNO055: - sensor = new BNO055Sensor(sensorID, address, rotation, sclPin, sdaPin); - break; - case IMU_MPU9250: - sensor = new MPU9250Sensor(sensorID, address, rotation, sclPin, sdaPin); - break; - case IMU_BMI160: - // Extra param used as axis remap descriptor - { - int axisRemap = extraParam; - // Valid remap will use all axes, so there will be non-zero term in upper 9 mag bits - // Used to avoid default INT_PIN misinterpreted as axis mapping - if (axisRemap < 256) { - sensor = new BMI160Sensor(sensorID, address, rotation, sclPin, sdaPin); - } else { - sensor = new BMI160Sensor(sensorID, address, rotation, sclPin, sdaPin, axisRemap); - } - } - break; - case IMU_MPU6500: case IMU_MPU6050: - sensor = new MPU6050Sensor(sensorID, imuType, address, rotation, sclPin, sdaPin); - break; - case IMU_ICM20948: - sensor = new ICM20948Sensor(sensorID, address, rotation, sclPin, sdaPin); - break; - case IMU_ICM42688: - sensor = new ICM42688Sensor(sensorID, address, rotation, sclPin, sdaPin); - break; - default: - sensor = new ErroneousSensor(sensorID, imuType); - break; - } - - sensor->motionSetup(); - return sensor; - } + using SoftFusionLSM6DS3TRC = SoftFusionSensor; + using SoftFusionICM42688 = SoftFusionSensor; + using SoftFusionBMI270 = SoftFusionSensor; + using SoftFusionLSM6DSV = SoftFusionSensor; + using SoftFusionLSM6DSO = SoftFusionSensor; + using SoftFusionLSM6DSR = SoftFusionSensor; + using SoftFusionMPU6050 = SoftFusionSensor; // TODO Make it more generic in the future and move another place (abstract sensor interface) void SensorManager::swapI2C(uint8_t sclPin, uint8_t sdaPin) @@ -129,8 +69,8 @@ namespace SlimeVR Wire.end(); } // Disconnect pins from HWI2C - pinMode(activeSCL, INPUT); - pinMode(activeSDA, INPUT); + gpio_set_direction((gpio_num_t)activeSCL, GPIO_MODE_INPUT); + gpio_set_direction((gpio_num_t)activeSDA, GPIO_MODE_INPUT); if (running) { i2c_set_pin(I2C_NUM_0, sdaPin, sclPin, false, false, I2C_MODE_MASTER); @@ -155,15 +95,15 @@ namespace SlimeVR uint8_t sensorID = 0; uint8_t activeSensorCount = 0; -#define IMU_DESC_ENTRY(...) \ - { \ - Sensor* sensor = buildSensor(sensorID, __VA_ARGS__); \ - m_Sensors[sensorID] = sensor; \ - sensorID++; \ - if (sensor->isWorking()) { \ - m_Logger.info("Sensor %d configured", sensorID); \ - activeSensorCount++; \ - } \ +#define IMU_DESC_ENTRY(ImuType, ...) \ + { \ + auto sensor = buildSensor(sensorID, __VA_ARGS__); \ + if (sensor->isWorking()) { \ + m_Logger.info("Sensor %d configured", sensorID+1);\ + activeSensorCount++; \ + } \ + m_Sensors.push_back(std::move(sensor)); \ + sensorID++; \ } // Apply descriptor list and expand to entrys IMU_DESC_LIST; @@ -180,7 +120,7 @@ namespace SlimeVR void SensorManager::postSetup() { running = true; - for (auto sensor : m_Sensors) { + for (auto &sensor : m_Sensors) { if (sensor->isWorking()) { swapI2C(sensor->sclPin, sensor->sdaPin); sensor->postSetup(); @@ -192,7 +132,7 @@ namespace SlimeVR { // Gather IMU data bool allIMUGood = true; - for (auto sensor : m_Sensors) { + for (auto &sensor : m_Sensors) { if (sensor->isWorking()) { swapI2C(sensor->sclPin, sensor->sdaPin); sensor->motionLoop(); @@ -216,7 +156,7 @@ namespace SlimeVR uint32_t now = micros(); bool shouldSend = false; bool allSensorsReady = true; - for (auto sensor : m_Sensors) { + for (auto &sensor : m_Sensors) { if (!sensor->isWorking()) continue; if (sensor->hasNewDataToSend()) shouldSend = true; allSensorsReady &= sensor->hasNewDataToSend(); @@ -237,7 +177,7 @@ namespace SlimeVR networkConnection.beginBundle(); #endif - for (auto sensor : m_Sensors) { + for (auto &sensor : m_Sensors) { if (sensor->isWorking()) { sensor->sendData(); } diff --git a/src/sensors/SensorManager.h b/src/sensors/SensorManager.h index e68033ee7..d1bc6318d 100644 --- a/src/sensors/SensorManager.h +++ b/src/sensors/SensorManager.h @@ -27,8 +27,14 @@ #include "globals.h" #include "sensor.h" #include "EmptySensor.h" +#include "ErroneousSensor.h" #include "logging/Logger.h" +#include + +#include + + namespace SlimeVR { namespace Sensors @@ -37,34 +43,62 @@ namespace SlimeVR { public: SensorManager() - : m_Logger(SlimeVR::Logging::Logger("SensorManager")) - , m_Sensors(MAX_IMU_COUNT, nullptr) { - for (auto & u : m_Sensors) { - u = new EmptySensor(0); - } - } - ~SensorManager() - { - for (auto u : m_Sensors) { - if (u != nullptr) { - delete u; - } - } - } - + : m_Logger(SlimeVR::Logging::Logger("SensorManager")) { } void setup(); void postSetup(); void update(); - std::vector & getSensors() { return m_Sensors; }; + std::vector> & getSensors() { return m_Sensors; }; + ImuID getSensorType(size_t id) { + if(id < m_Sensors.size()) { + return m_Sensors[id]->getSensorType(); + } + return ImuID::Unknown; + } private: SlimeVR::Logging::Logger m_Logger; - std::vector m_Sensors; - Sensor* buildSensor(uint8_t sensorID, uint8_t imuType, uint8_t address, float rotation, uint8_t sclPin, uint8_t sdaPin, bool optional = false, int extraParam = 0); - + std::vector> m_Sensors; + + template + std::unique_ptr buildSensor(uint8_t sensorID, uint8_t addrSuppl, float rotation, uint8_t sclPin, uint8_t sdaPin, bool optional = false, int extraParam = 0) + { + const uint8_t address = ImuType::Address + addrSuppl; + m_Logger.trace("Building IMU with: id=%d,\n\ + address=0x%02X, rotation=%f,\n\ + sclPin=%d, sdaPin=%d, extraParam=%d, optional=%d", + sensorID, address, rotation, + sclPin, sdaPin, extraParam, optional); + + // Now start detecting and building the IMU + std::unique_ptr sensor; + + // Clear and reset I2C bus for each sensor upon startup + I2CSCAN::clearBus(sdaPin, sclPin); + swapI2C(sclPin, sdaPin); + + if (I2CSCAN::hasDevOnBus(address)) { + m_Logger.trace("Sensor %d found at address 0x%02X", sensorID + 1, address); + } else { + if (!optional) { + m_Logger.error("Mandatory sensor %d not found at address 0x%02X", sensorID + 1, address); + sensor = std::make_unique(sensorID, ImuType::TypeID); + } + else { + m_Logger.debug("Optional sensor %d not found at address 0x%02X", sensorID + 1, address); + sensor = std::make_unique(sensorID); + } + return sensor; + } + + uint8_t intPin = extraParam; + sensor = std::make_unique(sensorID, addrSuppl, rotation, sclPin, sdaPin, intPin); + + sensor->motionSetup(); + return sensor; + } uint8_t activeSCL = 0; uint8_t activeSDA = 0; bool running = false; diff --git a/src/sensors/bmi160sensor.cpp b/src/sensors/bmi160sensor.cpp index a5b5482fc..c682c6557 100644 --- a/src/sensors/bmi160sensor.cpp +++ b/src/sensors/bmi160sensor.cpp @@ -332,6 +332,7 @@ void BMI160Sensor::motionLoop() { #endif optimistic_yield(100); if (!sfusion.isUpdated()) return; + hadData = true; sfusion.clearUpdated(); } } diff --git a/src/sensors/bmi160sensor.h b/src/sensors/bmi160sensor.h index 9ebfa7850..6d842e9a8 100644 --- a/src/sensors/bmi160sensor.h +++ b/src/sensors/bmi160sensor.h @@ -122,11 +122,19 @@ static_assert(0x7FFF * BMI160_TEMP_CALIBRATION_REQUIRED_SAMPLES_PER_STEP < 0x7FF class BMI160Sensor : public Sensor { public: - BMI160Sensor(uint8_t id, uint8_t address, float rotation, uint8_t sclPin, uint8_t sdaPin, int axisRemap=AXIS_REMAP_DEFAULT) : - Sensor("BMI160Sensor", IMU_BMI160, id, address, rotation, sclPin, sdaPin), - axisRemap(axisRemap), + static constexpr uint8_t Address = 0x68; + static constexpr auto TypeID = ImuID::BMI160; + + BMI160Sensor(uint8_t id, uint8_t addrSuppl, float rotation, uint8_t sclPin, uint8_t sdaPin, int axisRemapParam) : + Sensor("BMI160Sensor", ImuID::BMI160, id, Address+addrSuppl, rotation, sclPin, sdaPin), sfusion(BMI160_ODR_GYR_MICROS / 1e6f, BMI160_ODR_ACC_MICROS / 1e6f, BMI160_ODR_MAG_MICROS / 1e6f) { + if (axisRemapParam < 256) { + axisRemap = AXIS_REMAP_DEFAULT; + } + else { + axisRemap = axisRemapParam; + } }; ~BMI160Sensor(){}; void initHMC(BMI160MagRate magRate); @@ -167,6 +175,7 @@ class BMI160Sensor : public Sensor { void getRemappedAcceleration(int16_t* x, int16_t* y, int16_t* z); bool getTemperature(float* out); + private: BMI160 imu {}; int axisRemap; diff --git a/src/sensors/bno055sensor.cpp b/src/sensors/bno055sensor.cpp index 92b8baf87..def918c48 100644 --- a/src/sensors/bno055sensor.cpp +++ b/src/sensors/bno055sensor.cpp @@ -45,7 +45,6 @@ void BNO055Sensor::motionSetup() { m_Logger.info("Connected to BNO055 at address 0x%02x", addr); working = true; - configured = true; } void BNO055Sensor::motionLoop() { @@ -61,6 +60,7 @@ void BNO055Sensor::motionLoop() { // TODO Optimize a bit with setting rawQuat directly setFusedRotation(imu.getQuat()); + hadData = true; #if SEND_ACCELERATION setAcceleration(imu.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL)); diff --git a/src/sensors/bno055sensor.h b/src/sensors/bno055sensor.h index 0694643a0..0035cc85e 100644 --- a/src/sensors/bno055sensor.h +++ b/src/sensors/bno055sensor.h @@ -31,8 +31,11 @@ class BNO055Sensor : public Sensor { public: - BNO055Sensor(uint8_t id, uint8_t address, float rotation, uint8_t sclPin, uint8_t sdaPin) - : Sensor("BNO055Sensor", IMU_BNO055, id, address, rotation, sclPin, sdaPin){}; + static constexpr auto TypeID = ImuID::BNO055; + static constexpr uint8_t Address = 0x28; + + BNO055Sensor(uint8_t id, uint8_t addrSuppl, float rotation, uint8_t sclPin, uint8_t sdaPin, uint8_t) + : Sensor("BNO055Sensor", ImuID::BNO055, id, Address+addrSuppl, rotation, sclPin, sdaPin){}; ~BNO055Sensor(){}; void motionSetup() override final; void motionLoop() override final; diff --git a/src/sensors/bno080sensor.cpp b/src/sensors/bno080sensor.cpp index 7d7c0c2cf..bc7594ea0 100644 --- a/src/sensors/bno080sensor.cpp +++ b/src/sensors/bno080sensor.cpp @@ -54,7 +54,7 @@ void BNO080Sensor::motionSetup() this->imu.enableLinearAccelerometer(10); #if USE_6_AXIS - if ((sensorType == IMU_BNO085 || sensorType == IMU_BNO086) && BNO_USE_ARVR_STABILIZATION) { + if ((sensorType == ImuID::BNO085 || sensorType == ImuID::BNO086) && BNO_USE_ARVR_STABILIZATION) { imu.enableARVRStabilizedGameRotationVector(10); } else { imu.enableGameRotationVector(10); @@ -64,7 +64,7 @@ void BNO080Sensor::motionSetup() imu.enableRotationVector(1000); #endif #else - if ((sensorType == IMU_BNO085 || sensorType == IMU_BNO086) && BNO_USE_ARVR_STABILIZATION) { + if ((sensorType == ImuID::BNO085 || sensorType == ImuID::BNO086) && BNO_USE_ARVR_STABILIZATION) { imu.enableARVRStabilizedRotationVector(10); } else { imu.enableRotationVector(10); diff --git a/src/sensors/bno080sensor.h b/src/sensors/bno080sensor.h index 54b413f78..6056121aa 100644 --- a/src/sensors/bno080sensor.h +++ b/src/sensors/bno080sensor.h @@ -30,8 +30,11 @@ class BNO080Sensor : public Sensor { public: - BNO080Sensor(uint8_t id, uint8_t type, uint8_t address, float rotation, uint8_t sclPin, uint8_t sdaPin, uint8_t intPin) - : Sensor("BNO080Sensor", type, id, address, rotation, sclPin, sdaPin), m_IntPin(intPin) {}; + static constexpr auto TypeID = ImuID::BNO080; + static constexpr uint8_t Address = 0x4a; + + BNO080Sensor(uint8_t id, uint8_t addrSuppl, float rotation, uint8_t sclPin, uint8_t sdaPin, uint8_t intPin) + : Sensor("BNO080Sensor", ImuID::BNO080, id, Address+addrSuppl, rotation, sclPin, sdaPin), m_IntPin(intPin) {}; ~BNO080Sensor(){}; void motionSetup() override final; void postSetup() override { @@ -43,6 +46,10 @@ class BNO080Sensor : public Sensor void startCalibration(int calibrationType) override final; SensorStatus getSensorState() override final; +protected: + // forwarding constructor + BNO080Sensor(const char* sensorName, ImuID imuId, uint8_t id, uint8_t addrSuppl, float rotation, uint8_t sclPin, uint8_t sdaPin, uint8_t intPin) + : Sensor(sensorName, imuId, id, Address+addrSuppl, rotation, sclPin, sdaPin), m_IntPin(intPin) {}; private: BNO080 imu{}; @@ -58,6 +65,23 @@ class BNO080Sensor : public Sensor uint8_t magCalibrationAccuracy = 0; float magneticAccuracyEstimate = 999; bool newMagData = false; + bool configured = false; +}; + +class BNO085Sensor : public BNO080Sensor +{ +public: + static constexpr auto TypeID = ImuID::BNO085; + BNO085Sensor(uint8_t id, uint8_t address, float rotation, uint8_t sclPin, uint8_t sdaPin, uint8_t intPin) + : BNO080Sensor("BNO085Sensor", ImuID::BNO085, id, address, rotation, sclPin, sdaPin, intPin) {}; +}; + +class BNO086Sensor : public BNO080Sensor +{ +public: + static constexpr auto TypeID = ImuID::BNO086; + BNO086Sensor(uint8_t id, uint8_t address, float rotation, uint8_t sclPin, uint8_t sdaPin, uint8_t intPin) + : BNO080Sensor("BNO086Sensor", ImuID::BNO086, id, address, rotation, sclPin, sdaPin, intPin) {}; }; #endif diff --git a/src/sensors/icm20948sensor.cpp b/src/sensors/icm20948sensor.cpp index f0580bcf8..3c39beca5 100644 --- a/src/sensors/icm20948sensor.cpp +++ b/src/sensors/icm20948sensor.cpp @@ -98,6 +98,7 @@ void ICM20948Sensor::readFIFOToEnd() // Performance Test // cntbuf ++; hasdata = true; + hadData = true; readFIFOToEnd(); } } @@ -319,7 +320,6 @@ void ICM20948Sensor::startMotionLoop() { lastData = millis(); working = true; - hadData = true; } void ICM20948Sensor::checkSensorTimeout() diff --git a/src/sensors/icm20948sensor.h b/src/sensors/icm20948sensor.h index f422db06c..2f213e492 100644 --- a/src/sensors/icm20948sensor.h +++ b/src/sensors/icm20948sensor.h @@ -30,8 +30,11 @@ class ICM20948Sensor : public Sensor { public: - ICM20948Sensor(uint8_t id, uint8_t address, float rotation, uint8_t sclPin, uint8_t sdaPin) - : Sensor("ICM20948Sensor", IMU_ICM20948, id, address, rotation, sclPin, sdaPin) {} + static constexpr auto TypeID = ImuID::ICM20948; + static constexpr uint8_t Address = 0x68; + + ICM20948Sensor(uint8_t id, uint8_t addrSuppl, float rotation, uint8_t sclPin, uint8_t sdaPin, uint8_t) + : Sensor("ICM20948Sensor", ImuID::ICM20948, id, Address+addrSuppl, rotation, sclPin, sdaPin) {} ~ICM20948Sensor() override = default; void motionSetup() override final; void postSetup() override { diff --git a/src/sensors/icm42688sensor.cpp b/src/sensors/icm42688sensor.cpp deleted file mode 100644 index 8bc017325..000000000 --- a/src/sensors/icm42688sensor.cpp +++ /dev/null @@ -1,345 +0,0 @@ -/* - SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain, S.J. Remington & 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 "icm42688sensor.h" -#include "globals.h" -#include "helper_3dmath.h" -#include -#include "calibration.h" -#include "magneto1.4.h" -#include "GlobalVars.h" -#include "mahony.h" -// #include "madgwick.h" - -constexpr float gscale = (2000. / 32768.0) * (PI / 180.0); // gyro LSB/d/s -> rad/s -constexpr float ascale = (8. / 32768.) * CONST_EARTH_GRAVITY; // accel LSB/G -> m/s^2 - -void ICM42688Sensor::motionSetup() { - // initialize device - uint8_t temp; - I2Cdev::readByte(addr, ICM42688_WHO_AM_I, &temp); - if(!(temp == 0x47 || temp == 0xDB)) { - m_Logger.fatal("Can't connect to ICM42688 (reported device ID 0x%02x) at address 0x%02x", temp, addr); - return; - } - - m_Logger.info("Connected to ICM42688 (reported device ID 0x%02x) at address 0x%02x", temp, addr); - - if (I2CSCAN::hasDevOnBus(addr_mag)) { - I2Cdev::readByte(addr_mag, MMC5983MA_PRODUCT_ID, &temp); - if(!(temp == 0x30)) { - m_Logger.fatal("Can't connect to MMC5983MA (reported device ID 0x%02x) at address 0x%02x", temp, addr_mag); - m_Logger.info("Magnetometer unavailable!"); - magExists = false; - } else { - m_Logger.info("Connected to MMC5983MA (reported device ID 0x%02x) at address 0x%02x", temp, addr_mag); - magExists = true; - } - } else { - m_Logger.info("Magnetometer unavailable!"); - magExists = false; - } - - if (magExists) { - I2Cdev::writeByte(addr_mag, MMC5983MA_CONTROL_1, 0x80); // Reset MMC now - } - - I2Cdev::writeByte(addr, ICM42688_DEVICE_CONFIG, 1); // reset - delay(2); // wait 1ms for reset - I2Cdev::readByte(addr, ICM42688_INT_STATUS, &temp); // clear reset done int flag - I2Cdev::writeByte(addr, ICM42688_INT_SOURCE0, 0); // disable ints - I2Cdev::writeByte(addr, ICM42688_REG_BANK_SEL, 0x00); // select register bank 0 - I2Cdev::writeByte(addr, ICM42688_PWR_MGMT0, gMode_LN << 2 | aMode_LN); // set accel and gyro modes (low noise) - delay(1); // wait >200us (datasheet 14.36) - I2Cdev::writeByte(addr, ICM42688_ACCEL_CONFIG0, AFS_8G << 5 | AODR_200Hz); // set accel ODR and FS (200hz, 8g) - I2Cdev::writeByte(addr, ICM42688_GYRO_CONFIG0, GFS_2000DPS << 5 | GODR_1kHz); // set gyro ODR and FS (1khz, 2000dps) - I2Cdev::writeByte(addr, ICM42688_GYRO_ACCEL_CONFIG0, 0x44); // set gyro and accel bandwidth to ODR/10 - delay(50); // 10ms Accel, 30ms Gyro startup - - if (magExists) { - I2Cdev::writeByte(addr_mag, MMC5983MA_CONTROL_0, 0x08); // SET - delayMicroseconds(1); // auto clear after 500ns - I2Cdev::writeByte(addr_mag, MMC5983MA_CONTROL_0, 0x20); // auto SET/RESET - I2Cdev::writeByte(addr_mag, MMC5983MA_CONTROL_1, MBW_400Hz); // set mag BW (400Hz or ~50% duty cycle with 200Hz ODR) - I2Cdev::writeByte(addr_mag, MMC5983MA_CONTROL_2, 0x80 | (MSET_2000 << 4) | 0x08 | MODR_200Hz); // continuous measurement mode, set sample rate, auto SET/RESET, set SET/RESET rate (200Hz ODR, 2000 samples between SET/RESET) - } - - // turn on while flip back to calibrate. then, flip again after 5 seconds. - // TODO: Move calibration invoke after calibrate button on slimeVR server available - accel_read(); - if(Gxyz[2] < -0.75f) { - ledManager.on(); - m_Logger.info("Flip front to confirm start calibration"); - delay(5000); - ledManager.off(); - - accel_read(); - if(Gxyz[2] > 0.75f) { - m_Logger.debug("Starting calibration..."); - startCalibration(0); - } - } - - // Initialize the configuration - { - SlimeVR::Configuration::CalibrationConfig sensorCalibration = configuration.getCalibration(sensorId); - // If no compatible calibration data is found, the calibration data will just be zero-ed out - switch (sensorCalibration.type) { - case SlimeVR::Configuration::CalibrationConfigType::ICM42688: - m_Calibration = sensorCalibration.data.icm42688; - break; - - case SlimeVR::Configuration::CalibrationConfigType::NONE: - m_Logger.warn("No calibration data found for sensor %d, ignoring...", sensorId); - m_Logger.info("Calibration is advised"); - break; - - default: - m_Logger.warn("Incompatible calibration data found for sensor %d, ignoring...", sensorId); - m_Logger.info("Calibration is advised"); - } - } - - I2Cdev::writeByte(addr, ICM42688_FIFO_CONFIG, 0x00); // FIFO bypass mode - I2Cdev::writeByte(addr, ICM42688_FSYNC_CONFIG, 0x00); // disable FSYNC - I2Cdev::readByte(addr, ICM42688_TMST_CONFIG, &temp); // disable FSYNC - I2Cdev::writeByte(addr, ICM42688_TMST_CONFIG, temp & 0xfd); // disable FSYNC - I2Cdev::writeByte(addr, ICM42688_FIFO_CONFIG1, 0x02); // enable FIFO gyro only - I2Cdev::writeByte(addr, ICM42688_FIFO_CONFIG, 1<<6); // begin FIFO stream - - working = true; - configured = true; -} - -void ICM42688Sensor::motionLoop() { - uint8_t rawCount[2]; - I2Cdev::readBytes(addr, ICM42688_FIFO_COUNTH, 2, &rawCount[0]); - uint16_t count = (uint16_t)(rawCount[0] << 8 | rawCount[1]); // Turn the 16 bits into a unsigned 16-bit value - count += 32; // Add a few read buffer packets (4 ms) - uint16_t packets = count / 8; // Packet size 8 bytes - uint8_t rawData[2080]; - I2Cdev::readBytes(addr, ICM42688_FIFO_DATA, count, &rawData[0]); // Read buffer - - accel_read(); - parseAccelData(); - - if (magExists) { - mag_read(); - parseMagData(); - } - - for (uint16_t i = 0; i < packets; i++) { - uint16_t index = i * 8; // Packet size 8 bytes - if ((rawData[index] & 0x80) == 0x80) { - continue; // Skip empty packets - } - // combine into 16 bit values - float raw0 = (int16_t)((((int16_t)rawData[index + 1]) << 8) | rawData[index + 2]); // gx - float raw1 = (int16_t)((((int16_t)rawData[index + 3]) << 8) | rawData[index + 4]); // gy - float raw2 = (int16_t)((((int16_t)rawData[index + 5]) << 8) | rawData[index + 6]); // gz - if (raw0 < -32766 || raw1 < -32766 || raw2 < -32766) { - continue; // Skip invalid data - } - Gxyz[0] = raw0 * gscale; //gres - Gxyz[1] = raw1 * gscale; //gres - Gxyz[2] = raw2 * gscale; //gres - parseGyroData(); - - // TODO: mag axes will be different, make sure to change them??? - sfusion.updateGyro(Gxyz); - } - - sfusion.updateAcc(Axyz); - - if (magExists) - sfusion.updateMag(Mxyz); - - setFusedRotation(sfusion.getQuaternionQuat()); - setAcceleration(sfusion.getLinearAccVec()); -} - -void ICM42688Sensor::accel_read() { - uint8_t rawAccel[6]; - I2Cdev::readBytes(addr, ICM42688_ACCEL_DATA_X1, 6, &rawAccel[0]); - float raw0 = (int16_t)((((int16_t)rawAccel[0]) << 8) | rawAccel[1]); - float raw1 = (int16_t)((((int16_t)rawAccel[2]) << 8) | rawAccel[3]); - float raw2 = (int16_t)((((int16_t)rawAccel[4]) << 8) | rawAccel[5]); - Axyz[0] = raw0 * ascale; - Axyz[1] = raw1 * ascale; - Axyz[2] = raw2 * ascale; -} - -void ICM42688Sensor::gyro_read() { - uint8_t rawGyro[6]; - I2Cdev::readBytes(addr, ICM42688_GYRO_DATA_X1, 6, &rawGyro[0]); - float raw0 = (int16_t)((((int16_t)rawGyro[0]) << 8) | rawGyro[1]); - float raw1 = (int16_t)((((int16_t)rawGyro[2]) << 8) | rawGyro[3]); - float raw2 = (int16_t)((((int16_t)rawGyro[4]) << 8) | rawGyro[5]); - Gxyz[0] = raw0 * gscale; - Gxyz[1] = raw1 * gscale; - Gxyz[2] = raw2 * gscale; -} - -void ICM42688Sensor::mag_read() { - if (!magExists) return; - uint8_t rawMag[7]; - I2Cdev::readBytes(addr_mag, MMC5983MA_XOUT_0, 7, &rawMag[0]); - double raw0 = (uint32_t)(rawMag[0] << 10 | rawMag[1] << 2 | (rawMag[6] & 0xC0) >> 6); - double raw1 = (uint32_t)(rawMag[2] << 10 | rawMag[3] << 2 | (rawMag[6] & 0x30) >> 4); - double raw2 = (uint32_t)(rawMag[4] << 10 | rawMag[5] << 2 | (rawMag[6] & 0x0C) >> 2); - Mxyz[0] = (raw0 - MMC5983MA_offset) * MMC5983MA_mRes; - Mxyz[1] = (raw1 - MMC5983MA_offset) * MMC5983MA_mRes; - Mxyz[2] = (raw2 - MMC5983MA_offset) * MMC5983MA_mRes; -} - -void ICM42688Sensor::startCalibration(int calibrationType) { - ledManager.on(); - m_Logger.debug("Gathering raw data for device calibration..."); - constexpr int calibrationSamples = 500; - double GxyzC[3] = {0, 0, 0}; - - // Wait for sensor to calm down before calibration - m_Logger.info("Put down the device and wait for baseline gyro reading calibration"); - delay(2000); - - for (int i = 0; i < calibrationSamples; i++) { - delay(5); - gyro_read(); - GxyzC[0] += Gxyz[0]; - GxyzC[1] += Gxyz[1]; - GxyzC[2] += Gxyz[2]; - } - GxyzC[0] /= calibrationSamples; - GxyzC[1] /= calibrationSamples; - GxyzC[2] /= calibrationSamples; - -#ifdef DEBUG_SENSOR - m_Logger.trace("Gyro calibration results: %f %f %f", GxyzC[0], GxyzC[1], GxyzC[2]); -#endif - - // TODO: use offset registers? - m_Calibration.G_off[0] = GxyzC[0]; - m_Calibration.G_off[1] = GxyzC[1]; - m_Calibration.G_off[2] = GxyzC[2]; - - // Blink calibrating led before user should rotate the sensor - m_Logger.info("Gently rotate the device while it's gathering accelerometer and magnetometer data"); - ledManager.pattern(15, 300, 3000/310); - - MagnetoCalibration *magneto_acc = new MagnetoCalibration(); - MagnetoCalibration *magneto_mag = new MagnetoCalibration(); - - // NOTE: we don't use the FIFO here on *purpose*. This makes the difference between a calibration that takes a second or three and a calibration that takes much longer. - for (int i = 0; i < calibrationSamples; i++) { - ledManager.on(); - accel_read(); - magneto_acc->sample(Axyz[0], Axyz[1], Axyz[2]); - mag_read(); - magneto_mag->sample(Mxyz[0], Mxyz[1], Mxyz[2]); - - ledManager.off(); - delay(50); - } - m_Logger.debug("Calculating calibration data..."); - - float A_BAinv[4][3]; - magneto_acc->current_calibration(A_BAinv); - delete magneto_acc; - - float M_BAinv[4][3]; - if (magExists) { - magneto_mag->current_calibration(M_BAinv); - } - delete magneto_mag; - - m_Logger.debug("Finished Calculate Calibration data"); - m_Logger.debug("Accelerometer calibration matrix:"); - m_Logger.debug("{"); - for (int i = 0; i < 3; i++) - { - m_Calibration.A_B[i] = A_BAinv[0][i]; - m_Calibration.A_Ainv[0][i] = A_BAinv[1][i]; - m_Calibration.A_Ainv[1][i] = A_BAinv[2][i]; - m_Calibration.A_Ainv[2][i] = A_BAinv[3][i]; - m_Logger.debug(" %f, %f, %f, %f", A_BAinv[0][i], A_BAinv[1][i], A_BAinv[2][i], A_BAinv[3][i]); - } - m_Logger.debug("}"); - if (magExists) { - m_Logger.debug("[INFO] Magnetometer calibration matrix:"); - m_Logger.debug("{"); - for (int i = 0; i < 3; i++) { - m_Calibration.M_B[i] = M_BAinv[0][i]; - m_Calibration.M_Ainv[0][i] = M_BAinv[1][i]; - m_Calibration.M_Ainv[1][i] = M_BAinv[2][i]; - m_Calibration.M_Ainv[2][i] = M_BAinv[3][i]; - m_Logger.debug(" %f, %f, %f, %f", M_BAinv[0][i], M_BAinv[1][i], M_BAinv[2][i], M_BAinv[3][i]); - } - m_Logger.debug("}"); - } - - m_Logger.debug("Saving the calibration data"); - - SlimeVR::Configuration::CalibrationConfig calibration; - calibration.type = SlimeVR::Configuration::CalibrationConfigType::ICM42688; - calibration.data.icm42688 = m_Calibration; - configuration.setCalibration(sensorId, calibration); - configuration.save(); - - ledManager.off(); - m_Logger.debug("Saved the calibration data"); - - m_Logger.info("Calibration data gathered"); - I2Cdev::writeByte(addr, ICM42688_SIGNAL_PATH_RESET, 0x02); // flush FIFO before return -} - -void ICM42688Sensor::parseMagData() { - float temp[3]; - - //apply offsets and scale factors from Magneto - for (unsigned i = 0; i < 3; i++) { - temp[i] = (Mxyz[i] - m_Calibration.M_B[i]); - #if useFullCalibrationMatrix == true - Mxyz[i] = m_Calibration.M_Ainv[i][0] * temp[0] + m_Calibration.M_Ainv[i][1] * temp[1] + m_Calibration.M_Ainv[i][2] * temp[2]; - #else - Mxyz[i] = temp[i]; - #endif - } -} - -void ICM42688Sensor::parseAccelData() { - float temp[3]; - - //apply offsets (bias) and scale factors from Magneto - for (unsigned i = 0; i < 3; i++) { - temp[i] = (Axyz[i] - m_Calibration.A_B[i]); - #if useFullCalibrationMatrix == true - Axyz[i] = m_Calibration.A_Ainv[i][0] * temp[0] + m_Calibration.A_Ainv[i][1] * temp[1] + m_Calibration.A_Ainv[i][2] * temp[2]; - #else - Axyz[i] = temp[i]; - #endif - } -} - -void ICM42688Sensor::parseGyroData() { - Gxyz[0] = (Gxyz[0] - m_Calibration.G_off[0]); - Gxyz[1] = (Gxyz[1] - m_Calibration.G_off[1]); - Gxyz[2] = (Gxyz[2] - m_Calibration.G_off[2]); -} diff --git a/src/sensors/icm42688sensor.h b/src/sensors/icm42688sensor.h deleted file mode 100644 index dc3b936cb..000000000 --- a/src/sensors/icm42688sensor.h +++ /dev/null @@ -1,69 +0,0 @@ -/* - SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain & 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. -*/ - -#ifndef SENSORS_ICM42688SENSOR_H -#define SENSORS_ICM42688SENSOR_H - -#include "sensor.h" -#include "logging/Logger.h" - -#include -#include -#include "I2Cdev.h" - -#include "SensorFusion.h" - -class ICM42688Sensor : public Sensor -{ -public: - ICM42688Sensor(uint8_t id, uint8_t address, float rotation, uint8_t sclPin, uint8_t sdaPin) - : Sensor("ICM42688Sensor", IMU_ICM42688, id, address, rotation, sclPin, sdaPin), - sfusion(0.001f, 0.01f, 0.01f){}; - ~ICM42688Sensor(){}; - void motionSetup() override final; - void motionLoop() override final; - void startCalibration(int calibrationType) override final; - -private: - uint8_t addr_mag = 0x30; - bool magExists = false; - - // raw data and scaled as vector - float Axyz[3]{}; - float Gxyz[3]{}; - float Mxyz[3]{}; - - SlimeVR::Sensors::SensorFusion sfusion; - - SlimeVR::Configuration::ICM42688CalibrationConfig m_Calibration; - - void accel_read(); - void gyro_read(); - void mag_read(); - - void parseAccelData(); - void parseGyroData(); - void parseMagData(); -}; - -#endif diff --git a/src/sensors/mpu6050sensor.cpp b/src/sensors/mpu6050sensor.cpp index f3691ea94..44ac31591 100644 --- a/src/sensors/mpu6050sensor.cpp +++ b/src/sensors/mpu6050sensor.cpp @@ -106,7 +106,6 @@ void MPU6050Sensor::motionSetup() packetSize = imu.dmpGetFIFOPacketSize(); working = true; - configured = true; } else { @@ -136,6 +135,7 @@ void MPU6050Sensor::motionLoop() if (imu.dmpGetCurrentFIFOPacket(fifoBuffer)) { imu.dmpGetQuaternion(&rawQuat, fifoBuffer); + hadData = true; sfusion.updateQuaternion(rawQuat); diff --git a/src/sensors/mpu6050sensor.h b/src/sensors/mpu6050sensor.h index d7a7b6d90..c5667798f 100644 --- a/src/sensors/mpu6050sensor.h +++ b/src/sensors/mpu6050sensor.h @@ -31,8 +31,11 @@ class MPU6050Sensor : public Sensor { public: - MPU6050Sensor(uint8_t id, uint8_t type, uint8_t address, float rotation, uint8_t sclPin, uint8_t sdaPin) - : Sensor("MPU6050Sensor", type, id, address, rotation, sclPin, sdaPin){}; + static constexpr auto TypeID = ImuID::MPU6050; + static constexpr uint8_t Address = 0x68; + + MPU6050Sensor(uint8_t id, uint8_t addrSuppl, float rotation, uint8_t sclPin, uint8_t sdaPin, uint8_t) + : Sensor("MPU6050Sensor", ImuID::MPU6050, id, Address+addrSuppl, rotation, sclPin, sdaPin){}; ~MPU6050Sensor(){}; void motionSetup() override final; void motionLoop() override final; diff --git a/src/sensors/mpu9250sensor.cpp b/src/sensors/mpu9250sensor.cpp index 85db8db25..a4295975f 100644 --- a/src/sensors/mpu9250sensor.cpp +++ b/src/sensors/mpu9250sensor.cpp @@ -130,7 +130,6 @@ void MPU9250Sensor::motionSetup() { imu.setFIFOEnabled(true); working = true; - configured = true; #endif } @@ -154,6 +153,7 @@ void MPU9250Sensor::motionLoop() { uint8_t dmpPacket[packetSize]; if(!imu.GetCurrentFIFOPacket(dmpPacket, packetSize)) return; if(imu.dmpGetQuaternion(&rawQuat, dmpPacket)) return; // FIFO CORRUPTED + hadData = true; sfusion.updateQuaternion(rawQuat); diff --git a/src/sensors/mpu9250sensor.h b/src/sensors/mpu9250sensor.h index 612657974..25140cb90 100644 --- a/src/sensors/mpu9250sensor.h +++ b/src/sensors/mpu9250sensor.h @@ -44,8 +44,11 @@ constexpr float MPU9250_ODR_TS = ( 1.0f / MPU9250_DEFAULT_ODR_HZ) * (1+MPU9250_S class MPU9250Sensor : public Sensor { public: - MPU9250Sensor(uint8_t id, uint8_t address, float rotation, uint8_t sclPin, uint8_t sdaPin) - : Sensor("MPU9250Sensor", IMU_MPU9250, id, address, rotation, sclPin, sdaPin) + static constexpr auto TypeID = ImuID::MPU9250; + static constexpr uint8_t Address = 0x68; + + MPU9250Sensor(uint8_t id, uint8_t addrSuppl, float rotation, uint8_t sclPin, uint8_t sdaPin, uint8_t) + : Sensor("MPU9250Sensor", ImuID::MPU9250, id, Address+addrSuppl, rotation, sclPin, sdaPin) #if !MPU_USE_DMPMAG , sfusion(MPU9250_ODR_TS) #endif diff --git a/src/sensors/sensor.cpp b/src/sensors/sensor.cpp index b992b3485..94299f12c 100644 --- a/src/sensors/sensor.cpp +++ b/src/sensors/sensor.cpp @@ -69,28 +69,41 @@ void Sensor::printDebugTemperatureCalibrationState() { printTemperatureCalibrati void Sensor::saveTemperatureCalibration() { printTemperatureCalibrationUnsupported(); }; void Sensor::resetTemperatureCalibrationState() { printTemperatureCalibrationUnsupported(); }; -const char * getIMUNameByType(int imuType) { +const char * getIMUNameByType(ImuID imuType) { switch(imuType) { - case IMU_MPU9250: + case ImuID::MPU9250: return "MPU9250"; - case IMU_MPU6500: + case ImuID::MPU6500: return "MPU6500"; - case IMU_BNO080: + case ImuID::BNO080: return "BNO080"; - case IMU_BNO085: + case ImuID::BNO085: return "BNO085"; - case IMU_BNO055: + case ImuID::BNO055: return "BNO055"; - case IMU_MPU6050: + case ImuID::MPU6050: return "MPU6050"; - case IMU_BNO086: + case ImuID::BNO086: return "BNO086"; - case IMU_BMI160: + case ImuID::BMI160: return "BMI160"; - case IMU_ICM20948: + case ImuID::ICM20948: return "ICM20948"; - case IMU_ICM42688: + case ImuID::ICM42688: return "ICM42688"; + case ImuID::BMI270: + return "BMI270"; + case ImuID::LSM6DS3TRC: + return "LSM6DS3TRC"; + case ImuID::LSM6DSV: + return "LSM6DSV"; + case ImuID::LSM6DSO: + return "LSM6DSO"; + case ImuID::LSM6DSR: + return "LSM6DSR"; + case ImuID::Unknown: + case ImuID::Empty: + return "UNKNOWN"; } return "Unknown"; } diff --git a/src/sensors/sensor.h b/src/sensors/sensor.h index 6d603a8c8..f225aebd3 100644 --- a/src/sensors/sensor.h +++ b/src/sensors/sensor.h @@ -44,7 +44,7 @@ enum class SensorStatus : uint8_t { class Sensor { public: - Sensor(const char *sensorName, uint8_t type, uint8_t id, uint8_t address, float rotation, uint8_t sclpin=0, uint8_t sdapin=0) + Sensor(const char *sensorName, ImuID type, uint8_t id, uint8_t address, float rotation, uint8_t sclpin=0, uint8_t sdapin=0) : addr(address), sensorId(id), sensorType(type), sensorOffset({Quat(Vector3(0, 0, 1), rotation)}), m_Logger(SlimeVR::Logging::Logger(sensorName)), sclPin(sclpin), sdaPin(sdapin) { @@ -69,13 +69,16 @@ class Sensor bool isWorking() { return working; }; + bool getHadData() const { + return hadData; + }; bool isValid() { return sclPin != sdaPin; }; uint8_t getSensorId() { return sensorId; }; - uint8_t getSensorType() { + ImuID getSensorType() { return sensorType; }; const Vector3& getAcceleration() { @@ -88,13 +91,12 @@ class Sensor return newFusedRotation || newAcceleration; }; - bool hadData = false; protected: uint8_t addr = 0; uint8_t sensorId = 0; - uint8_t sensorType = 0; - bool configured = false; + ImuID sensorType = ImuID::Unknown; bool working = false; + bool hadData = false; uint8_t calibrationAccuracy = 0; Quat sensorOffset; @@ -105,7 +107,7 @@ class Sensor bool newAcceleration = false; Vector3 acceleration{}; - SlimeVR::Logging::Logger m_Logger; + mutable SlimeVR::Logging::Logger m_Logger; public: uint8_t sclPin = 0; @@ -115,6 +117,6 @@ class Sensor void printTemperatureCalibrationUnsupported(); }; -const char * getIMUNameByType(int imuType); +const char * getIMUNameByType(ImuID imuType); #endif // SLIMEVR_SENSOR_H_ diff --git a/src/sensors/sensoraddresses.h b/src/sensors/sensoraddresses.h index 567120d45..0282e32f4 100644 --- a/src/sensors/sensoraddresses.h +++ b/src/sensors/sensoraddresses.h @@ -1,35 +1,7 @@ -//This is useful if you want to use an address shifter -#define DEFAULT_IMU_ADDRESS true +// those variables are used as "supplement" to base IMU address coming directly from IMU driver +// they are remaining to keep backward compatibility with old style of defines.h -//We use fixed address values instead of scanning to keep the sensorID consistent and to avoid issues with some breakout board with multiple active ic2 addresses -#if DEFAULT_IMU_ADDRESS - - #if IMU == IMU_BNO080 || IMU == IMU_BNO085 || IMU == IMU_BNO086 - #define PRIMARY_IMU_ADDRESS_ONE 0x4A - #define PRIMARY_IMU_ADDRESS_TWO 0x4B - #elif IMU == IMU_BNO055 - #define PRIMARY_IMU_ADDRESS_ONE 0x29 - #define PRIMARY_IMU_ADDRESS_TWO 0x28 - #elif IMU == IMU_MPU9250 || IMU == IMU_BMI160 || IMU == IMU_MPU6500 || IMU == IMU_MPU6050 || IMU == IMU_ICM20948 || IMU == IMU_ICM42688 - #define PRIMARY_IMU_ADDRESS_ONE 0x68 - #define PRIMARY_IMU_ADDRESS_TWO 0x69 - #endif - - #if SECOND_IMU == IMU_BNO080 || SECOND_IMU == IMU_BNO085 || SECOND_IMU == IMU_BNO086 - #define SECONDARY_IMU_ADDRESS_ONE 0x4A - #define SECONDARY_IMU_ADDRESS_TWO 0x4B - #elif SECOND_IMU == IMU_BNO055 - #define SECONDARY_IMU_ADDRESS_ONE 0x29 - #define SECONDARY_IMU_ADDRESS_TWO 0x28 - #elif SECOND_IMU == IMU_MPU9250 || SECOND_IMU == IMU_BMI160 || SECOND_IMU == IMU_MPU6500 || SECOND_IMU == IMU_MPU6050 || SECOND_IMU == IMU_ICM20948 || SECOND_IMU == IMU_ICM42688 - #define SECONDARY_IMU_ADDRESS_ONE 0x68 - #define SECONDARY_IMU_ADDRESS_TWO 0x69 - #endif - -#else - //If not using the default address you can set custom addresses here - #define PRIMARY_IMU_ADDRESS_ONE 0x69 - #define PRIMARY_IMU_ADDRESS_TWO 0x68 - #define SECONDARY_IMU_ADDRESS_ONE 0x69 - #define SECONDARY_IMU_ADDRESS_TWO 0x68 -#endif \ No newline at end of file +#define PRIMARY_IMU_ADDRESS_ONE 0 +#define PRIMARY_IMU_ADDRESS_TWO 1 +#define SECONDARY_IMU_ADDRESS_ONE 0 +#define SECONDARY_IMU_ADDRESS_TWO 1 diff --git a/src/sensors/softfusion/drivers/bmi270.h b/src/sensors/softfusion/drivers/bmi270.h new file mode 100644 index 000000000..b5f49e52e --- /dev/null +++ b/src/sensors/softfusion/drivers/bmi270.h @@ -0,0 +1,419 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 Tailsy13 & 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 +#include +#include +#include +#include "bmi270fw.h" + +namespace SlimeVR::Sensors::SoftFusion::Drivers +{ + +// Driver uses acceleration range at 16g +// and gyroscope range at 1000dps +// Gyroscope ODR = 400Hz, accel ODR = 100Hz +// Timestamps reading are not used + +template +struct BMI270 +{ + static constexpr uint8_t Address = 0x68; + static constexpr auto Name = "BMI270"; + static constexpr auto Type = ImuID::BMI270; + + static constexpr float GyrTs=1.0/400.0; + static constexpr float AccTs=1.0/100.0; + + static constexpr float MagTs=1.0/100; + + static constexpr float GyroSensitivity = 32.768f; + static constexpr float AccelSensitivity = 2048.0f; + + struct MotionlessCalibrationData + { + bool valid; + uint8_t x, y, z; + }; + + I2CImpl i2c; + SlimeVR::Logging::Logger &logger; + int8_t zxFactor; + BMI270(I2CImpl i2c, SlimeVR::Logging::Logger &logger) + : i2c(i2c), logger(logger), zxFactor(0) {} + + struct Regs { + struct WhoAmI { + static constexpr uint8_t reg = 0x00; + static constexpr uint8_t value = 0x24; + }; + static constexpr uint8_t TempData = 0x22; + + struct Cmd { + static constexpr uint8_t reg = 0x7e; + static constexpr uint8_t valueSwReset = 0xb6; + static constexpr uint8_t valueFifoFlush = 0xb0; + static constexpr uint8_t valueGTrigger = 0x02; + }; + + struct PwrConf { + static constexpr uint8_t reg = 0x7c; + static constexpr uint8_t valueNoPowerSaving = 0x0; + static constexpr uint8_t valueFifoSelfWakeup = 0x2; + }; + + struct PwrCtrl { + static constexpr uint8_t reg = 0x7d; + static constexpr uint8_t valueOff = 0x0; + static constexpr uint8_t valueGyrAccTempOn = 0b1110; // aux off + static constexpr uint8_t valueAccOn = 0b0100; // aux, gyr, temp off + }; + + struct InitCtrl { + static constexpr uint8_t reg = 0x59; + static constexpr uint8_t valueStartInit = 0x00; + static constexpr uint8_t valueEndInit = 0x01; + }; + + static constexpr uint8_t InitAddr = 0x5b; + static constexpr uint8_t InitData = 0x5e; + + struct InternalStatus { + static constexpr uint8_t reg = 0x21; + static constexpr uint8_t initializedBit = 0x01; + }; + + struct GyrConf { + static constexpr uint8_t reg = 0x42; + + static constexpr uint8_t rate25Hz = 6; + static constexpr uint8_t rate50Hz = 7; + static constexpr uint8_t rate100Hz = 8; + static constexpr uint8_t rate200Hz = 9; + static constexpr uint8_t rate400Hz = 10; + static constexpr uint8_t rate800Hz = 11; + static constexpr uint8_t rate1600Hz = 12; + static constexpr uint8_t rate3200Hz = 13; + + static constexpr uint8_t DLPFModeOsr4 = 0 << 4; + static constexpr uint8_t DLPFModeOsr2 = 1 << 4; + static constexpr uint8_t DLPFModeNorm = 2 << 4; + + static constexpr uint8_t noisePerfMode = 1 << 6; + static constexpr uint8_t filterHighPerfMode = 1 << 7; + + static constexpr uint8_t value = rate400Hz | DLPFModeNorm | noisePerfMode | filterHighPerfMode; + }; + + struct GyrRange { + static constexpr uint8_t reg = 0x43; + + static constexpr uint8_t range125dps = 4; + static constexpr uint8_t range250dps = 3; + static constexpr uint8_t range500dps = 2; + static constexpr uint8_t range1000dps = 1; + static constexpr uint8_t range2000dps = 0; + + static constexpr uint8_t value = range1000dps; + }; + + struct AccConf { + static constexpr uint8_t reg = 0x40; + + static constexpr uint8_t rate0_78Hz = 1; + static constexpr uint8_t rate1_5Hz = 2; + static constexpr uint8_t rate3_1Hz = 3; + static constexpr uint8_t rate6_25Hz = 4; + static constexpr uint8_t rate12_5Hz = 5; + static constexpr uint8_t rate25Hz = 6; + static constexpr uint8_t rate50Hz = 7; + static constexpr uint8_t rate100Hz = 8; + static constexpr uint8_t rate200Hz = 9; + static constexpr uint8_t rate400Hz = 10; + static constexpr uint8_t rate800Hz = 11; + static constexpr uint8_t rate1600Hz = 12; + + static constexpr uint8_t DLPFModeAvg1 = 0 << 4; + static constexpr uint8_t DLPFModeAvg2 = 1 << 4; + static constexpr uint8_t DLPFModeAvg4 = 2 << 4; + static constexpr uint8_t DLPFModeAvg8 = 3 << 4; + + static constexpr uint8_t filterHighPerfMode = 1 << 7; + + static constexpr uint8_t value = rate100Hz | DLPFModeAvg4 | filterHighPerfMode; + }; + + struct AccRange { + static constexpr uint8_t reg = 0x41; + + static constexpr uint8_t range2G = 0; + static constexpr uint8_t range4G = 1; + static constexpr uint8_t range8G = 2; + static constexpr uint8_t range16G = 3; + + static constexpr uint8_t value = range16G; + }; + + struct FifoConfig0 { + static constexpr uint8_t reg = 0x48; + static constexpr uint8_t value = 0x01; // fifo_stop_on_full=1, fifo_time_en=0 + }; + + struct FifoConfig1 { + static constexpr uint8_t reg = 0x49; + static constexpr uint8_t value = (1 << 4) | (1 << 6) | (1 << 7); // header en, acc en, gyr en + }; + + struct GyrCrtConf { + static constexpr uint8_t reg = 0x69; + static constexpr uint8_t valueRunning = (1 << 2); // crt_running = 1 + static constexpr uint8_t valueStopped = 0x0; // crt_running = 0 + }; + + struct GTrig1 { // on feature page 1! + static constexpr uint8_t reg = 0x32; + static constexpr uint16_t valueTriggerCRT = (1 << 8); // select=crt + }; + + struct GyrGainStatus { // on feature page 0! + static constexpr uint8_t reg = 0x38; + static constexpr uint8_t statusOffset = 3; + }; + + struct Offset6 { // on feature page 0! + static constexpr uint8_t reg = 0x77; + static constexpr uint8_t value = (1 << 7); // gyr_gain_en = 1 + }; + + static constexpr uint8_t FeatPage = 0x2f; + + static constexpr uint8_t GyrUserGain = 0x78; // undocumented reg, got from official bmi270 driver + + static constexpr uint8_t FifoCount = 0x24; + static constexpr uint8_t FifoData = 0x26; + static constexpr uint8_t RaGyrCas = 0x3c; // on feature page 0! + }; + + struct Fifo { + static constexpr uint8_t ModeMask = 0b11000000; + static constexpr uint8_t SkipFrame = 0b01000000; + static constexpr uint8_t DataFrame = 0b10000000; + + static constexpr uint8_t GyrDataBit = 0b00001000; + static constexpr uint8_t AccelDataBit = 0b00000100; + }; + + bool restartAndInit() { + // perform initialization step + i2c.writeReg(Regs::Cmd::reg, Regs::Cmd::valueSwReset); + delay(12); + // disable power saving + i2c.writeReg(Regs::PwrConf::reg, Regs::PwrConf::valueNoPowerSaving); + delay(1); + + // firmware upload + i2c.writeReg(Regs::InitCtrl::reg, Regs::InitCtrl::valueStartInit); + for (uint16_t pos=0; pos> 1; // convert current position to words + const uint16_t position = (pos_words & 0x0F) | ((pos_words << 4) & 0xff00); + i2c.writeReg16(Regs::InitAddr, position); + // write actual payload chunk + const uint16_t burstWrite = std::min(sizeof(bmi270_firmware) - pos, I2CImpl::MaxTransactionLength); + i2c.writeBytes(Regs::InitData, burstWrite, const_cast(bmi270_firmware + pos)); + pos += burstWrite; + } + i2c.writeReg(Regs::InitCtrl::reg, Regs::InitCtrl::valueEndInit); + delay(140); + + // leave fifo_self_wakeup enabled + i2c.writeReg(Regs::PwrConf::reg, Regs::PwrConf::valueFifoSelfWakeup); + // check if IMU initialized correctly + if (!(i2c.readReg(Regs::InternalStatus::reg) & Regs::InternalStatus::initializedBit)) + { + // firmware upload fail or sensor not initialized + return false; + } + + // read zx factor used to reduce gyro cross-sensitivity error + const uint8_t zx_factor_reg = i2c.readReg(Regs::RaGyrCas); + const uint8_t sign_byte = (zx_factor_reg << 1) & 0x80; + zxFactor = static_cast(zx_factor_reg | sign_byte); + return true; + } + + void setNormalConfig(MotionlessCalibrationData &gyroSensitivity) + { + i2c.writeReg(Regs::GyrConf::reg, Regs::GyrConf::value); + i2c.writeReg(Regs::GyrRange::reg, Regs::GyrRange::value); + + i2c.writeReg(Regs::AccConf::reg, Regs::AccConf::value); + i2c.writeReg(Regs::AccRange::reg, Regs::AccRange::value); + + if (gyroSensitivity.valid) + { + i2c.writeReg(Regs::Offset6::reg, Regs::Offset6::value); + i2c.writeBytes(Regs::GyrUserGain, 3, &gyroSensitivity.x); + } + + i2c.writeReg(Regs::PwrCtrl::reg, Regs::PwrCtrl::valueGyrAccTempOn); + delay(100); // power up delay + i2c.writeReg(Regs::FifoConfig0::reg, Regs::FifoConfig0::value); + i2c.writeReg(Regs::FifoConfig1::reg, Regs::FifoConfig1::value); + + delay(4); + i2c.writeReg(Regs::Cmd::reg, Regs::Cmd::valueFifoFlush); + delay(2); + } + + bool initialize(MotionlessCalibrationData &gyroSensitivity) + { + if (!restartAndInit()) { + return false; + } + + setNormalConfig(gyroSensitivity); + + return true; + } + + void motionlessCalibration(MotionlessCalibrationData &gyroSensitivity) + { + // perfrom gyroscope motionless sensitivity calibration (CRT) + // need to start from clean state according to spec + restartAndInit(); + // only Accel ON + i2c.writeReg(Regs::PwrCtrl::reg, Regs::PwrCtrl::valueAccOn); + delay(100); + i2c.writeReg(Regs::GyrCrtConf::reg, Regs::GyrCrtConf::valueRunning); + i2c.writeReg(Regs::FeatPage, 1); + i2c.writeReg16(Regs::GTrig1::reg, Regs::GTrig1::valueTriggerCRT); + i2c.writeReg(Regs::Cmd::reg, Regs::Cmd::valueGTrigger); + delay(200); + + while(i2c.readReg(Regs::GyrCrtConf::reg) == Regs::GyrCrtConf::valueRunning) { + logger.info("CRT running. Do not move tracker!"); + delay(200); + } + + i2c.writeReg(Regs::FeatPage, 0); + + uint8_t status = i2c.readReg(Regs::GyrGainStatus::reg) >> Regs::GyrGainStatus::statusOffset; + // turn gyroscope back on + i2c.writeReg(Regs::PwrCtrl::reg, Regs::PwrCtrl::valueGyrAccTempOn); + delay(100); + + if (status != 0) { + logger.error("CRT failed with status 0x%x. Recalibrate again to enable CRT.", status); + if (status == 0x03) { + logger.error("Reason: tracker was moved during CRT!"); + } + } + else { + std::array crt_values; + i2c.readBytes(Regs::GyrUserGain, crt_values.size(), crt_values.data()); + logger.debug("CRT finished successfully, result 0x%x, 0x%x, 0x%x", crt_values[0], crt_values[1], crt_values[2]); + gyroSensitivity.valid = true; + gyroSensitivity.x = crt_values[0]; + gyroSensitivity.y = crt_values[1]; + gyroSensitivity.z = crt_values[2]; + } + + setNormalConfig(gyroSensitivity); + } + + float getDirectTemp() const + { + // middle value is 23 degrees C (0x0000) + // temperature per step from -41 + 1/2^9 degrees C (0x8001) to 87 - 1/2^9 degrees C (0x7FFF) + constexpr float TempStep = 128. / 65535; + const auto value = static_cast(i2c.readReg16(Regs::TempData)); + return static_cast(value) * TempStep + 23.0f; + } + + using FifoBuffer = std::array; + FifoBuffer read_buffer; + + template + inline T getFromFifo(uint32_t &position, FifoBuffer& fifo) { + T to_ret; + std::memcpy(&to_ret, &fifo[position], sizeof(T)); + position += sizeof(T); + return to_ret; + } + + template + void bulkRead(AccelCall &&processAccelSample, GyroCall &&processGyroSample) { + const auto fifo_bytes = i2c.readReg16(Regs::FifoCount); + + const auto bytes_to_read = std::min(static_cast(read_buffer.size()), + static_cast(fifo_bytes)); + i2c.readBytes(Regs::FifoData, bytes_to_read, read_buffer.data()); + + for (uint32_t i=0u; i(i, read_buffer); + if ((header & Fifo::ModeMask) == Fifo::SkipFrame && (i - bytes_to_read) >= 1) { + getFromFifo(i, read_buffer); // skip 1 byte + } + else if ((header & Fifo::ModeMask) == Fifo::DataFrame) { + const uint8_t required_length = + (((header & Fifo::GyrDataBit) >> Fifo::GyrDataBit) + + ((header & Fifo::AccelDataBit) >> Fifo::AccelDataBit)) * 6; + if (i - bytes_to_read < required_length) { + // incomplete frame, will be re-read next time + break; + } + if (header & Fifo::GyrDataBit) { + int16_t gyro[3]; + gyro[0] = getFromFifo(i, read_buffer); + gyro[1] = getFromFifo(i, read_buffer); + gyro[2] = getFromFifo(i, read_buffer); + using ShortLimit = std::numeric_limits; + // apply zx factor, todo: this awful line should be simplified and validated + gyro[0] = std::clamp(static_cast(gyro[0]) - static_cast((static_cast(zxFactor) * gyro[2]) / 512), + static_cast(ShortLimit::min()), static_cast(ShortLimit::max())); + processGyroSample(gyro, GyrTs); + } + + if (header & Fifo::AccelDataBit) { + int16_t accel[3]; + accel[0] = getFromFifo(i, read_buffer); + accel[1] = getFromFifo(i, read_buffer); + accel[2] = getFromFifo(i, read_buffer); + processAccelSample(accel, AccTs); + } + } + } + } + +}; + +} // namespace \ No newline at end of file diff --git a/src/sensors/softfusion/drivers/bmi270fw.h b/src/sensors/softfusion/drivers/bmi270fw.h new file mode 100644 index 000000000..87dda78e8 --- /dev/null +++ b/src/sensors/softfusion/drivers/bmi270fw.h @@ -0,0 +1,478 @@ +/** +* Firmware extracted from BMI270 library. +* https://github.com/boschsensortec/BMI270-Sensor-API/blob/master/bmi270.c +* +* Copyright (c) 2021 Bosch Sensortec GmbH. All rights reserved. +* +* BSD-3-Clause +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* +* 3. Neither the name of the copyright holder nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, +* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING +* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +*/ + +#include + +namespace SlimeVR::Sensors::SoftFusion::Drivers +{ + +const uint8_t bmi270_firmware[] = { + 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x3d, 0xb1, 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x91, 0x03, 0x80, 0x2e, 0xbc, + 0xb0, 0x80, 0x2e, 0xa3, 0x03, 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x00, 0xb0, 0x50, 0x30, 0x21, 0x2e, 0x59, 0xf5, + 0x10, 0x30, 0x21, 0x2e, 0x6a, 0xf5, 0x80, 0x2e, 0x3b, 0x03, 0x00, 0x00, 0x00, 0x00, 0x08, 0x19, 0x01, 0x00, 0x22, + 0x00, 0x75, 0x00, 0x00, 0x10, 0x00, 0x10, 0xd1, 0x00, 0xb3, 0x43, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0xe0, 0x5f, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x92, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x08, 0x19, 0x00, 0x00, 0x88, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, + 0xe0, 0xaa, 0x38, 0x05, 0xe0, 0x90, 0x30, 0xfa, 0x00, 0x96, 0x00, 0x4b, 0x09, 0x11, 0x00, 0x11, 0x00, 0x02, 0x00, + 0x2d, 0x01, 0xd4, 0x7b, 0x3b, 0x01, 0xdb, 0x7a, 0x04, 0x00, 0x3f, 0x7b, 0xcd, 0x6c, 0xc3, 0x04, 0x85, 0x09, 0xc3, + 0x04, 0xec, 0xe6, 0x0c, 0x46, 0x01, 0x00, 0x27, 0x00, 0x19, 0x00, 0x96, 0x00, 0xa0, 0x00, 0x01, 0x00, 0x0c, 0x00, + 0xf0, 0x3c, 0x00, 0x01, 0x01, 0x00, 0x03, 0x00, 0x01, 0x00, 0x0e, 0x00, 0x00, 0x00, 0x32, 0x00, 0x05, 0x00, 0xee, + 0x06, 0x04, 0x00, 0xc8, 0x00, 0x00, 0x00, 0x04, 0x00, 0xa8, 0x05, 0xee, 0x06, 0x00, 0x04, 0xbc, 0x02, 0xb3, 0x00, + 0x85, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0xb4, 0x00, 0x01, 0x00, 0xb9, 0x00, 0x01, 0x00, 0x98, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x01, 0x00, 0x80, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x2e, 0x00, 0xc1, 0xfd, 0x2d, 0xde, + 0x00, 0xeb, 0x00, 0xda, 0x00, 0x00, 0x0c, 0xff, 0x0f, 0x00, 0x04, 0xc0, 0x00, 0x5b, 0xf5, 0xc9, 0x01, 0x1e, 0xf2, + 0x80, 0x00, 0x3f, 0xff, 0x19, 0xf4, 0x58, 0xf5, 0x66, 0xf5, 0x64, 0xf5, 0xc0, 0xf1, 0xf0, 0x00, 0xe0, 0x00, 0xcd, + 0x01, 0xd3, 0x01, 0xdb, 0x01, 0xff, 0x7f, 0xff, 0x01, 0xe4, 0x00, 0x74, 0xf7, 0xf3, 0x00, 0xfa, 0x00, 0xff, 0x3f, + 0xca, 0x03, 0x6c, 0x38, 0x56, 0xfe, 0x44, 0xfd, 0xbc, 0x02, 0xf9, 0x06, 0x00, 0xfc, 0x12, 0x02, 0xae, 0x01, 0x58, + 0xfa, 0x9a, 0xfd, 0x77, 0x05, 0xbb, 0x02, 0x96, 0x01, 0x95, 0x01, 0x7f, 0x01, 0x82, 0x01, 0x89, 0x01, 0x87, 0x01, + 0x88, 0x01, 0x8a, 0x01, 0x8c, 0x01, 0x8f, 0x01, 0x8d, 0x01, 0x92, 0x01, 0x91, 0x01, 0xdd, 0x00, 0x9f, 0x01, 0x7e, + 0x01, 0xdb, 0x00, 0xb6, 0x01, 0x70, 0x69, 0x26, 0xd3, 0x9c, 0x07, 0x1f, 0x05, 0x9d, 0x00, 0x00, 0x08, 0xbc, 0x05, + 0x37, 0xfa, 0xa2, 0x01, 0xaa, 0x01, 0xa1, 0x01, 0xa8, 0x01, 0xa0, 0x01, 0xa8, 0x05, 0xb4, 0x01, 0xb4, 0x01, 0xce, + 0x00, 0xd0, 0x00, 0xfc, 0x00, 0xc5, 0x01, 0xff, 0xfb, 0xb1, 0x00, 0x00, 0x38, 0x00, 0x30, 0xfd, 0xf5, 0xfc, 0xf5, + 0xcd, 0x01, 0xa0, 0x00, 0x5f, 0xff, 0x00, 0x40, 0xff, 0x00, 0x00, 0x80, 0x6d, 0x0f, 0xeb, 0x00, 0x7f, 0xff, 0xc2, + 0xf5, 0x68, 0xf7, 0xb3, 0xf1, 0x67, 0x0f, 0x5b, 0x0f, 0x61, 0x0f, 0x80, 0x0f, 0x58, 0xf7, 0x5b, 0xf7, 0x83, 0x0f, + 0x86, 0x00, 0x72, 0x0f, 0x85, 0x0f, 0xc6, 0xf1, 0x7f, 0x0f, 0x6c, 0xf7, 0x00, 0xe0, 0x00, 0xff, 0xd1, 0xf5, 0x87, + 0x0f, 0x8a, 0x0f, 0xff, 0x03, 0xf0, 0x3f, 0x8b, 0x00, 0x8e, 0x00, 0x90, 0x00, 0xb9, 0x00, 0x2d, 0xf5, 0xca, 0xf5, + 0xcb, 0x01, 0x20, 0xf2, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x30, 0x50, 0x98, 0x2e, + 0xd7, 0x0e, 0x50, 0x32, 0x98, 0x2e, 0xfa, 0x03, 0x00, 0x30, 0xf0, 0x7f, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0x00, + 0x2e, 0x01, 0x80, 0x08, 0xa2, 0xfb, 0x2f, 0x98, 0x2e, 0xba, 0x03, 0x21, 0x2e, 0x19, 0x00, 0x01, 0x2e, 0xee, 0x00, + 0x00, 0xb2, 0x07, 0x2f, 0x01, 0x2e, 0x19, 0x00, 0x00, 0xb2, 0x03, 0x2f, 0x01, 0x50, 0x03, 0x52, 0x98, 0x2e, 0x07, + 0xcc, 0x01, 0x2e, 0xdd, 0x00, 0x00, 0xb2, 0x27, 0x2f, 0x05, 0x2e, 0x8a, 0x00, 0x05, 0x52, 0x98, 0x2e, 0xc7, 0xc1, + 0x03, 0x2e, 0xe9, 0x00, 0x40, 0xb2, 0xf0, 0x7f, 0x08, 0x2f, 0x01, 0x2e, 0x19, 0x00, 0x00, 0xb2, 0x04, 0x2f, 0x00, + 0x30, 0x21, 0x2e, 0xe9, 0x00, 0x98, 0x2e, 0xb4, 0xb1, 0x01, 0x2e, 0x18, 0x00, 0x00, 0xb2, 0x10, 0x2f, 0x05, 0x50, + 0x98, 0x2e, 0x4d, 0xc3, 0x05, 0x50, 0x98, 0x2e, 0x5a, 0xc7, 0x98, 0x2e, 0xf9, 0xb4, 0x98, 0x2e, 0x54, 0xb2, 0x98, + 0x2e, 0x67, 0xb6, 0x98, 0x2e, 0x17, 0xb2, 0x10, 0x30, 0x21, 0x2e, 0x77, 0x00, 0x01, 0x2e, 0xef, 0x00, 0x00, 0xb2, + 0x04, 0x2f, 0x98, 0x2e, 0x7a, 0xb7, 0x00, 0x30, 0x21, 0x2e, 0xef, 0x00, 0x01, 0x2e, 0xd4, 0x00, 0x04, 0xae, 0x0b, + 0x2f, 0x01, 0x2e, 0xdd, 0x00, 0x00, 0xb2, 0x07, 0x2f, 0x05, 0x52, 0x98, 0x2e, 0x8e, 0x0e, 0x00, 0xb2, 0x02, 0x2f, + 0x10, 0x30, 0x21, 0x2e, 0x7d, 0x00, 0x01, 0x2e, 0x7d, 0x00, 0x00, 0x90, 0x90, 0x2e, 0xf1, 0x02, 0x01, 0x2e, 0xd7, + 0x00, 0x00, 0xb2, 0x04, 0x2f, 0x98, 0x2e, 0x2f, 0x0e, 0x00, 0x30, 0x21, 0x2e, 0x7b, 0x00, 0x01, 0x2e, 0x7b, 0x00, + 0x00, 0xb2, 0x12, 0x2f, 0x01, 0x2e, 0xd4, 0x00, 0x00, 0x90, 0x02, 0x2f, 0x98, 0x2e, 0x1f, 0x0e, 0x09, 0x2d, 0x98, + 0x2e, 0x81, 0x0d, 0x01, 0x2e, 0xd4, 0x00, 0x04, 0x90, 0x02, 0x2f, 0x50, 0x32, 0x98, 0x2e, 0xfa, 0x03, 0x00, 0x30, + 0x21, 0x2e, 0x7b, 0x00, 0x01, 0x2e, 0x7c, 0x00, 0x00, 0xb2, 0x90, 0x2e, 0x09, 0x03, 0x01, 0x2e, 0x7c, 0x00, 0x01, + 0x31, 0x01, 0x08, 0x00, 0xb2, 0x04, 0x2f, 0x98, 0x2e, 0x47, 0xcb, 0x10, 0x30, 0x21, 0x2e, 0x77, 0x00, 0x81, 0x30, + 0x01, 0x2e, 0x7c, 0x00, 0x01, 0x08, 0x00, 0xb2, 0x61, 0x2f, 0x03, 0x2e, 0x89, 0x00, 0x01, 0x2e, 0xd4, 0x00, 0x98, + 0xbc, 0x98, 0xb8, 0x05, 0xb2, 0x0f, 0x58, 0x23, 0x2f, 0x07, 0x90, 0x09, 0x54, 0x00, 0x30, 0x37, 0x2f, 0x15, 0x41, + 0x04, 0x41, 0xdc, 0xbe, 0x44, 0xbe, 0xdc, 0xba, 0x2c, 0x01, 0x61, 0x00, 0x0f, 0x56, 0x4a, 0x0f, 0x0c, 0x2f, 0xd1, + 0x42, 0x94, 0xb8, 0xc1, 0x42, 0x11, 0x30, 0x05, 0x2e, 0x6a, 0xf7, 0x2c, 0xbd, 0x2f, 0xb9, 0x80, 0xb2, 0x08, 0x22, + 0x98, 0x2e, 0xc3, 0xb7, 0x21, 0x2d, 0x61, 0x30, 0x23, 0x2e, 0xd4, 0x00, 0x98, 0x2e, 0xc3, 0xb7, 0x00, 0x30, 0x21, + 0x2e, 0x5a, 0xf5, 0x18, 0x2d, 0xe1, 0x7f, 0x50, 0x30, 0x98, 0x2e, 0xfa, 0x03, 0x0f, 0x52, 0x07, 0x50, 0x50, 0x42, + 0x70, 0x30, 0x0d, 0x54, 0x42, 0x42, 0x7e, 0x82, 0xe2, 0x6f, 0x80, 0xb2, 0x42, 0x42, 0x05, 0x2f, 0x21, 0x2e, 0xd4, + 0x00, 0x10, 0x30, 0x98, 0x2e, 0xc3, 0xb7, 0x03, 0x2d, 0x60, 0x30, 0x21, 0x2e, 0xd4, 0x00, 0x01, 0x2e, 0xd4, 0x00, + 0x06, 0x90, 0x18, 0x2f, 0x01, 0x2e, 0x76, 0x00, 0x0b, 0x54, 0x07, 0x52, 0xe0, 0x7f, 0x98, 0x2e, 0x7a, 0xc1, 0xe1, + 0x6f, 0x08, 0x1a, 0x40, 0x30, 0x08, 0x2f, 0x21, 0x2e, 0xd4, 0x00, 0x20, 0x30, 0x98, 0x2e, 0xaf, 0xb7, 0x50, 0x32, + 0x98, 0x2e, 0xfa, 0x03, 0x05, 0x2d, 0x98, 0x2e, 0x38, 0x0e, 0x00, 0x30, 0x21, 0x2e, 0xd4, 0x00, 0x00, 0x30, 0x21, + 0x2e, 0x7c, 0x00, 0x18, 0x2d, 0x01, 0x2e, 0xd4, 0x00, 0x03, 0xaa, 0x01, 0x2f, 0x98, 0x2e, 0x45, 0x0e, 0x01, 0x2e, + 0xd4, 0x00, 0x3f, 0x80, 0x03, 0xa2, 0x01, 0x2f, 0x00, 0x2e, 0x02, 0x2d, 0x98, 0x2e, 0x5b, 0x0e, 0x30, 0x30, 0x98, + 0x2e, 0xce, 0xb7, 0x00, 0x30, 0x21, 0x2e, 0x7d, 0x00, 0x50, 0x32, 0x98, 0x2e, 0xfa, 0x03, 0x01, 0x2e, 0x77, 0x00, + 0x00, 0xb2, 0x24, 0x2f, 0x98, 0x2e, 0xf5, 0xcb, 0x03, 0x2e, 0xd5, 0x00, 0x11, 0x54, 0x01, 0x0a, 0xbc, 0x84, 0x83, + 0x86, 0x21, 0x2e, 0xc9, 0x01, 0xe0, 0x40, 0x13, 0x52, 0xc4, 0x40, 0x82, 0x40, 0xa8, 0xb9, 0x52, 0x42, 0x43, 0xbe, + 0x53, 0x42, 0x04, 0x0a, 0x50, 0x42, 0xe1, 0x7f, 0xf0, 0x31, 0x41, 0x40, 0xf2, 0x6f, 0x25, 0xbd, 0x08, 0x08, 0x02, + 0x0a, 0xd0, 0x7f, 0x98, 0x2e, 0xa8, 0xcf, 0x06, 0xbc, 0xd1, 0x6f, 0xe2, 0x6f, 0x08, 0x0a, 0x80, 0x42, 0x98, 0x2e, + 0x58, 0xb7, 0x00, 0x30, 0x21, 0x2e, 0xee, 0x00, 0x21, 0x2e, 0x77, 0x00, 0x21, 0x2e, 0xdd, 0x00, 0x80, 0x2e, 0xf4, + 0x01, 0x1a, 0x24, 0x22, 0x00, 0x80, 0x2e, 0xec, 0x01, 0x10, 0x50, 0xfb, 0x7f, 0x98, 0x2e, 0xf3, 0x03, 0x57, 0x50, + 0xfb, 0x6f, 0x01, 0x30, 0x71, 0x54, 0x11, 0x42, 0x42, 0x0e, 0xfc, 0x2f, 0xc0, 0x2e, 0x01, 0x42, 0xf0, 0x5f, 0x80, + 0x2e, 0x00, 0xc1, 0xfd, 0x2d, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x9a, 0x01, + 0x34, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x20, 0x50, 0xe7, 0x7f, 0xf6, 0x7f, 0x06, 0x32, 0x0f, 0x2e, 0x61, 0xf5, 0xfe, 0x09, 0xc0, 0xb3, 0x04, + 0x2f, 0x17, 0x30, 0x2f, 0x2e, 0xef, 0x00, 0x2d, 0x2e, 0x61, 0xf5, 0xf6, 0x6f, 0xe7, 0x6f, 0xe0, 0x5f, 0xc8, 0x2e, + 0x20, 0x50, 0xe7, 0x7f, 0xf6, 0x7f, 0x46, 0x30, 0x0f, 0x2e, 0xa4, 0xf1, 0xbe, 0x09, 0x80, 0xb3, 0x06, 0x2f, 0x0d, + 0x2e, 0xd4, 0x00, 0x84, 0xaf, 0x02, 0x2f, 0x16, 0x30, 0x2d, 0x2e, 0x7b, 0x00, 0x86, 0x30, 0x2d, 0x2e, 0x60, 0xf5, + 0xf6, 0x6f, 0xe7, 0x6f, 0xe0, 0x5f, 0xc8, 0x2e, 0x01, 0x2e, 0x77, 0xf7, 0x09, 0xbc, 0x0f, 0xb8, 0x00, 0xb2, 0x10, + 0x50, 0xfb, 0x7f, 0x10, 0x30, 0x0b, 0x2f, 0x03, 0x2e, 0x8a, 0x00, 0x96, 0xbc, 0x9f, 0xb8, 0x40, 0xb2, 0x05, 0x2f, + 0x03, 0x2e, 0x68, 0xf7, 0x9e, 0xbc, 0x9f, 0xb8, 0x40, 0xb2, 0x07, 0x2f, 0x03, 0x2e, 0x7e, 0x00, 0x41, 0x90, 0x01, + 0x2f, 0x98, 0x2e, 0xdc, 0x03, 0x03, 0x2c, 0x00, 0x30, 0x21, 0x2e, 0x7e, 0x00, 0xfb, 0x6f, 0xf0, 0x5f, 0xb8, 0x2e, + 0x20, 0x50, 0xe0, 0x7f, 0xfb, 0x7f, 0x00, 0x2e, 0x27, 0x50, 0x98, 0x2e, 0x3b, 0xc8, 0x29, 0x50, 0x98, 0x2e, 0xa7, + 0xc8, 0x01, 0x50, 0x98, 0x2e, 0x55, 0xcc, 0xe1, 0x6f, 0x2b, 0x50, 0x98, 0x2e, 0xe0, 0xc9, 0xfb, 0x6f, 0x00, 0x30, + 0xe0, 0x5f, 0x21, 0x2e, 0x7e, 0x00, 0xb8, 0x2e, 0x73, 0x50, 0x01, 0x30, 0x57, 0x54, 0x11, 0x42, 0x42, 0x0e, 0xfc, + 0x2f, 0xb8, 0x2e, 0x21, 0x2e, 0x59, 0xf5, 0x10, 0x30, 0xc0, 0x2e, 0x21, 0x2e, 0x4a, 0xf1, 0x90, 0x50, 0xf7, 0x7f, + 0xe6, 0x7f, 0xd5, 0x7f, 0xc4, 0x7f, 0xb3, 0x7f, 0xa1, 0x7f, 0x90, 0x7f, 0x82, 0x7f, 0x7b, 0x7f, 0x98, 0x2e, 0x35, + 0xb7, 0x00, 0xb2, 0x90, 0x2e, 0x97, 0xb0, 0x03, 0x2e, 0x8f, 0x00, 0x07, 0x2e, 0x91, 0x00, 0x05, 0x2e, 0xb1, 0x00, + 0x3f, 0xba, 0x9f, 0xb8, 0x01, 0x2e, 0xb1, 0x00, 0xa3, 0xbd, 0x4c, 0x0a, 0x05, 0x2e, 0xb1, 0x00, 0x04, 0xbe, 0xbf, + 0xb9, 0xcb, 0x0a, 0x4f, 0xba, 0x22, 0xbd, 0x01, 0x2e, 0xb3, 0x00, 0xdc, 0x0a, 0x2f, 0xb9, 0x03, 0x2e, 0xb8, 0x00, + 0x0a, 0xbe, 0x9a, 0x0a, 0xcf, 0xb9, 0x9b, 0xbc, 0x01, 0x2e, 0x97, 0x00, 0x9f, 0xb8, 0x93, 0x0a, 0x0f, 0xbc, 0x91, + 0x0a, 0x0f, 0xb8, 0x90, 0x0a, 0x25, 0x2e, 0x18, 0x00, 0x05, 0x2e, 0xc1, 0xf5, 0x2e, 0xbd, 0x2e, 0xb9, 0x01, 0x2e, + 0x19, 0x00, 0x31, 0x30, 0x8a, 0x04, 0x00, 0x90, 0x07, 0x2f, 0x01, 0x2e, 0xd4, 0x00, 0x04, 0xa2, 0x03, 0x2f, 0x01, + 0x2e, 0x18, 0x00, 0x00, 0xb2, 0x0c, 0x2f, 0x19, 0x50, 0x05, 0x52, 0x98, 0x2e, 0x4d, 0xb7, 0x05, 0x2e, 0x78, 0x00, + 0x80, 0x90, 0x10, 0x30, 0x01, 0x2f, 0x21, 0x2e, 0x78, 0x00, 0x25, 0x2e, 0xdd, 0x00, 0x98, 0x2e, 0x3e, 0xb7, 0x00, + 0xb2, 0x02, 0x30, 0x01, 0x30, 0x04, 0x2f, 0x01, 0x2e, 0x19, 0x00, 0x00, 0xb2, 0x00, 0x2f, 0x21, 0x30, 0x01, 0x2e, + 0xea, 0x00, 0x08, 0x1a, 0x0e, 0x2f, 0x23, 0x2e, 0xea, 0x00, 0x33, 0x30, 0x1b, 0x50, 0x0b, 0x09, 0x01, 0x40, 0x17, + 0x56, 0x46, 0xbe, 0x4b, 0x08, 0x4c, 0x0a, 0x01, 0x42, 0x0a, 0x80, 0x15, 0x52, 0x01, 0x42, 0x00, 0x2e, 0x01, 0x2e, + 0x18, 0x00, 0x00, 0xb2, 0x1f, 0x2f, 0x03, 0x2e, 0xc0, 0xf5, 0xf0, 0x30, 0x48, 0x08, 0x47, 0xaa, 0x74, 0x30, 0x07, + 0x2e, 0x7a, 0x00, 0x61, 0x22, 0x4b, 0x1a, 0x05, 0x2f, 0x07, 0x2e, 0x66, 0xf5, 0xbf, 0xbd, 0xbf, 0xb9, 0xc0, 0x90, + 0x0b, 0x2f, 0x1d, 0x56, 0x2b, 0x30, 0xd2, 0x42, 0xdb, 0x42, 0x01, 0x04, 0xc2, 0x42, 0x04, 0xbd, 0xfe, 0x80, 0x81, + 0x84, 0x23, 0x2e, 0x7a, 0x00, 0x02, 0x42, 0x02, 0x32, 0x25, 0x2e, 0x62, 0xf5, 0x05, 0x2e, 0xd6, 0x00, 0x81, 0x84, + 0x25, 0x2e, 0xd6, 0x00, 0x02, 0x31, 0x25, 0x2e, 0x60, 0xf5, 0x05, 0x2e, 0x8a, 0x00, 0x0b, 0x50, 0x90, 0x08, 0x80, + 0xb2, 0x0b, 0x2f, 0x05, 0x2e, 0xca, 0xf5, 0xf0, 0x3e, 0x90, 0x08, 0x25, 0x2e, 0xca, 0xf5, 0x05, 0x2e, 0x59, 0xf5, + 0xe0, 0x3f, 0x90, 0x08, 0x25, 0x2e, 0x59, 0xf5, 0x90, 0x6f, 0xa1, 0x6f, 0xb3, 0x6f, 0xc4, 0x6f, 0xd5, 0x6f, 0xe6, + 0x6f, 0xf7, 0x6f, 0x7b, 0x6f, 0x82, 0x6f, 0x70, 0x5f, 0xc8, 0x2e, 0xc0, 0x50, 0x90, 0x7f, 0xe5, 0x7f, 0xd4, 0x7f, + 0xc3, 0x7f, 0xb1, 0x7f, 0xa2, 0x7f, 0x87, 0x7f, 0xf6, 0x7f, 0x7b, 0x7f, 0x00, 0x2e, 0x01, 0x2e, 0x60, 0xf5, 0x60, + 0x7f, 0x98, 0x2e, 0x35, 0xb7, 0x02, 0x30, 0x63, 0x6f, 0x15, 0x52, 0x50, 0x7f, 0x62, 0x7f, 0x5a, 0x2c, 0x02, 0x32, + 0x1a, 0x09, 0x00, 0xb3, 0x14, 0x2f, 0x00, 0xb2, 0x03, 0x2f, 0x09, 0x2e, 0x18, 0x00, 0x00, 0x91, 0x0c, 0x2f, 0x43, + 0x7f, 0x98, 0x2e, 0x97, 0xb7, 0x1f, 0x50, 0x02, 0x8a, 0x02, 0x32, 0x04, 0x30, 0x25, 0x2e, 0x64, 0xf5, 0x15, 0x52, + 0x50, 0x6f, 0x43, 0x6f, 0x44, 0x43, 0x25, 0x2e, 0x60, 0xf5, 0xd9, 0x08, 0xc0, 0xb2, 0x36, 0x2f, 0x98, 0x2e, 0x3e, + 0xb7, 0x00, 0xb2, 0x06, 0x2f, 0x01, 0x2e, 0x19, 0x00, 0x00, 0xb2, 0x02, 0x2f, 0x50, 0x6f, 0x00, 0x90, 0x0a, 0x2f, + 0x01, 0x2e, 0x79, 0x00, 0x00, 0x90, 0x19, 0x2f, 0x10, 0x30, 0x21, 0x2e, 0x79, 0x00, 0x00, 0x30, 0x98, 0x2e, 0xdc, + 0x03, 0x13, 0x2d, 0x01, 0x2e, 0xc3, 0xf5, 0x0c, 0xbc, 0x0f, 0xb8, 0x12, 0x30, 0x10, 0x04, 0x03, 0xb0, 0x26, 0x25, + 0x21, 0x50, 0x03, 0x52, 0x98, 0x2e, 0x4d, 0xb7, 0x10, 0x30, 0x21, 0x2e, 0xee, 0x00, 0x02, 0x30, 0x60, 0x7f, 0x25, + 0x2e, 0x79, 0x00, 0x60, 0x6f, 0x00, 0x90, 0x05, 0x2f, 0x00, 0x30, 0x21, 0x2e, 0xea, 0x00, 0x15, 0x50, 0x21, 0x2e, + 0x64, 0xf5, 0x15, 0x52, 0x23, 0x2e, 0x60, 0xf5, 0x02, 0x32, 0x50, 0x6f, 0x00, 0x90, 0x02, 0x2f, 0x03, 0x30, 0x27, + 0x2e, 0x78, 0x00, 0x07, 0x2e, 0x60, 0xf5, 0x1a, 0x09, 0x00, 0x91, 0xa3, 0x2f, 0x19, 0x09, 0x00, 0x91, 0xa0, 0x2f, + 0x90, 0x6f, 0xa2, 0x6f, 0xb1, 0x6f, 0xc3, 0x6f, 0xd4, 0x6f, 0xe5, 0x6f, 0x7b, 0x6f, 0xf6, 0x6f, 0x87, 0x6f, 0x40, + 0x5f, 0xc8, 0x2e, 0xc0, 0x50, 0xe7, 0x7f, 0xf6, 0x7f, 0x26, 0x30, 0x0f, 0x2e, 0x61, 0xf5, 0x2f, 0x2e, 0x7c, 0x00, + 0x0f, 0x2e, 0x7c, 0x00, 0xbe, 0x09, 0xa2, 0x7f, 0x80, 0x7f, 0x80, 0xb3, 0xd5, 0x7f, 0xc4, 0x7f, 0xb3, 0x7f, 0x91, + 0x7f, 0x7b, 0x7f, 0x0b, 0x2f, 0x23, 0x50, 0x1a, 0x25, 0x12, 0x40, 0x42, 0x7f, 0x74, 0x82, 0x12, 0x40, 0x52, 0x7f, + 0x00, 0x2e, 0x00, 0x40, 0x60, 0x7f, 0x98, 0x2e, 0x6a, 0xd6, 0x81, 0x30, 0x01, 0x2e, 0x7c, 0x00, 0x01, 0x08, 0x00, + 0xb2, 0x42, 0x2f, 0x03, 0x2e, 0x89, 0x00, 0x01, 0x2e, 0x89, 0x00, 0x97, 0xbc, 0x06, 0xbc, 0x9f, 0xb8, 0x0f, 0xb8, + 0x00, 0x90, 0x23, 0x2e, 0xd8, 0x00, 0x10, 0x30, 0x01, 0x30, 0x2a, 0x2f, 0x03, 0x2e, 0xd4, 0x00, 0x44, 0xb2, 0x05, + 0x2f, 0x47, 0xb2, 0x00, 0x30, 0x2d, 0x2f, 0x21, 0x2e, 0x7c, 0x00, 0x2b, 0x2d, 0x03, 0x2e, 0xfd, 0xf5, 0x9e, 0xbc, + 0x9f, 0xb8, 0x40, 0x90, 0x14, 0x2f, 0x03, 0x2e, 0xfc, 0xf5, 0x99, 0xbc, 0x9f, 0xb8, 0x40, 0x90, 0x0e, 0x2f, 0x03, + 0x2e, 0x49, 0xf1, 0x25, 0x54, 0x4a, 0x08, 0x40, 0x90, 0x08, 0x2f, 0x98, 0x2e, 0x35, 0xb7, 0x00, 0xb2, 0x10, 0x30, + 0x03, 0x2f, 0x50, 0x30, 0x21, 0x2e, 0xd4, 0x00, 0x10, 0x2d, 0x98, 0x2e, 0xaf, 0xb7, 0x00, 0x30, 0x21, 0x2e, 0x7c, + 0x00, 0x0a, 0x2d, 0x05, 0x2e, 0x69, 0xf7, 0x2d, 0xbd, 0x2f, 0xb9, 0x80, 0xb2, 0x01, 0x2f, 0x21, 0x2e, 0x7d, 0x00, + 0x23, 0x2e, 0x7c, 0x00, 0xe0, 0x31, 0x21, 0x2e, 0x61, 0xf5, 0xf6, 0x6f, 0xe7, 0x6f, 0x80, 0x6f, 0xa2, 0x6f, 0xb3, + 0x6f, 0xc4, 0x6f, 0xd5, 0x6f, 0x7b, 0x6f, 0x91, 0x6f, 0x40, 0x5f, 0xc8, 0x2e, 0x60, 0x51, 0x0a, 0x25, 0x36, 0x88, + 0xf4, 0x7f, 0xeb, 0x7f, 0x00, 0x32, 0x31, 0x52, 0x32, 0x30, 0x13, 0x30, 0x98, 0x2e, 0x15, 0xcb, 0x0a, 0x25, 0x33, + 0x84, 0xd2, 0x7f, 0x43, 0x30, 0x05, 0x50, 0x2d, 0x52, 0x98, 0x2e, 0x95, 0xc1, 0xd2, 0x6f, 0x27, 0x52, 0x98, 0x2e, + 0xd7, 0xc7, 0x2a, 0x25, 0xb0, 0x86, 0xc0, 0x7f, 0xd3, 0x7f, 0xaf, 0x84, 0x29, 0x50, 0xf1, 0x6f, 0x98, 0x2e, 0x4d, + 0xc8, 0x2a, 0x25, 0xae, 0x8a, 0xaa, 0x88, 0xf2, 0x6e, 0x2b, 0x50, 0xc1, 0x6f, 0xd3, 0x6f, 0xf4, 0x7f, 0x98, 0x2e, + 0xb6, 0xc8, 0xe0, 0x6e, 0x00, 0xb2, 0x32, 0x2f, 0x33, 0x54, 0x83, 0x86, 0xf1, 0x6f, 0xc3, 0x7f, 0x04, 0x30, 0x30, + 0x30, 0xf4, 0x7f, 0xd0, 0x7f, 0xb2, 0x7f, 0xe3, 0x30, 0xc5, 0x6f, 0x56, 0x40, 0x45, 0x41, 0x28, 0x08, 0x03, 0x14, + 0x0e, 0xb4, 0x08, 0xbc, 0x82, 0x40, 0x10, 0x0a, 0x2f, 0x54, 0x26, 0x05, 0x91, 0x7f, 0x44, 0x28, 0xa3, 0x7f, 0x98, + 0x2e, 0xd9, 0xc0, 0x08, 0xb9, 0x33, 0x30, 0x53, 0x09, 0xc1, 0x6f, 0xd3, 0x6f, 0xf4, 0x6f, 0x83, 0x17, 0x47, 0x40, + 0x6c, 0x15, 0xb2, 0x6f, 0xbe, 0x09, 0x75, 0x0b, 0x90, 0x42, 0x45, 0x42, 0x51, 0x0e, 0x32, 0xbc, 0x02, 0x89, 0xa1, + 0x6f, 0x7e, 0x86, 0xf4, 0x7f, 0xd0, 0x7f, 0xb2, 0x7f, 0x04, 0x30, 0x91, 0x6f, 0xd6, 0x2f, 0xeb, 0x6f, 0xa0, 0x5e, + 0xb8, 0x2e, 0x03, 0x2e, 0x97, 0x00, 0x1b, 0xbc, 0x60, 0x50, 0x9f, 0xbc, 0x0c, 0xb8, 0xf0, 0x7f, 0x40, 0xb2, 0xeb, + 0x7f, 0x2b, 0x2f, 0x03, 0x2e, 0x7f, 0x00, 0x41, 0x40, 0x01, 0x2e, 0xc8, 0x00, 0x01, 0x1a, 0x11, 0x2f, 0x37, 0x58, + 0x23, 0x2e, 0xc8, 0x00, 0x10, 0x41, 0xa0, 0x7f, 0x38, 0x81, 0x01, 0x41, 0xd0, 0x7f, 0xb1, 0x7f, 0x98, 0x2e, 0x64, + 0xcf, 0xd0, 0x6f, 0x07, 0x80, 0xa1, 0x6f, 0x11, 0x42, 0x00, 0x2e, 0xb1, 0x6f, 0x01, 0x42, 0x11, 0x30, 0x01, 0x2e, + 0xfc, 0x00, 0x00, 0xa8, 0x03, 0x30, 0xcb, 0x22, 0x4a, 0x25, 0x01, 0x2e, 0x7f, 0x00, 0x3c, 0x89, 0x35, 0x52, 0x05, + 0x54, 0x98, 0x2e, 0xc4, 0xce, 0xc1, 0x6f, 0xf0, 0x6f, 0x98, 0x2e, 0x95, 0xcf, 0x04, 0x2d, 0x01, 0x30, 0xf0, 0x6f, + 0x98, 0x2e, 0x95, 0xcf, 0xeb, 0x6f, 0xa0, 0x5f, 0xb8, 0x2e, 0x03, 0x2e, 0xb3, 0x00, 0x02, 0x32, 0xf0, 0x30, 0x03, + 0x31, 0x30, 0x50, 0x8a, 0x08, 0x08, 0x08, 0xcb, 0x08, 0xe0, 0x7f, 0x80, 0xb2, 0xf3, 0x7f, 0xdb, 0x7f, 0x25, 0x2f, + 0x03, 0x2e, 0xca, 0x00, 0x41, 0x90, 0x04, 0x2f, 0x01, 0x30, 0x23, 0x2e, 0xca, 0x00, 0x98, 0x2e, 0x3f, 0x03, 0xc0, + 0xb2, 0x05, 0x2f, 0x03, 0x2e, 0xda, 0x00, 0x00, 0x30, 0x41, 0x04, 0x23, 0x2e, 0xda, 0x00, 0x98, 0x2e, 0x92, 0xb2, + 0x10, 0x25, 0xf0, 0x6f, 0x00, 0xb2, 0x05, 0x2f, 0x01, 0x2e, 0xda, 0x00, 0x02, 0x30, 0x10, 0x04, 0x21, 0x2e, 0xda, + 0x00, 0x40, 0xb2, 0x01, 0x2f, 0x23, 0x2e, 0xc8, 0x01, 0xdb, 0x6f, 0xe0, 0x6f, 0xd0, 0x5f, 0x80, 0x2e, 0x95, 0xcf, + 0x01, 0x30, 0xe0, 0x6f, 0x98, 0x2e, 0x95, 0xcf, 0x11, 0x30, 0x23, 0x2e, 0xca, 0x00, 0xdb, 0x6f, 0xd0, 0x5f, 0xb8, + 0x2e, 0xd0, 0x50, 0x0a, 0x25, 0x33, 0x84, 0x55, 0x50, 0xd2, 0x7f, 0xe2, 0x7f, 0x03, 0x8c, 0xc0, 0x7f, 0xbb, 0x7f, + 0x00, 0x30, 0x05, 0x5a, 0x39, 0x54, 0x51, 0x41, 0xa5, 0x7f, 0x96, 0x7f, 0x80, 0x7f, 0x98, 0x2e, 0xd9, 0xc0, 0x05, + 0x30, 0xf5, 0x7f, 0x20, 0x25, 0x91, 0x6f, 0x3b, 0x58, 0x3d, 0x5c, 0x3b, 0x56, 0x98, 0x2e, 0x67, 0xcc, 0xc1, 0x6f, + 0xd5, 0x6f, 0x52, 0x40, 0x50, 0x43, 0xc1, 0x7f, 0xd5, 0x7f, 0x10, 0x25, 0x98, 0x2e, 0xfe, 0xc9, 0x10, 0x25, 0x98, + 0x2e, 0x74, 0xc0, 0x86, 0x6f, 0x30, 0x28, 0x92, 0x6f, 0x82, 0x8c, 0xa5, 0x6f, 0x6f, 0x52, 0x69, 0x0e, 0x39, 0x54, + 0xdb, 0x2f, 0x19, 0xa0, 0x15, 0x30, 0x03, 0x2f, 0x00, 0x30, 0x21, 0x2e, 0x81, 0x01, 0x0a, 0x2d, 0x01, 0x2e, 0x81, + 0x01, 0x05, 0x28, 0x42, 0x36, 0x21, 0x2e, 0x81, 0x01, 0x02, 0x0e, 0x01, 0x2f, 0x98, 0x2e, 0xf3, 0x03, 0x57, 0x50, + 0x12, 0x30, 0x01, 0x40, 0x98, 0x2e, 0xfe, 0xc9, 0x51, 0x6f, 0x0b, 0x5c, 0x8e, 0x0e, 0x3b, 0x6f, 0x57, 0x58, 0x02, + 0x30, 0x21, 0x2e, 0x95, 0x01, 0x45, 0x6f, 0x2a, 0x8d, 0xd2, 0x7f, 0xcb, 0x7f, 0x13, 0x2f, 0x02, 0x30, 0x3f, 0x50, + 0xd2, 0x7f, 0xa8, 0x0e, 0x0e, 0x2f, 0xc0, 0x6f, 0x53, 0x54, 0x02, 0x00, 0x51, 0x54, 0x42, 0x0e, 0x10, 0x30, 0x59, + 0x52, 0x02, 0x30, 0x01, 0x2f, 0x00, 0x2e, 0x03, 0x2d, 0x50, 0x42, 0x42, 0x42, 0x12, 0x30, 0xd2, 0x7f, 0x80, 0xb2, + 0x03, 0x2f, 0x00, 0x30, 0x21, 0x2e, 0x80, 0x01, 0x12, 0x2d, 0x01, 0x2e, 0xc9, 0x00, 0x02, 0x80, 0x05, 0x2e, 0x80, + 0x01, 0x11, 0x30, 0x91, 0x28, 0x00, 0x40, 0x25, 0x2e, 0x80, 0x01, 0x10, 0x0e, 0x05, 0x2f, 0x01, 0x2e, 0x7f, 0x01, + 0x01, 0x90, 0x01, 0x2f, 0x98, 0x2e, 0xf3, 0x03, 0x00, 0x2e, 0xa0, 0x41, 0x01, 0x90, 0xa6, 0x7f, 0x90, 0x2e, 0xe3, + 0xb4, 0x01, 0x2e, 0x95, 0x01, 0x00, 0xa8, 0x90, 0x2e, 0xe3, 0xb4, 0x5b, 0x54, 0x95, 0x80, 0x82, 0x40, 0x80, 0xb2, + 0x02, 0x40, 0x2d, 0x8c, 0x3f, 0x52, 0x96, 0x7f, 0x90, 0x2e, 0xc2, 0xb3, 0x29, 0x0e, 0x76, 0x2f, 0x01, 0x2e, 0xc9, + 0x00, 0x00, 0x40, 0x81, 0x28, 0x45, 0x52, 0xb3, 0x30, 0x98, 0x2e, 0x0f, 0xca, 0x5d, 0x54, 0x80, 0x7f, 0x00, 0x2e, + 0xa1, 0x40, 0x72, 0x7f, 0x82, 0x80, 0x82, 0x40, 0x60, 0x7f, 0x98, 0x2e, 0xfe, 0xc9, 0x10, 0x25, 0x98, 0x2e, 0x74, + 0xc0, 0x62, 0x6f, 0x05, 0x30, 0x87, 0x40, 0xc0, 0x91, 0x04, 0x30, 0x05, 0x2f, 0x05, 0x2e, 0x83, 0x01, 0x80, 0xb2, + 0x14, 0x30, 0x00, 0x2f, 0x04, 0x30, 0x05, 0x2e, 0xc9, 0x00, 0x73, 0x6f, 0x81, 0x40, 0xe2, 0x40, 0x69, 0x04, 0x11, + 0x0f, 0xe1, 0x40, 0x16, 0x30, 0xfe, 0x29, 0xcb, 0x40, 0x02, 0x2f, 0x83, 0x6f, 0x83, 0x0f, 0x22, 0x2f, 0x47, 0x56, + 0x13, 0x0f, 0x12, 0x30, 0x77, 0x2f, 0x49, 0x54, 0x42, 0x0e, 0x12, 0x30, 0x73, 0x2f, 0x00, 0x91, 0x0a, 0x2f, 0x01, + 0x2e, 0x8b, 0x01, 0x19, 0xa8, 0x02, 0x30, 0x6c, 0x2f, 0x63, 0x50, 0x00, 0x2e, 0x17, 0x42, 0x05, 0x42, 0x68, 0x2c, + 0x12, 0x30, 0x0b, 0x25, 0x08, 0x0f, 0x50, 0x30, 0x02, 0x2f, 0x21, 0x2e, 0x83, 0x01, 0x03, 0x2d, 0x40, 0x30, 0x21, + 0x2e, 0x83, 0x01, 0x2b, 0x2e, 0x85, 0x01, 0x5a, 0x2c, 0x12, 0x30, 0x00, 0x91, 0x2b, 0x25, 0x04, 0x2f, 0x63, 0x50, + 0x02, 0x30, 0x17, 0x42, 0x17, 0x2c, 0x02, 0x42, 0x98, 0x2e, 0xfe, 0xc9, 0x10, 0x25, 0x98, 0x2e, 0x74, 0xc0, 0x05, + 0x2e, 0xc9, 0x00, 0x81, 0x84, 0x5b, 0x30, 0x82, 0x40, 0x37, 0x2e, 0x83, 0x01, 0x02, 0x0e, 0x07, 0x2f, 0x5f, 0x52, + 0x40, 0x30, 0x62, 0x40, 0x41, 0x40, 0x91, 0x0e, 0x01, 0x2f, 0x21, 0x2e, 0x83, 0x01, 0x05, 0x30, 0x2b, 0x2e, 0x85, + 0x01, 0x12, 0x30, 0x36, 0x2c, 0x16, 0x30, 0x15, 0x25, 0x81, 0x7f, 0x98, 0x2e, 0xfe, 0xc9, 0x10, 0x25, 0x98, 0x2e, + 0x74, 0xc0, 0x19, 0xa2, 0x16, 0x30, 0x15, 0x2f, 0x05, 0x2e, 0x97, 0x01, 0x80, 0x6f, 0x82, 0x0e, 0x05, 0x2f, 0x01, + 0x2e, 0x86, 0x01, 0x06, 0x28, 0x21, 0x2e, 0x86, 0x01, 0x0b, 0x2d, 0x03, 0x2e, 0x87, 0x01, 0x5f, 0x54, 0x4e, 0x28, + 0x91, 0x42, 0x00, 0x2e, 0x82, 0x40, 0x90, 0x0e, 0x01, 0x2f, 0x21, 0x2e, 0x88, 0x01, 0x02, 0x30, 0x13, 0x2c, 0x05, + 0x30, 0xc0, 0x6f, 0x08, 0x1c, 0xa8, 0x0f, 0x16, 0x30, 0x05, 0x30, 0x5b, 0x50, 0x09, 0x2f, 0x02, 0x80, 0x2d, 0x2e, + 0x82, 0x01, 0x05, 0x42, 0x05, 0x80, 0x00, 0x2e, 0x02, 0x42, 0x3e, 0x80, 0x00, 0x2e, 0x06, 0x42, 0x02, 0x30, 0x90, + 0x6f, 0x3e, 0x88, 0x01, 0x40, 0x04, 0x41, 0x4c, 0x28, 0x01, 0x42, 0x07, 0x80, 0x10, 0x25, 0x24, 0x40, 0x00, 0x40, + 0x00, 0xa8, 0xf5, 0x22, 0x23, 0x29, 0x44, 0x42, 0x7a, 0x82, 0x7e, 0x88, 0x43, 0x40, 0x04, 0x41, 0x00, 0xab, 0xf5, + 0x23, 0xdf, 0x28, 0x43, 0x42, 0xd9, 0xa0, 0x14, 0x2f, 0x00, 0x90, 0x02, 0x2f, 0xd2, 0x6f, 0x81, 0xb2, 0x05, 0x2f, + 0x63, 0x54, 0x06, 0x28, 0x90, 0x42, 0x85, 0x42, 0x09, 0x2c, 0x02, 0x30, 0x5b, 0x50, 0x03, 0x80, 0x29, 0x2e, 0x7e, + 0x01, 0x2b, 0x2e, 0x82, 0x01, 0x05, 0x42, 0x12, 0x30, 0x2b, 0x2e, 0x83, 0x01, 0x45, 0x82, 0x00, 0x2e, 0x40, 0x40, + 0x7a, 0x82, 0x02, 0xa0, 0x08, 0x2f, 0x63, 0x50, 0x3b, 0x30, 0x15, 0x42, 0x05, 0x42, 0x37, 0x80, 0x37, 0x2e, 0x7e, + 0x01, 0x05, 0x42, 0x12, 0x30, 0x01, 0x2e, 0xc9, 0x00, 0x02, 0x8c, 0x40, 0x40, 0x84, 0x41, 0x7a, 0x8c, 0x04, 0x0f, + 0x03, 0x2f, 0x01, 0x2e, 0x8b, 0x01, 0x19, 0xa4, 0x04, 0x2f, 0x2b, 0x2e, 0x82, 0x01, 0x98, 0x2e, 0xf3, 0x03, 0x12, + 0x30, 0x81, 0x90, 0x61, 0x52, 0x08, 0x2f, 0x65, 0x42, 0x65, 0x42, 0x43, 0x80, 0x39, 0x84, 0x82, 0x88, 0x05, 0x42, + 0x45, 0x42, 0x85, 0x42, 0x05, 0x43, 0x00, 0x2e, 0x80, 0x41, 0x00, 0x90, 0x90, 0x2e, 0xe1, 0xb4, 0x65, 0x54, 0xc1, + 0x6f, 0x80, 0x40, 0x00, 0xb2, 0x43, 0x58, 0x69, 0x50, 0x44, 0x2f, 0x55, 0x5c, 0xb7, 0x87, 0x8c, 0x0f, 0x0d, 0x2e, + 0x96, 0x01, 0xc4, 0x40, 0x36, 0x2f, 0x41, 0x56, 0x8b, 0x0e, 0x2a, 0x2f, 0x0b, 0x52, 0xa1, 0x0e, 0x0a, 0x2f, 0x05, + 0x2e, 0x8f, 0x01, 0x14, 0x25, 0x98, 0x2e, 0xfe, 0xc9, 0x4b, 0x54, 0x02, 0x0f, 0x69, 0x50, 0x05, 0x30, 0x65, 0x54, + 0x15, 0x2f, 0x03, 0x2e, 0x8e, 0x01, 0x4d, 0x5c, 0x8e, 0x0f, 0x3a, 0x2f, 0x05, 0x2e, 0x8f, 0x01, 0x98, 0x2e, 0xfe, + 0xc9, 0x4f, 0x54, 0x82, 0x0f, 0x05, 0x30, 0x69, 0x50, 0x65, 0x54, 0x30, 0x2f, 0x6d, 0x52, 0x15, 0x30, 0x42, 0x8c, + 0x45, 0x42, 0x04, 0x30, 0x2b, 0x2c, 0x84, 0x43, 0x6b, 0x52, 0x42, 0x8c, 0x00, 0x2e, 0x85, 0x43, 0x15, 0x30, 0x24, + 0x2c, 0x45, 0x42, 0x8e, 0x0f, 0x20, 0x2f, 0x0d, 0x2e, 0x8e, 0x01, 0xb1, 0x0e, 0x1c, 0x2f, 0x23, 0x2e, 0x8e, 0x01, + 0x1a, 0x2d, 0x0e, 0x0e, 0x17, 0x2f, 0xa1, 0x0f, 0x15, 0x2f, 0x23, 0x2e, 0x8d, 0x01, 0x13, 0x2d, 0x98, 0x2e, 0x74, + 0xc0, 0x43, 0x54, 0xc2, 0x0e, 0x0a, 0x2f, 0x65, 0x50, 0x04, 0x80, 0x0b, 0x30, 0x06, 0x82, 0x0b, 0x42, 0x79, 0x80, + 0x41, 0x40, 0x12, 0x30, 0x25, 0x2e, 0x8c, 0x01, 0x01, 0x42, 0x05, 0x30, 0x69, 0x50, 0x65, 0x54, 0x84, 0x82, 0x43, + 0x84, 0xbe, 0x8c, 0x84, 0x40, 0x86, 0x41, 0x26, 0x29, 0x94, 0x42, 0xbe, 0x8e, 0xd5, 0x7f, 0x19, 0xa1, 0x43, 0x40, + 0x0b, 0x2e, 0x8c, 0x01, 0x84, 0x40, 0xc7, 0x41, 0x5d, 0x29, 0x27, 0x29, 0x45, 0x42, 0x84, 0x42, 0xc2, 0x7f, 0x01, + 0x2f, 0xc0, 0xb3, 0x1d, 0x2f, 0x05, 0x2e, 0x94, 0x01, 0x99, 0xa0, 0x01, 0x2f, 0x80, 0xb3, 0x13, 0x2f, 0x80, 0xb3, + 0x18, 0x2f, 0xc0, 0xb3, 0x16, 0x2f, 0x12, 0x40, 0x01, 0x40, 0x92, 0x7f, 0x98, 0x2e, 0x74, 0xc0, 0x92, 0x6f, 0x10, + 0x0f, 0x20, 0x30, 0x03, 0x2f, 0x10, 0x30, 0x21, 0x2e, 0x7e, 0x01, 0x0a, 0x2d, 0x21, 0x2e, 0x7e, 0x01, 0x07, 0x2d, + 0x20, 0x30, 0x21, 0x2e, 0x7e, 0x01, 0x03, 0x2d, 0x10, 0x30, 0x21, 0x2e, 0x7e, 0x01, 0xc2, 0x6f, 0x01, 0x2e, 0xc9, + 0x00, 0xbc, 0x84, 0x02, 0x80, 0x82, 0x40, 0x00, 0x40, 0x90, 0x0e, 0xd5, 0x6f, 0x02, 0x2f, 0x15, 0x30, 0x98, 0x2e, + 0xf3, 0x03, 0x41, 0x91, 0x05, 0x30, 0x07, 0x2f, 0x67, 0x50, 0x3d, 0x80, 0x2b, 0x2e, 0x8f, 0x01, 0x05, 0x42, 0x04, + 0x80, 0x00, 0x2e, 0x05, 0x42, 0x02, 0x2c, 0x00, 0x30, 0x00, 0x30, 0xa2, 0x6f, 0x98, 0x8a, 0x86, 0x40, 0x80, 0xa7, + 0x05, 0x2f, 0x98, 0x2e, 0xf3, 0x03, 0xc0, 0x30, 0x21, 0x2e, 0x95, 0x01, 0x06, 0x25, 0x1a, 0x25, 0xe2, 0x6f, 0x76, + 0x82, 0x96, 0x40, 0x56, 0x43, 0x51, 0x0e, 0xfb, 0x2f, 0xbb, 0x6f, 0x30, 0x5f, 0xb8, 0x2e, 0x01, 0x2e, 0xb8, 0x00, + 0x01, 0x31, 0x41, 0x08, 0x40, 0xb2, 0x20, 0x50, 0xf2, 0x30, 0x02, 0x08, 0xfb, 0x7f, 0x01, 0x30, 0x10, 0x2f, 0x05, + 0x2e, 0xcc, 0x00, 0x81, 0x90, 0xe0, 0x7f, 0x03, 0x2f, 0x23, 0x2e, 0xcc, 0x00, 0x98, 0x2e, 0x55, 0xb6, 0x98, 0x2e, + 0x1d, 0xb5, 0x10, 0x25, 0xfb, 0x6f, 0xe0, 0x6f, 0xe0, 0x5f, 0x80, 0x2e, 0x95, 0xcf, 0x98, 0x2e, 0x95, 0xcf, 0x10, + 0x30, 0x21, 0x2e, 0xcc, 0x00, 0xfb, 0x6f, 0xe0, 0x5f, 0xb8, 0x2e, 0x00, 0x51, 0x05, 0x58, 0xeb, 0x7f, 0x2a, 0x25, + 0x89, 0x52, 0x6f, 0x5a, 0x89, 0x50, 0x13, 0x41, 0x06, 0x40, 0xb3, 0x01, 0x16, 0x42, 0xcb, 0x16, 0x06, 0x40, 0xf3, + 0x02, 0x13, 0x42, 0x65, 0x0e, 0xf5, 0x2f, 0x05, 0x40, 0x14, 0x30, 0x2c, 0x29, 0x04, 0x42, 0x08, 0xa1, 0x00, 0x30, + 0x90, 0x2e, 0x52, 0xb6, 0xb3, 0x88, 0xb0, 0x8a, 0xb6, 0x84, 0xa4, 0x7f, 0xc4, 0x7f, 0xb5, 0x7f, 0xd5, 0x7f, 0x92, + 0x7f, 0x73, 0x30, 0x04, 0x30, 0x55, 0x40, 0x42, 0x40, 0x8a, 0x17, 0xf3, 0x08, 0x6b, 0x01, 0x90, 0x02, 0x53, 0xb8, + 0x4b, 0x82, 0xad, 0xbe, 0x71, 0x7f, 0x45, 0x0a, 0x09, 0x54, 0x84, 0x7f, 0x98, 0x2e, 0xd9, 0xc0, 0xa3, 0x6f, 0x7b, + 0x54, 0xd0, 0x42, 0xa3, 0x7f, 0xf2, 0x7f, 0x60, 0x7f, 0x20, 0x25, 0x71, 0x6f, 0x75, 0x5a, 0x77, 0x58, 0x79, 0x5c, + 0x75, 0x56, 0x98, 0x2e, 0x67, 0xcc, 0xb1, 0x6f, 0x62, 0x6f, 0x50, 0x42, 0xb1, 0x7f, 0xb3, 0x30, 0x10, 0x25, 0x98, + 0x2e, 0x0f, 0xca, 0x84, 0x6f, 0x20, 0x29, 0x71, 0x6f, 0x92, 0x6f, 0xa5, 0x6f, 0x76, 0x82, 0x6a, 0x0e, 0x73, 0x30, + 0x00, 0x30, 0xd0, 0x2f, 0xd2, 0x6f, 0xd1, 0x7f, 0xb4, 0x7f, 0x98, 0x2e, 0x2b, 0xb7, 0x15, 0xbd, 0x0b, 0xb8, 0x02, + 0x0a, 0xc2, 0x6f, 0xc0, 0x7f, 0x98, 0x2e, 0x2b, 0xb7, 0x15, 0xbd, 0x0b, 0xb8, 0x42, 0x0a, 0xc0, 0x6f, 0x08, 0x17, + 0x41, 0x18, 0x89, 0x16, 0xe1, 0x18, 0xd0, 0x18, 0xa1, 0x7f, 0x27, 0x25, 0x16, 0x25, 0x98, 0x2e, 0x79, 0xc0, 0x8b, + 0x54, 0x90, 0x7f, 0xb3, 0x30, 0x82, 0x40, 0x80, 0x90, 0x0d, 0x2f, 0x7d, 0x52, 0x92, 0x6f, 0x98, 0x2e, 0x0f, 0xca, + 0xb2, 0x6f, 0x90, 0x0e, 0x06, 0x2f, 0x8b, 0x50, 0x14, 0x30, 0x42, 0x6f, 0x51, 0x6f, 0x14, 0x42, 0x12, 0x42, 0x01, + 0x42, 0x00, 0x2e, 0x31, 0x6f, 0x98, 0x2e, 0x74, 0xc0, 0x41, 0x6f, 0x80, 0x7f, 0x98, 0x2e, 0x74, 0xc0, 0x82, 0x6f, + 0x10, 0x04, 0x43, 0x52, 0x01, 0x0f, 0x05, 0x2e, 0xcb, 0x00, 0x00, 0x30, 0x04, 0x30, 0x21, 0x2f, 0x51, 0x6f, 0x43, + 0x58, 0x8c, 0x0e, 0x04, 0x30, 0x1c, 0x2f, 0x85, 0x88, 0x41, 0x6f, 0x04, 0x41, 0x8c, 0x0f, 0x04, 0x30, 0x16, 0x2f, + 0x84, 0x88, 0x00, 0x2e, 0x04, 0x41, 0x04, 0x05, 0x8c, 0x0e, 0x04, 0x30, 0x0f, 0x2f, 0x82, 0x88, 0x31, 0x6f, 0x04, + 0x41, 0x04, 0x05, 0x8c, 0x0e, 0x04, 0x30, 0x08, 0x2f, 0x83, 0x88, 0x00, 0x2e, 0x04, 0x41, 0x8c, 0x0f, 0x04, 0x30, + 0x02, 0x2f, 0x21, 0x2e, 0xad, 0x01, 0x14, 0x30, 0x00, 0x91, 0x14, 0x2f, 0x03, 0x2e, 0xa1, 0x01, 0x41, 0x90, 0x0e, + 0x2f, 0x03, 0x2e, 0xad, 0x01, 0x14, 0x30, 0x4c, 0x28, 0x23, 0x2e, 0xad, 0x01, 0x46, 0xa0, 0x06, 0x2f, 0x81, 0x84, + 0x8d, 0x52, 0x48, 0x82, 0x82, 0x40, 0x21, 0x2e, 0xa1, 0x01, 0x42, 0x42, 0x5c, 0x2c, 0x02, 0x30, 0x05, 0x2e, 0xaa, + 0x01, 0x80, 0xb2, 0x02, 0x30, 0x55, 0x2f, 0x03, 0x2e, 0xa9, 0x01, 0x92, 0x6f, 0xb3, 0x30, 0x98, 0x2e, 0x0f, 0xca, + 0xb2, 0x6f, 0x90, 0x0f, 0x00, 0x30, 0x02, 0x30, 0x4a, 0x2f, 0xa2, 0x6f, 0x87, 0x52, 0x91, 0x00, 0x85, 0x52, 0x51, + 0x0e, 0x02, 0x2f, 0x00, 0x2e, 0x43, 0x2c, 0x02, 0x30, 0xc2, 0x6f, 0x7f, 0x52, 0x91, 0x0e, 0x02, 0x30, 0x3c, 0x2f, + 0x51, 0x6f, 0x81, 0x54, 0x98, 0x2e, 0xfe, 0xc9, 0x10, 0x25, 0xb3, 0x30, 0x21, 0x25, 0x98, 0x2e, 0x0f, 0xca, 0x32, + 0x6f, 0xc0, 0x7f, 0xb3, 0x30, 0x12, 0x25, 0x98, 0x2e, 0x0f, 0xca, 0x42, 0x6f, 0xb0, 0x7f, 0xb3, 0x30, 0x12, 0x25, + 0x98, 0x2e, 0x0f, 0xca, 0xb2, 0x6f, 0x90, 0x28, 0x83, 0x52, 0x98, 0x2e, 0xfe, 0xc9, 0xc2, 0x6f, 0x90, 0x0f, 0x00, + 0x30, 0x02, 0x30, 0x1d, 0x2f, 0x05, 0x2e, 0xa1, 0x01, 0x80, 0xb2, 0x12, 0x30, 0x0f, 0x2f, 0x42, 0x6f, 0x03, 0x2e, + 0xab, 0x01, 0x91, 0x0e, 0x02, 0x30, 0x12, 0x2f, 0x52, 0x6f, 0x03, 0x2e, 0xac, 0x01, 0x91, 0x0f, 0x02, 0x30, 0x0c, + 0x2f, 0x21, 0x2e, 0xaa, 0x01, 0x0a, 0x2c, 0x12, 0x30, 0x03, 0x2e, 0xcb, 0x00, 0x8d, 0x58, 0x08, 0x89, 0x41, 0x40, + 0x11, 0x43, 0x00, 0x43, 0x25, 0x2e, 0xa1, 0x01, 0xd4, 0x6f, 0x8f, 0x52, 0x00, 0x43, 0x3a, 0x89, 0x00, 0x2e, 0x10, + 0x43, 0x10, 0x43, 0x61, 0x0e, 0xfb, 0x2f, 0x03, 0x2e, 0xa0, 0x01, 0x11, 0x1a, 0x02, 0x2f, 0x02, 0x25, 0x21, 0x2e, + 0xa0, 0x01, 0xeb, 0x6f, 0x00, 0x5f, 0xb8, 0x2e, 0x91, 0x52, 0x10, 0x30, 0x02, 0x30, 0x95, 0x56, 0x52, 0x42, 0x4b, + 0x0e, 0xfc, 0x2f, 0x8d, 0x54, 0x88, 0x82, 0x93, 0x56, 0x80, 0x42, 0x53, 0x42, 0x40, 0x42, 0x42, 0x86, 0x83, 0x54, + 0xc0, 0x2e, 0xc2, 0x42, 0x00, 0x2e, 0xa3, 0x52, 0x00, 0x51, 0x52, 0x40, 0x47, 0x40, 0x1a, 0x25, 0x01, 0x2e, 0x97, + 0x00, 0x8f, 0xbe, 0x72, 0x86, 0xfb, 0x7f, 0x0b, 0x30, 0x7c, 0xbf, 0xa5, 0x50, 0x10, 0x08, 0xdf, 0xba, 0x70, 0x88, + 0xf8, 0xbf, 0xcb, 0x42, 0xd3, 0x7f, 0x6c, 0xbb, 0xfc, 0xbb, 0xc5, 0x0a, 0x90, 0x7f, 0x1b, 0x7f, 0x0b, 0x43, 0xc0, + 0xb2, 0xe5, 0x7f, 0xb7, 0x7f, 0xa6, 0x7f, 0xc4, 0x7f, 0x90, 0x2e, 0x1c, 0xb7, 0x07, 0x2e, 0xd2, 0x00, 0xc0, 0xb2, + 0x0b, 0x2f, 0x97, 0x52, 0x01, 0x2e, 0xcd, 0x00, 0x82, 0x7f, 0x98, 0x2e, 0xbb, 0xcc, 0x0b, 0x30, 0x37, 0x2e, 0xd2, + 0x00, 0x82, 0x6f, 0x90, 0x6f, 0x1a, 0x25, 0x00, 0xb2, 0x8b, 0x7f, 0x14, 0x2f, 0xa6, 0xbd, 0x25, 0xbd, 0xb6, 0xb9, + 0x2f, 0xb9, 0x80, 0xb2, 0xd4, 0xb0, 0x0c, 0x2f, 0x99, 0x54, 0x9b, 0x56, 0x0b, 0x30, 0x0b, 0x2e, 0xb1, 0x00, 0xa1, + 0x58, 0x9b, 0x42, 0xdb, 0x42, 0x6c, 0x09, 0x2b, 0x2e, 0xb1, 0x00, 0x8b, 0x42, 0xcb, 0x42, 0x86, 0x7f, 0x73, 0x84, + 0xa7, 0x56, 0xc3, 0x08, 0x39, 0x52, 0x05, 0x50, 0x72, 0x7f, 0x63, 0x7f, 0x98, 0x2e, 0xc2, 0xc0, 0xe1, 0x6f, 0x62, + 0x6f, 0xd1, 0x0a, 0x01, 0x2e, 0xcd, 0x00, 0xd5, 0x6f, 0xc4, 0x6f, 0x72, 0x6f, 0x97, 0x52, 0x9d, 0x5c, 0x98, 0x2e, + 0x06, 0xcd, 0x23, 0x6f, 0x90, 0x6f, 0x99, 0x52, 0xc0, 0xb2, 0x04, 0xbd, 0x54, 0x40, 0xaf, 0xb9, 0x45, 0x40, 0xe1, + 0x7f, 0x02, 0x30, 0x06, 0x2f, 0xc0, 0xb2, 0x02, 0x30, 0x03, 0x2f, 0x9b, 0x5c, 0x12, 0x30, 0x94, 0x43, 0x85, 0x43, + 0x03, 0xbf, 0x6f, 0xbb, 0x80, 0xb3, 0x20, 0x2f, 0x06, 0x6f, 0x26, 0x01, 0x16, 0x6f, 0x6e, 0x03, 0x45, 0x42, 0xc0, + 0x90, 0x29, 0x2e, 0xce, 0x00, 0x9b, 0x52, 0x14, 0x2f, 0x9b, 0x5c, 0x00, 0x2e, 0x93, 0x41, 0x86, 0x41, 0xe3, 0x04, + 0xae, 0x07, 0x80, 0xab, 0x04, 0x2f, 0x80, 0x91, 0x0a, 0x2f, 0x86, 0x6f, 0x73, 0x0f, 0x07, 0x2f, 0x83, 0x6f, 0xc0, + 0xb2, 0x04, 0x2f, 0x54, 0x42, 0x45, 0x42, 0x12, 0x30, 0x04, 0x2c, 0x11, 0x30, 0x02, 0x2c, 0x11, 0x30, 0x11, 0x30, + 0x02, 0xbc, 0x0f, 0xb8, 0xd2, 0x7f, 0x00, 0xb2, 0x0a, 0x2f, 0x01, 0x2e, 0xfc, 0x00, 0x05, 0x2e, 0xc7, 0x01, 0x10, + 0x1a, 0x02, 0x2f, 0x21, 0x2e, 0xc7, 0x01, 0x03, 0x2d, 0x02, 0x2c, 0x01, 0x30, 0x01, 0x30, 0xb0, 0x6f, 0x98, 0x2e, + 0x95, 0xcf, 0xd1, 0x6f, 0xa0, 0x6f, 0x98, 0x2e, 0x95, 0xcf, 0xe2, 0x6f, 0x9f, 0x52, 0x01, 0x2e, 0xce, 0x00, 0x82, + 0x40, 0x50, 0x42, 0x0c, 0x2c, 0x42, 0x42, 0x11, 0x30, 0x23, 0x2e, 0xd2, 0x00, 0x01, 0x30, 0xb0, 0x6f, 0x98, 0x2e, + 0x95, 0xcf, 0xa0, 0x6f, 0x01, 0x30, 0x98, 0x2e, 0x95, 0xcf, 0x00, 0x2e, 0xfb, 0x6f, 0x00, 0x5f, 0xb8, 0x2e, 0x83, + 0x86, 0x01, 0x30, 0x00, 0x30, 0x94, 0x40, 0x24, 0x18, 0x06, 0x00, 0x53, 0x0e, 0x4f, 0x02, 0xf9, 0x2f, 0xb8, 0x2e, + 0xa9, 0x52, 0x00, 0x2e, 0x60, 0x40, 0x41, 0x40, 0x0d, 0xbc, 0x98, 0xbc, 0xc0, 0x2e, 0x01, 0x0a, 0x0f, 0xb8, 0xab, + 0x52, 0x53, 0x3c, 0x52, 0x40, 0x40, 0x40, 0x4b, 0x00, 0x82, 0x16, 0x26, 0xb9, 0x01, 0xb8, 0x41, 0x40, 0x10, 0x08, + 0x97, 0xb8, 0x01, 0x08, 0xc0, 0x2e, 0x11, 0x30, 0x01, 0x08, 0x43, 0x86, 0x25, 0x40, 0x04, 0x40, 0xd8, 0xbe, 0x2c, + 0x0b, 0x22, 0x11, 0x54, 0x42, 0x03, 0x80, 0x4b, 0x0e, 0xf6, 0x2f, 0xb8, 0x2e, 0x9f, 0x50, 0x10, 0x50, 0xad, 0x52, + 0x05, 0x2e, 0xd3, 0x00, 0xfb, 0x7f, 0x00, 0x2e, 0x13, 0x40, 0x93, 0x42, 0x41, 0x0e, 0xfb, 0x2f, 0x98, 0x2e, 0xa5, + 0xb7, 0x98, 0x2e, 0x87, 0xcf, 0x01, 0x2e, 0xd9, 0x00, 0x00, 0xb2, 0xfb, 0x6f, 0x0b, 0x2f, 0x01, 0x2e, 0x69, 0xf7, + 0xb1, 0x3f, 0x01, 0x08, 0x01, 0x30, 0xf0, 0x5f, 0x23, 0x2e, 0xd9, 0x00, 0x21, 0x2e, 0x69, 0xf7, 0x80, 0x2e, 0x7a, + 0xb7, 0xf0, 0x5f, 0xb8, 0x2e, 0x01, 0x2e, 0xc0, 0xf8, 0x03, 0x2e, 0xfc, 0xf5, 0x15, 0x54, 0xaf, 0x56, 0x82, 0x08, + 0x0b, 0x2e, 0x69, 0xf7, 0xcb, 0x0a, 0xb1, 0x58, 0x80, 0x90, 0xdd, 0xbe, 0x4c, 0x08, 0x5f, 0xb9, 0x59, 0x22, 0x80, + 0x90, 0x07, 0x2f, 0x03, 0x34, 0xc3, 0x08, 0xf2, 0x3a, 0x0a, 0x08, 0x02, 0x35, 0xc0, 0x90, 0x4a, 0x0a, 0x48, 0x22, + 0xc0, 0x2e, 0x23, 0x2e, 0xfc, 0xf5, 0x10, 0x50, 0xfb, 0x7f, 0x98, 0x2e, 0x56, 0xc7, 0x98, 0x2e, 0x49, 0xc3, 0x10, + 0x30, 0xfb, 0x6f, 0xf0, 0x5f, 0x21, 0x2e, 0xcc, 0x00, 0x21, 0x2e, 0xca, 0x00, 0xb8, 0x2e, 0x03, 0x2e, 0xd3, 0x00, + 0x16, 0xb8, 0x02, 0x34, 0x4a, 0x0c, 0x21, 0x2e, 0x2d, 0xf5, 0xc0, 0x2e, 0x23, 0x2e, 0xd3, 0x00, 0x03, 0xbc, 0x21, + 0x2e, 0xd5, 0x00, 0x03, 0x2e, 0xd5, 0x00, 0x40, 0xb2, 0x10, 0x30, 0x21, 0x2e, 0x77, 0x00, 0x01, 0x30, 0x05, 0x2f, + 0x05, 0x2e, 0xd8, 0x00, 0x80, 0x90, 0x01, 0x2f, 0x23, 0x2e, 0x6f, 0xf5, 0xc0, 0x2e, 0x21, 0x2e, 0xd9, 0x00, 0x11, + 0x30, 0x81, 0x08, 0x01, 0x2e, 0x6a, 0xf7, 0x71, 0x3f, 0x23, 0xbd, 0x01, 0x08, 0x02, 0x0a, 0xc0, 0x2e, 0x21, 0x2e, + 0x6a, 0xf7, 0x30, 0x25, 0x00, 0x30, 0x21, 0x2e, 0x5a, 0xf5, 0x10, 0x50, 0x21, 0x2e, 0x7b, 0x00, 0x21, 0x2e, 0x7c, + 0x00, 0xfb, 0x7f, 0x98, 0x2e, 0xc3, 0xb7, 0x40, 0x30, 0x21, 0x2e, 0xd4, 0x00, 0xfb, 0x6f, 0xf0, 0x5f, 0x03, 0x25, + 0x80, 0x2e, 0xaf, 0xb7, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x01, 0x2e, 0x5d, 0xf7, 0x08, 0xbc, 0x80, 0xac, 0x0e, 0xbb, 0x02, 0x2f, + 0x00, 0x30, 0x41, 0x04, 0x82, 0x06, 0xc0, 0xa4, 0x00, 0x30, 0x11, 0x2f, 0x40, 0xa9, 0x03, 0x2f, 0x40, 0x91, 0x0d, + 0x2f, 0x00, 0xa7, 0x0b, 0x2f, 0x80, 0xb3, 0xb3, 0x58, 0x02, 0x2f, 0x90, 0xa1, 0x26, 0x13, 0x20, 0x23, 0x80, 0x90, + 0x10, 0x30, 0x01, 0x2f, 0xcc, 0x0e, 0x00, 0x2f, 0x00, 0x30, 0xb8, 0x2e, 0xb5, 0x50, 0x18, 0x08, 0x08, 0xbc, 0x88, + 0xb6, 0x0d, 0x17, 0xc6, 0xbd, 0x56, 0xbc, 0xb7, 0x58, 0xda, 0xba, 0x04, 0x01, 0x1d, 0x0a, 0x10, 0x50, 0x05, 0x30, + 0x32, 0x25, 0x45, 0x03, 0xfb, 0x7f, 0xf6, 0x30, 0x21, 0x25, 0x98, 0x2e, 0x37, 0xca, 0x16, 0xb5, 0x9a, 0xbc, 0x06, + 0xb8, 0x80, 0xa8, 0x41, 0x0a, 0x0e, 0x2f, 0x80, 0x90, 0x02, 0x2f, 0x2d, 0x50, 0x48, 0x0f, 0x09, 0x2f, 0xbf, 0xa0, + 0x04, 0x2f, 0xbf, 0x90, 0x06, 0x2f, 0xb7, 0x54, 0xca, 0x0f, 0x03, 0x2f, 0x00, 0x2e, 0x02, 0x2c, 0xb7, 0x52, 0x2d, + 0x52, 0xf2, 0x33, 0x98, 0x2e, 0xd9, 0xc0, 0xfb, 0x6f, 0xf1, 0x37, 0xc0, 0x2e, 0x01, 0x08, 0xf0, 0x5f, 0xbf, 0x56, + 0xb9, 0x54, 0xd0, 0x40, 0xc4, 0x40, 0x0b, 0x2e, 0xfd, 0xf3, 0xbf, 0x52, 0x90, 0x42, 0x94, 0x42, 0x95, 0x42, 0x05, + 0x30, 0xc1, 0x50, 0x0f, 0x88, 0x06, 0x40, 0x04, 0x41, 0x96, 0x42, 0xc5, 0x42, 0x48, 0xbe, 0x73, 0x30, 0x0d, 0x2e, + 0xd8, 0x00, 0x4f, 0xba, 0x84, 0x42, 0x03, 0x42, 0x81, 0xb3, 0x02, 0x2f, 0x2b, 0x2e, 0x6f, 0xf5, 0x06, 0x2d, 0x05, + 0x2e, 0x77, 0xf7, 0xbd, 0x56, 0x93, 0x08, 0x25, 0x2e, 0x77, 0xf7, 0xbb, 0x54, 0x25, 0x2e, 0xc2, 0xf5, 0x07, 0x2e, + 0xfd, 0xf3, 0x42, 0x30, 0xb4, 0x33, 0xda, 0x0a, 0x4c, 0x00, 0x27, 0x2e, 0xfd, 0xf3, 0x43, 0x40, 0xd4, 0x3f, 0xdc, + 0x08, 0x43, 0x42, 0x00, 0x2e, 0x00, 0x2e, 0x43, 0x40, 0x24, 0x30, 0xdc, 0x0a, 0x43, 0x42, 0x04, 0x80, 0x03, 0x2e, + 0xfd, 0xf3, 0x4a, 0x0a, 0x23, 0x2e, 0xfd, 0xf3, 0x61, 0x34, 0xc0, 0x2e, 0x01, 0x42, 0x00, 0x2e, 0x60, 0x50, 0x1a, + 0x25, 0x7a, 0x86, 0xe0, 0x7f, 0xf3, 0x7f, 0x03, 0x25, 0xc3, 0x52, 0x41, 0x84, 0xdb, 0x7f, 0x33, 0x30, 0x98, 0x2e, + 0x16, 0xc2, 0x1a, 0x25, 0x7d, 0x82, 0xf0, 0x6f, 0xe2, 0x6f, 0x32, 0x25, 0x16, 0x40, 0x94, 0x40, 0x26, 0x01, 0x85, + 0x40, 0x8e, 0x17, 0xc4, 0x42, 0x6e, 0x03, 0x95, 0x42, 0x41, 0x0e, 0xf4, 0x2f, 0xdb, 0x6f, 0xa0, 0x5f, 0xb8, 0x2e, + 0xb0, 0x51, 0xfb, 0x7f, 0x98, 0x2e, 0xe8, 0x0d, 0x5a, 0x25, 0x98, 0x2e, 0x0f, 0x0e, 0xcb, 0x58, 0x32, 0x87, 0xc4, + 0x7f, 0x65, 0x89, 0x6b, 0x8d, 0xc5, 0x5a, 0x65, 0x7f, 0xe1, 0x7f, 0x83, 0x7f, 0xa6, 0x7f, 0x74, 0x7f, 0xd0, 0x7f, + 0xb6, 0x7f, 0x94, 0x7f, 0x17, 0x30, 0xc7, 0x52, 0xc9, 0x54, 0x51, 0x7f, 0x00, 0x2e, 0x85, 0x6f, 0x42, 0x7f, 0x00, + 0x2e, 0x51, 0x41, 0x45, 0x81, 0x42, 0x41, 0x13, 0x40, 0x3b, 0x8a, 0x00, 0x40, 0x4b, 0x04, 0xd0, 0x06, 0xc0, 0xac, + 0x85, 0x7f, 0x02, 0x2f, 0x02, 0x30, 0x51, 0x04, 0xd3, 0x06, 0x41, 0x84, 0x05, 0x30, 0x5d, 0x02, 0xc9, 0x16, 0xdf, + 0x08, 0xd3, 0x00, 0x8d, 0x02, 0xaf, 0xbc, 0xb1, 0xb9, 0x59, 0x0a, 0x65, 0x6f, 0x11, 0x43, 0xa1, 0xb4, 0x52, 0x41, + 0x53, 0x41, 0x01, 0x43, 0x34, 0x7f, 0x65, 0x7f, 0x26, 0x31, 0xe5, 0x6f, 0xd4, 0x6f, 0x98, 0x2e, 0x37, 0xca, 0x32, + 0x6f, 0x75, 0x6f, 0x83, 0x40, 0x42, 0x41, 0x23, 0x7f, 0x12, 0x7f, 0xf6, 0x30, 0x40, 0x25, 0x51, 0x25, 0x98, 0x2e, + 0x37, 0xca, 0x14, 0x6f, 0x20, 0x05, 0x70, 0x6f, 0x25, 0x6f, 0x69, 0x07, 0xa2, 0x6f, 0x31, 0x6f, 0x0b, 0x30, 0x04, + 0x42, 0x9b, 0x42, 0x8b, 0x42, 0x55, 0x42, 0x32, 0x7f, 0x40, 0xa9, 0xc3, 0x6f, 0x71, 0x7f, 0x02, 0x30, 0xd0, 0x40, + 0xc3, 0x7f, 0x03, 0x2f, 0x40, 0x91, 0x15, 0x2f, 0x00, 0xa7, 0x13, 0x2f, 0x00, 0xa4, 0x11, 0x2f, 0x84, 0xbd, 0x98, + 0x2e, 0x79, 0xca, 0x55, 0x6f, 0xb7, 0x54, 0x54, 0x41, 0x82, 0x00, 0xf3, 0x3f, 0x45, 0x41, 0xcb, 0x02, 0xf6, 0x30, + 0x98, 0x2e, 0x37, 0xca, 0x35, 0x6f, 0xa4, 0x6f, 0x41, 0x43, 0x03, 0x2c, 0x00, 0x43, 0xa4, 0x6f, 0x35, 0x6f, 0x17, + 0x30, 0x42, 0x6f, 0x51, 0x6f, 0x93, 0x40, 0x42, 0x82, 0x00, 0x41, 0xc3, 0x00, 0x03, 0x43, 0x51, 0x7f, 0x00, 0x2e, + 0x94, 0x40, 0x41, 0x41, 0x4c, 0x02, 0xc4, 0x6f, 0xd1, 0x56, 0x63, 0x0e, 0x74, 0x6f, 0x51, 0x43, 0xa5, 0x7f, 0x8a, + 0x2f, 0x09, 0x2e, 0xd8, 0x00, 0x01, 0xb3, 0x21, 0x2f, 0xcb, 0x58, 0x90, 0x6f, 0x13, 0x41, 0xb6, 0x6f, 0xe4, 0x7f, + 0x00, 0x2e, 0x91, 0x41, 0x14, 0x40, 0x92, 0x41, 0x15, 0x40, 0x17, 0x2e, 0x6f, 0xf5, 0xb6, 0x7f, 0xd0, 0x7f, 0xcb, + 0x7f, 0x98, 0x2e, 0x00, 0x0c, 0x07, 0x15, 0xc2, 0x6f, 0x14, 0x0b, 0x29, 0x2e, 0x6f, 0xf5, 0xc3, 0xa3, 0xc1, 0x8f, + 0xe4, 0x6f, 0xd0, 0x6f, 0xe6, 0x2f, 0x14, 0x30, 0x05, 0x2e, 0x6f, 0xf5, 0x14, 0x0b, 0x29, 0x2e, 0x6f, 0xf5, 0x18, + 0x2d, 0xcd, 0x56, 0x04, 0x32, 0xb5, 0x6f, 0x1c, 0x01, 0x51, 0x41, 0x52, 0x41, 0xc3, 0x40, 0xb5, 0x7f, 0xe4, 0x7f, + 0x98, 0x2e, 0x1f, 0x0c, 0xe4, 0x6f, 0x21, 0x87, 0x00, 0x43, 0x04, 0x32, 0xcf, 0x54, 0x5a, 0x0e, 0xef, 0x2f, 0x15, + 0x54, 0x09, 0x2e, 0x77, 0xf7, 0x22, 0x0b, 0x29, 0x2e, 0x77, 0xf7, 0xfb, 0x6f, 0x50, 0x5e, 0xb8, 0x2e, 0x10, 0x50, + 0x01, 0x2e, 0xd4, 0x00, 0x00, 0xb2, 0xfb, 0x7f, 0x51, 0x2f, 0x01, 0xb2, 0x48, 0x2f, 0x02, 0xb2, 0x42, 0x2f, 0x03, + 0x90, 0x56, 0x2f, 0xd7, 0x52, 0x79, 0x80, 0x42, 0x40, 0x81, 0x84, 0x00, 0x40, 0x42, 0x42, 0x98, 0x2e, 0x93, 0x0c, + 0xd9, 0x54, 0xd7, 0x50, 0xa1, 0x40, 0x98, 0xbd, 0x82, 0x40, 0x3e, 0x82, 0xda, 0x0a, 0x44, 0x40, 0x8b, 0x16, 0xe3, + 0x00, 0x53, 0x42, 0x00, 0x2e, 0x43, 0x40, 0x9a, 0x02, 0x52, 0x42, 0x00, 0x2e, 0x41, 0x40, 0x15, 0x54, 0x4a, 0x0e, + 0x3a, 0x2f, 0x3a, 0x82, 0x00, 0x30, 0x41, 0x40, 0x21, 0x2e, 0x85, 0x0f, 0x40, 0xb2, 0x0a, 0x2f, 0x98, 0x2e, 0xb1, + 0x0c, 0x98, 0x2e, 0x45, 0x0e, 0x98, 0x2e, 0x5b, 0x0e, 0xfb, 0x6f, 0xf0, 0x5f, 0x00, 0x30, 0x80, 0x2e, 0xce, 0xb7, + 0xdd, 0x52, 0xd3, 0x54, 0x42, 0x42, 0x4f, 0x84, 0x73, 0x30, 0xdb, 0x52, 0x83, 0x42, 0x1b, 0x30, 0x6b, 0x42, 0x23, + 0x30, 0x27, 0x2e, 0xd7, 0x00, 0x37, 0x2e, 0xd4, 0x00, 0x21, 0x2e, 0xd6, 0x00, 0x7a, 0x84, 0x17, 0x2c, 0x42, 0x42, + 0x30, 0x30, 0x21, 0x2e, 0xd4, 0x00, 0x12, 0x2d, 0x21, 0x30, 0x00, 0x30, 0x23, 0x2e, 0xd4, 0x00, 0x21, 0x2e, 0x7b, + 0xf7, 0x0b, 0x2d, 0x17, 0x30, 0x98, 0x2e, 0x51, 0x0c, 0xd5, 0x50, 0x0c, 0x82, 0x72, 0x30, 0x2f, 0x2e, 0xd4, 0x00, + 0x25, 0x2e, 0x7b, 0xf7, 0x40, 0x42, 0x00, 0x2e, 0xfb, 0x6f, 0xf0, 0x5f, 0xb8, 0x2e, 0x70, 0x50, 0x0a, 0x25, 0x39, + 0x86, 0xfb, 0x7f, 0xe1, 0x32, 0x62, 0x30, 0x98, 0x2e, 0xc2, 0xc4, 0xb5, 0x56, 0xa5, 0x6f, 0xab, 0x08, 0x91, 0x6f, + 0x4b, 0x08, 0xdf, 0x56, 0xc4, 0x6f, 0x23, 0x09, 0x4d, 0xba, 0x93, 0xbc, 0x8c, 0x0b, 0xd1, 0x6f, 0x0b, 0x09, 0xcb, + 0x52, 0xe1, 0x5e, 0x56, 0x42, 0xaf, 0x09, 0x4d, 0xba, 0x23, 0xbd, 0x94, 0x0a, 0xe5, 0x6f, 0x68, 0xbb, 0xeb, 0x08, + 0xbd, 0xb9, 0x63, 0xbe, 0xfb, 0x6f, 0x52, 0x42, 0xe3, 0x0a, 0xc0, 0x2e, 0x43, 0x42, 0x90, 0x5f, 0xd1, 0x50, 0x03, + 0x2e, 0x25, 0xf3, 0x13, 0x40, 0x00, 0x40, 0x9b, 0xbc, 0x9b, 0xb4, 0x08, 0xbd, 0xb8, 0xb9, 0x98, 0xbc, 0xda, 0x0a, + 0x08, 0xb6, 0x89, 0x16, 0xc0, 0x2e, 0x19, 0x00, 0x62, 0x02, 0x10, 0x50, 0xfb, 0x7f, 0x98, 0x2e, 0x81, 0x0d, 0x01, + 0x2e, 0xd4, 0x00, 0x31, 0x30, 0x08, 0x04, 0xfb, 0x6f, 0x01, 0x30, 0xf0, 0x5f, 0x23, 0x2e, 0xd6, 0x00, 0x21, 0x2e, + 0xd7, 0x00, 0xb8, 0x2e, 0x01, 0x2e, 0xd7, 0x00, 0x03, 0x2e, 0xd6, 0x00, 0x48, 0x0e, 0x01, 0x2f, 0x80, 0x2e, 0x1f, + 0x0e, 0xb8, 0x2e, 0xe3, 0x50, 0x21, 0x34, 0x01, 0x42, 0x82, 0x30, 0xc1, 0x32, 0x25, 0x2e, 0x62, 0xf5, 0x01, 0x00, + 0x22, 0x30, 0x01, 0x40, 0x4a, 0x0a, 0x01, 0x42, 0xb8, 0x2e, 0xe3, 0x54, 0xf0, 0x3b, 0x83, 0x40, 0xd8, 0x08, 0xe5, + 0x52, 0x83, 0x42, 0x00, 0x30, 0x83, 0x30, 0x50, 0x42, 0xc4, 0x32, 0x27, 0x2e, 0x64, 0xf5, 0x94, 0x00, 0x50, 0x42, + 0x40, 0x42, 0xd3, 0x3f, 0x84, 0x40, 0x7d, 0x82, 0xe3, 0x08, 0x40, 0x42, 0x83, 0x42, 0xb8, 0x2e, 0xdd, 0x52, 0x00, + 0x30, 0x40, 0x42, 0x7c, 0x86, 0xb9, 0x52, 0x09, 0x2e, 0x70, 0x0f, 0xbf, 0x54, 0xc4, 0x42, 0xd3, 0x86, 0x54, 0x40, + 0x55, 0x40, 0x94, 0x42, 0x85, 0x42, 0x21, 0x2e, 0xd7, 0x00, 0x42, 0x40, 0x25, 0x2e, 0xfd, 0xf3, 0xc0, 0x42, 0x7e, + 0x82, 0x05, 0x2e, 0x7d, 0x00, 0x80, 0xb2, 0x14, 0x2f, 0x05, 0x2e, 0x89, 0x00, 0x27, 0xbd, 0x2f, 0xb9, 0x80, 0x90, + 0x02, 0x2f, 0x21, 0x2e, 0x6f, 0xf5, 0x0c, 0x2d, 0x07, 0x2e, 0x71, 0x0f, 0x14, 0x30, 0x1c, 0x09, 0x05, 0x2e, 0x77, + 0xf7, 0xbd, 0x56, 0x47, 0xbe, 0x93, 0x08, 0x94, 0x0a, 0x25, 0x2e, 0x77, 0xf7, 0xe7, 0x54, 0x50, 0x42, 0x4a, 0x0e, + 0xfc, 0x2f, 0xb8, 0x2e, 0x50, 0x50, 0x02, 0x30, 0x43, 0x86, 0xe5, 0x50, 0xfb, 0x7f, 0xe3, 0x7f, 0xd2, 0x7f, 0xc0, + 0x7f, 0xb1, 0x7f, 0x00, 0x2e, 0x41, 0x40, 0x00, 0x40, 0x48, 0x04, 0x98, 0x2e, 0x74, 0xc0, 0x1e, 0xaa, 0xd3, 0x6f, + 0x14, 0x30, 0xb1, 0x6f, 0xe3, 0x22, 0xc0, 0x6f, 0x52, 0x40, 0xe4, 0x6f, 0x4c, 0x0e, 0x12, 0x42, 0xd3, 0x7f, 0xeb, + 0x2f, 0x03, 0x2e, 0x86, 0x0f, 0x40, 0x90, 0x11, 0x30, 0x03, 0x2f, 0x23, 0x2e, 0x86, 0x0f, 0x02, 0x2c, 0x00, 0x30, + 0xd0, 0x6f, 0xfb, 0x6f, 0xb0, 0x5f, 0xb8, 0x2e, 0x40, 0x50, 0xf1, 0x7f, 0x0a, 0x25, 0x3c, 0x86, 0xeb, 0x7f, 0x41, + 0x33, 0x22, 0x30, 0x98, 0x2e, 0xc2, 0xc4, 0xd3, 0x6f, 0xf4, 0x30, 0xdc, 0x09, 0x47, 0x58, 0xc2, 0x6f, 0x94, 0x09, + 0xeb, 0x58, 0x6a, 0xbb, 0xdc, 0x08, 0xb4, 0xb9, 0xb1, 0xbd, 0xe9, 0x5a, 0x95, 0x08, 0x21, 0xbd, 0xf6, 0xbf, 0x77, + 0x0b, 0x51, 0xbe, 0xf1, 0x6f, 0xeb, 0x6f, 0x52, 0x42, 0x54, 0x42, 0xc0, 0x2e, 0x43, 0x42, 0xc0, 0x5f, 0x50, 0x50, + 0xf5, 0x50, 0x31, 0x30, 0x11, 0x42, 0xfb, 0x7f, 0x7b, 0x30, 0x0b, 0x42, 0x11, 0x30, 0x02, 0x80, 0x23, 0x33, 0x01, + 0x42, 0x03, 0x00, 0x07, 0x2e, 0x80, 0x03, 0x05, 0x2e, 0xd3, 0x00, 0x23, 0x52, 0xe2, 0x7f, 0xd3, 0x7f, 0xc0, 0x7f, + 0x98, 0x2e, 0xb6, 0x0e, 0xd1, 0x6f, 0x08, 0x0a, 0x1a, 0x25, 0x7b, 0x86, 0xd0, 0x7f, 0x01, 0x33, 0x12, 0x30, 0x98, + 0x2e, 0xc2, 0xc4, 0xd1, 0x6f, 0x08, 0x0a, 0x00, 0xb2, 0x0d, 0x2f, 0xe3, 0x6f, 0x01, 0x2e, 0x80, 0x03, 0x51, 0x30, + 0xc7, 0x86, 0x23, 0x2e, 0x21, 0xf2, 0x08, 0xbc, 0xc0, 0x42, 0x98, 0x2e, 0xa5, 0xb7, 0x00, 0x2e, 0x00, 0x2e, 0xd0, + 0x2e, 0xb0, 0x6f, 0x0b, 0xb8, 0x03, 0x2e, 0x1b, 0x00, 0x08, 0x1a, 0xb0, 0x7f, 0x70, 0x30, 0x04, 0x2f, 0x21, 0x2e, + 0x21, 0xf2, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0x98, 0x2e, 0x6d, 0xc0, 0x98, 0x2e, 0x5d, 0xc0, 0xed, 0x50, 0x98, + 0x2e, 0x44, 0xcb, 0xef, 0x50, 0x98, 0x2e, 0x46, 0xc3, 0xf1, 0x50, 0x98, 0x2e, 0x53, 0xc7, 0x35, 0x50, 0x98, 0x2e, + 0x64, 0xcf, 0x10, 0x30, 0x98, 0x2e, 0xdc, 0x03, 0x20, 0x26, 0xc0, 0x6f, 0x02, 0x31, 0x12, 0x42, 0xab, 0x33, 0x0b, + 0x42, 0x37, 0x80, 0x01, 0x30, 0x01, 0x42, 0xf3, 0x37, 0xf7, 0x52, 0xfb, 0x50, 0x44, 0x40, 0xa2, 0x0a, 0x42, 0x42, + 0x8b, 0x31, 0x09, 0x2e, 0x5e, 0xf7, 0xf9, 0x54, 0xe3, 0x08, 0x83, 0x42, 0x1b, 0x42, 0x23, 0x33, 0x4b, 0x00, 0xbc, + 0x84, 0x0b, 0x40, 0x33, 0x30, 0x83, 0x42, 0x0b, 0x42, 0xe0, 0x7f, 0xd1, 0x7f, 0x98, 0x2e, 0x58, 0xb7, 0xd1, 0x6f, + 0x80, 0x30, 0x40, 0x42, 0x03, 0x30, 0xe0, 0x6f, 0xf3, 0x54, 0x04, 0x30, 0x00, 0x2e, 0x00, 0x2e, 0x01, 0x89, 0x62, + 0x0e, 0xfa, 0x2f, 0x43, 0x42, 0x11, 0x30, 0xfb, 0x6f, 0xc0, 0x2e, 0x01, 0x42, 0xb0, 0x5f, 0xc1, 0x4a, 0x00, 0x00, + 0x6d, 0x57, 0x00, 0x00, 0x77, 0x8e, 0x00, 0x00, 0xe0, 0xff, 0xff, 0xff, 0xd3, 0xff, 0xff, 0xff, 0xe5, 0xff, 0xff, + 0xff, 0xee, 0xe1, 0xff, 0xff, 0x7c, 0x13, 0x00, 0x00, 0x46, 0xe6, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1 +}; + +} \ No newline at end of file diff --git a/src/sensors/softfusion/drivers/icm42688.h b/src/sensors/softfusion/drivers/icm42688.h new file mode 100644 index 000000000..e071db55a --- /dev/null +++ b/src/sensors/softfusion/drivers/icm42688.h @@ -0,0 +1,164 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 Tailsy13 & 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 +#include +#include + +namespace SlimeVR::Sensors::SoftFusion::Drivers +{ + +// Driver uses acceleration range at 8g +// and gyroscope range at 1000dps +// Gyroscope ODR = 500Hz, accel ODR = 100Hz +// Timestamps reading not used, as they're useless (constant predefined increment) + +template +struct ICM42688 +{ + static constexpr uint8_t Address = 0x68; + static constexpr auto Name = "ICM-42688"; + static constexpr auto Type = ImuID::ICM42688; + + static constexpr float GyrTs=1.0/500.0; + static constexpr float AccTs=1.0/100.0; + + static constexpr float MagTs=1.0/100; + + static constexpr float GyroSensitivity = 32.8f; + static constexpr float AccelSensitivity = 4096.0f; + + I2CImpl i2c; + SlimeVR::Logging::Logger &logger; + ICM42688(I2CImpl i2c, SlimeVR::Logging::Logger &logger) + : i2c(i2c), logger(logger) {} + + struct Regs { + struct WhoAmI { + static constexpr uint8_t reg = 0x75; + static constexpr uint8_t value = 0x47; + }; + static constexpr uint8_t TempData = 0x1d; + + struct DeviceConfig { + static constexpr uint8_t reg = 0x11; + static constexpr uint8_t valueSwReset = 1; + }; + struct IntfConfig0 { + static constexpr uint8_t reg = 0x4c; + static constexpr uint8_t value = (0 << 4) | (0 << 5) | (0 << 6); //fifo count in LE, sensor data in LE, fifo size in bytes + }; + struct FifoConfig0 { + static constexpr uint8_t reg = 0x16; + static constexpr uint8_t value = (0b01 << 6); //stream to FIFO mode + }; + struct FifoConfig1 { + static constexpr uint8_t reg = 0x5f; + static constexpr uint8_t value = 0b1 | (0b1 << 1) | (0b0 << 2); //fifo accel en=1, gyro=1, temp=0 todo: fsync, hires + }; + struct GyroConfig { + static constexpr uint8_t reg = 0x4f; + static constexpr uint8_t value = (0b001 << 5) | 0b1111; //1000dps, odr=500Hz + }; + struct AccelConfig { + static constexpr uint8_t reg = 0x50; + static constexpr uint8_t value = (0b001 << 5) | 0b1000; //8g, odr = 100Hz + }; + struct PwrMgmt { + static constexpr uint8_t reg = 0x4e; + static constexpr uint8_t value = 0b11 | (0b11 << 2); //accel in low noise mode, gyro in low noise + }; + + // TODO: might be worth checking + //GYRO_CONFIG1 + //GYRO_ACCEL_CONFIG0 + //ACCEL_CONFIG1 + + static constexpr uint8_t FifoCount = 0x2e; + static constexpr uint8_t FifoData = 0x30; + }; + + #pragma pack(push, 1) + struct FifoEntryAligned { + union { + struct { + int16_t accel[3]; + int16_t gyro[3]; + uint8_t temp; + uint8_t timestamp[2]; // cannot do uint16_t because it's unaligned + } part; + uint8_t raw[15]; + }; + }; + #pragma pack(pop) + + static constexpr size_t FullFifoEntrySize = 16; + + bool initialize() + { + // perform initialization step + i2c.writeReg(Regs::DeviceConfig::reg, Regs::DeviceConfig::valueSwReset); + delay(20); + + i2c.writeReg(Regs::IntfConfig0::reg, Regs::IntfConfig0::value); + i2c.writeReg(Regs::GyroConfig::reg, Regs::GyroConfig::value); + i2c.writeReg(Regs::AccelConfig::reg, Regs::AccelConfig::value); + i2c.writeReg(Regs::FifoConfig0::reg, Regs::FifoConfig0::value); + i2c.writeReg(Regs::FifoConfig1::reg, Regs::FifoConfig1::value); + i2c.writeReg(Regs::PwrMgmt::reg, Regs::PwrMgmt::value); + delay(1); + + return true; + } + + float getDirectTemp() const + { + const auto value = static_cast(i2c.readReg16(Regs::TempData)); + float result = ((float)value / 132.48f) + 25.0f; + return result; + } + + template + void bulkRead(AccelCall &&processAccelSample, GyroCall &&processGyroSample) { + const auto fifo_bytes = i2c.readReg16(Regs::FifoCount); + + std::array read_buffer; // max 8 readings + const auto bytes_to_read = std::min(static_cast(read_buffer.size()), + static_cast(fifo_bytes)) / FullFifoEntrySize * FullFifoEntrySize; + i2c.readBytes(Regs::FifoData, bytes_to_read, read_buffer.data()); + for (auto i=0u; i +#include +#include + +namespace SlimeVR::Sensors::SoftFusion::Drivers +{ + +template +struct LSM6DSOutputHandler +{ + LSM6DSOutputHandler(I2CImpl i2c, SlimeVR::Logging::Logger &logger) + : i2c(i2c), logger(logger) + {} + + I2CImpl i2c; + SlimeVR::Logging::Logger &logger; + + template + float getDirectTemp() const + { + const auto value = static_cast(i2c.readReg16(Regs::OutTemp)); + float result = ((float)value / 256.0f) + 25.0f; + + return result; + } + + #pragma pack(push, 1) + struct FifoEntryAligned { + union { + int16_t xyz[3]; + uint8_t raw[6]; + }; + }; + #pragma pack(pop) + + static constexpr size_t FullFifoEntrySize = sizeof(FifoEntryAligned) + 1; + + template + void bulkRead(AccelCall &processAccelSample, GyroCall &processGyroSample, float GyrTs, float AccTs) { + constexpr auto FIFO_SAMPLES_MASK = 0x3ff; + constexpr auto FIFO_OVERRUN_LATCHED_MASK = 0x800; + + const auto fifo_status = i2c.readReg16(Regs::FifoStatus); + const auto available_axes = fifo_status & FIFO_SAMPLES_MASK; + const auto fifo_bytes = available_axes * FullFifoEntrySize; + if (fifo_status & FIFO_OVERRUN_LATCHED_MASK) { + // FIFO overrun is expected to happen during startup and calibration + logger.error("FIFO OVERRUN! This occuring during normal usage is an issue."); + } + + std::array read_buffer; // max 8 readings + const auto bytes_to_read = std::min(static_cast(read_buffer.size()), + static_cast(fifo_bytes)) / FullFifoEntrySize * FullFifoEntrySize; + i2c.readBytes(Regs::FifoData, bytes_to_read, read_buffer.data()); + for (auto i=0u; i> 3; + memcpy(entry.raw, &read_buffer[i+0x1], sizeof(FifoEntryAligned)); // skip fifo header + + switch (tag) { + case 0x01: // Gyro NC + processGyroSample(entry.xyz, GyrTs); + break; + case 0x02: // Accel NC + processAccelSample(entry.xyz, AccTs); + break; + } + } + } + + +}; + +} // namespace \ No newline at end of file diff --git a/src/sensors/softfusion/drivers/lsm6ds3trc.h b/src/sensors/softfusion/drivers/lsm6ds3trc.h new file mode 100644 index 000000000..785a5e325 --- /dev/null +++ b/src/sensors/softfusion/drivers/lsm6ds3trc.h @@ -0,0 +1,139 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 Tailsy13 & 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 +#include +#include + +namespace SlimeVR::Sensors::SoftFusion::Drivers +{ + +// Driver uses acceleration range at 8g +// and gyroscope range at 1000dps +// Gyroscope ODR = 416Hz, accel ODR = 416Hz + +template +struct LSM6DS3TRC +{ + static constexpr uint8_t Address = 0x6a; + static constexpr auto Name = "LSM6DS3TR-C"; + static constexpr auto Type = ImuID::LSM6DS3TRC; + + static constexpr float Freq = 416; + + static constexpr float GyrTs=1.0/Freq; + static constexpr float AccTs=1.0/Freq; + static constexpr float MagTs=1.0/Freq; + + static constexpr float GyroSensitivity = 28.571428571f; + static constexpr float AccelSensitivity = 4098.360655738f; + + I2CImpl i2c; + SlimeVR::Logging::Logger logger; + LSM6DS3TRC(I2CImpl i2c, SlimeVR::Logging::Logger &logger) + : i2c(i2c), logger(logger) {} + + struct Regs { + struct WhoAmI { + static constexpr uint8_t reg = 0x0f; + static constexpr uint8_t value = 0x6a; + }; + static constexpr uint8_t OutTemp = 0x20; + struct Ctrl1XL { + static constexpr uint8_t reg = 0x10; + static constexpr uint8_t value = (0b11 << 2) | (0b0110 << 4); //8g, 416Hz + }; + struct Ctrl2G { + static constexpr uint8_t reg = 0x11; + static constexpr uint8_t value = (0b10 << 2) | (0b0110 << 4); //1000dps, 416Hz + }; + struct Ctrl3C { + static constexpr uint8_t reg = 0x12; + static constexpr uint8_t valueSwReset = 1; + static constexpr uint8_t value = (1 << 6) | (1 << 2); //BDU = 1, IF_INC = 1 + }; + struct FifoCtrl3 { + static constexpr uint8_t reg = 0x08; + static constexpr uint8_t value = 0b001 | (0b001 << 3); //accel no decimation, gyro no decimation + }; + struct FifoCtrl5 { + static constexpr uint8_t reg = 0x0a; + static constexpr uint8_t value = 0b110 | (0b0111 << 3); //continuous mode, odr = 833Hz + }; + + static constexpr uint8_t FifoStatus = 0x3a; + static constexpr uint8_t FifoData = 0x3e; + }; + + bool initialize() + { + // perform initialization step + i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset); + delay(20); + i2c.writeReg(Regs::Ctrl1XL::reg, Regs::Ctrl1XL::value); + i2c.writeReg(Regs::Ctrl2G::reg, Regs::Ctrl2G::value); + i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value); + i2c.writeReg(Regs::FifoCtrl3::reg, Regs::FifoCtrl3::value); + i2c.writeReg(Regs::FifoCtrl5::reg, Regs::FifoCtrl5::value); + return true; + } + + float getDirectTemp() const + { + const auto value = static_cast(i2c.readReg16(Regs::OutTemp)); + float result = ((float)value / 256.0f) + 25.0f; + + return result; + } + + template + void bulkRead(AccelCall &&processAccelSample, GyroCall &&processGyroSample) { + const auto read_result = i2c.readReg16(Regs::FifoStatus); + if (read_result & 0x4000) { // overrun! + // disable and re-enable fifo to clear it + logger.debug("Fifo overrun, resetting..."); + i2c.writeReg(Regs::FifoCtrl5::reg, 0); + i2c.writeReg(Regs::FifoCtrl5::reg, Regs::FifoCtrl5::value); + return; + } + const auto unread_entries = read_result & 0x7ff; + constexpr auto single_measurement_words = 6; + constexpr auto single_measurement_bytes = sizeof(uint16_t) * single_measurement_words; + + std::array read_buffer; // max 10 packages of 6 16bit values of data form fifo + const auto bytes_to_read = std::min(static_cast(read_buffer.size()), static_cast(unread_entries)) \ + * sizeof(uint16_t) / single_measurement_bytes * single_measurement_bytes; + + i2c.readBytes(Regs::FifoData, bytes_to_read, reinterpret_cast(read_buffer.data())); + for (uint16_t i=0; i(&read_buffer[i]), GyrTs); + processAccelSample(reinterpret_cast(&read_buffer[i+3]), AccTs); + } + } + + +}; + +} // namespace \ No newline at end of file diff --git a/src/sensors/softfusion/drivers/lsm6dso.h b/src/sensors/softfusion/drivers/lsm6dso.h new file mode 100644 index 000000000..0dc4870f8 --- /dev/null +++ b/src/sensors/softfusion/drivers/lsm6dso.h @@ -0,0 +1,120 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 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 +#include +#include + +#include "lsm6ds-common.h" + +namespace SlimeVR::Sensors::SoftFusion::Drivers +{ + +// Driver uses acceleration range at 8g +// and gyroscope range at 1000dps +// Gyroscope ODR = 416Hz, accel ODR = 104Hz + +template +struct LSM6DSO : LSM6DSOutputHandler +{ + static constexpr uint8_t Address = 0x6a; + static constexpr auto Name = "LSM6DSO"; + static constexpr auto Type = ImuID::LSM6DSO; + + static constexpr float GyrFreq = 416; + static constexpr float AccFreq = 104; + static constexpr float MagFreq = 120; + + static constexpr float GyrTs=1.0/GyrFreq; + static constexpr float AccTs=1.0/AccFreq; + static constexpr float MagTs=1.0/MagFreq; + + static constexpr float GyroSensitivity = 1000 / 35.0f; + static constexpr float AccelSensitivity = 1000 / 0.244f; + + using LSM6DSOutputHandler::i2c; + + struct Regs { + struct WhoAmI { + static constexpr uint8_t reg = 0x0f; + static constexpr uint8_t value = 0x6c; + }; + static constexpr uint8_t OutTemp = 0x20; + struct Ctrl1XL { + static constexpr uint8_t reg = 0x10; + static constexpr uint8_t value = (0b01001100); // XL at 104 Hz, 8g FS + }; + struct Ctrl2GY { + static constexpr uint8_t reg = 0x11; + static constexpr uint8_t value = (0b01101000); //GY at 416 Hz, 1000dps FS + }; + struct Ctrl3C { + static constexpr uint8_t reg = 0x12; + static constexpr uint8_t valueSwReset = 1; + static constexpr uint8_t value = (1 << 6) | (1 << 2); //BDU = 1, IF_INC = 1 + }; + struct FifoCtrl3BDR { + static constexpr uint8_t reg = 0x09; + static constexpr uint8_t value = (0b0110) | (0b0110 << 4); //gyro and accel batched at 417Hz + }; + struct FifoCtrl4Mode { + static constexpr uint8_t reg = 0x0a; + static constexpr uint8_t value = (0b110); //continuous mode + }; + + static constexpr uint8_t FifoStatus = 0x3a; + static constexpr uint8_t FifoData = 0x78; + }; + + LSM6DSO(I2CImpl i2c, SlimeVR::Logging::Logger &logger) + : LSM6DSOutputHandler(i2c, logger) { + } + + bool initialize() + { + // perform initialization step + i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset); + delay(20); + i2c.writeReg(Regs::Ctrl1XL::reg, Regs::Ctrl1XL::value); + i2c.writeReg(Regs::Ctrl2GY::reg, Regs::Ctrl2GY::value); + i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value); + i2c.writeReg(Regs::FifoCtrl3BDR::reg, Regs::FifoCtrl3BDR::value); + i2c.writeReg(Regs::FifoCtrl4Mode::reg, Regs::FifoCtrl4Mode::value); + return true; + } + + float getDirectTemp() const + { + return LSM6DSOutputHandler::template getDirectTemp(); + } + + template + void bulkRead(AccelCall &&processAccelSample, GyroCall &&processGyroSample) { + LSM6DSOutputHandler::template bulkRead(processAccelSample, processGyroSample, GyrTs, AccTs); + } + +}; + +} // namespace \ No newline at end of file diff --git a/src/sensors/softfusion/drivers/lsm6dsr.h b/src/sensors/softfusion/drivers/lsm6dsr.h new file mode 100644 index 000000000..ebef63ca2 --- /dev/null +++ b/src/sensors/softfusion/drivers/lsm6dsr.h @@ -0,0 +1,120 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 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 +#include +#include + +#include "lsm6ds-common.h" + +namespace SlimeVR::Sensors::SoftFusion::Drivers +{ + +// Driver uses acceleration range at 8g +// and gyroscope range at 1000dps +// Gyroscope ODR = 416Hz, accel ODR = 104Hz + +template +struct LSM6DSR : LSM6DSOutputHandler +{ + static constexpr uint8_t Address = 0x6a; + static constexpr auto Name = "LSM6DSR"; + static constexpr auto Type = ImuID::LSM6DSR; + + static constexpr float GyrFreq = 416; + static constexpr float AccFreq = 104; + static constexpr float MagFreq = 120; + + static constexpr float GyrTs=1.0/GyrFreq; + static constexpr float AccTs=1.0/AccFreq; + static constexpr float MagTs=1.0/MagFreq; + + static constexpr float GyroSensitivity = 1000 / 35.0f; + static constexpr float AccelSensitivity = 1000 / 0.244f; + + using LSM6DSOutputHandler::i2c; + + struct Regs { + struct WhoAmI { + static constexpr uint8_t reg = 0x0f; + static constexpr uint8_t value = 0x6b; + }; + static constexpr uint8_t OutTemp = 0x20; + struct Ctrl1XL { + static constexpr uint8_t reg = 0x10; + static constexpr uint8_t value = (0b01001100); // XL at 104 Hz, 8g FS + }; + struct Ctrl2GY { + static constexpr uint8_t reg = 0x11; + static constexpr uint8_t value = (0b01101000); //GY at 416 Hz, 1000dps FS + }; + struct Ctrl3C { + static constexpr uint8_t reg = 0x12; + static constexpr uint8_t valueSwReset = 1; + static constexpr uint8_t value = (1 << 6) | (1 << 2); //BDU = 1, IF_INC = 1 + }; + struct FifoCtrl3BDR { + static constexpr uint8_t reg = 0x09; + static constexpr uint8_t value = (0b0110) | (0b0110 << 4); //gyro and accel batched at 417Hz + }; + struct FifoCtrl4Mode { + static constexpr uint8_t reg = 0x0a; + static constexpr uint8_t value = (0b110); //continuous mode + }; + + static constexpr uint8_t FifoStatus = 0x3a; + static constexpr uint8_t FifoData = 0x78; + }; + + LSM6DSR(I2CImpl i2c, SlimeVR::Logging::Logger &logger) + : LSM6DSOutputHandler(i2c, logger) { + } + + bool initialize() + { + // perform initialization step + i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset); + delay(20); + i2c.writeReg(Regs::Ctrl1XL::reg, Regs::Ctrl1XL::value); + i2c.writeReg(Regs::Ctrl2GY::reg, Regs::Ctrl2GY::value); + i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value); + i2c.writeReg(Regs::FifoCtrl3BDR::reg, Regs::FifoCtrl3BDR::value); + i2c.writeReg(Regs::FifoCtrl4Mode::reg, Regs::FifoCtrl4Mode::value); + return true; + } + + float getDirectTemp() const + { + return LSM6DSOutputHandler::template getDirectTemp(); + } + + template + void bulkRead(AccelCall &&processAccelSample, GyroCall &&processGyroSample) { + LSM6DSOutputHandler::template bulkRead(processAccelSample, processGyroSample, GyrTs, AccTs); + } + +}; + +} // namespace \ No newline at end of file diff --git a/src/sensors/softfusion/drivers/lsm6dsv.h b/src/sensors/softfusion/drivers/lsm6dsv.h new file mode 100644 index 000000000..ef073ecf6 --- /dev/null +++ b/src/sensors/softfusion/drivers/lsm6dsv.h @@ -0,0 +1,135 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 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 +#include +#include + +#include "lsm6ds-common.h" + +namespace SlimeVR::Sensors::SoftFusion::Drivers +{ + +// Driver uses acceleration range at 8g +// and gyroscope range at 1000dps +// Gyroscope ODR = 480Hz, accel ODR = 120Hz + +template +struct LSM6DSV : LSM6DSOutputHandler +{ + static constexpr uint8_t Address = 0x6a; + static constexpr auto Name = "LSM6DSV"; + static constexpr auto Type = ImuID::LSM6DSV; + + static constexpr float GyrFreq = 480; + static constexpr float AccFreq = 120; + static constexpr float MagFreq = 120; + + static constexpr float GyrTs=1.0/GyrFreq; + static constexpr float AccTs=1.0/AccFreq; + static constexpr float MagTs=1.0/MagFreq; + + static constexpr float GyroSensitivity = 1000 / 35.0f; + static constexpr float AccelSensitivity = 1000 / 0.244f; + + using LSM6DSOutputHandler::i2c; + + struct Regs { + struct WhoAmI { + static constexpr uint8_t reg = 0x0f; + static constexpr uint8_t value = 0x70; + }; + static constexpr uint8_t OutTemp = 0x20; + struct HAODRCFG { + static constexpr uint8_t reg = 0x62; + static constexpr uint8_t value = (0b00); //1st ODR table + }; + struct Ctrl1XLODR { + static constexpr uint8_t reg = 0x10; + static constexpr uint8_t value = (0b0010110); //120Hz, HAODR + }; + struct Ctrl2GODR { + static constexpr uint8_t reg = 0x11; + static constexpr uint8_t value = (0b0011000); //480Hz, HAODR + }; + struct Ctrl3C { + static constexpr uint8_t reg = 0x12; + static constexpr uint8_t valueSwReset = 1; + static constexpr uint8_t value = (1 << 6) | (1 << 2); //BDU = 1, IF_INC = 1 + }; + struct Ctrl6GFS { + static constexpr uint8_t reg = 0x15; + static constexpr uint8_t value = (0b0011); //1000dps + }; + struct Ctrl8XLFS { + static constexpr uint8_t reg = 0x17; + static constexpr uint8_t value = (0b10); //8g + }; + struct FifoCtrl3BDR { + static constexpr uint8_t reg = 0x09; + static constexpr uint8_t value = (0b1000) | (0b1000 << 4); //gyro and accel batched at 480Hz + }; + struct FifoCtrl4Mode { + static constexpr uint8_t reg = 0x0a; + static constexpr uint8_t value = (0b110); //continuous mode + }; + + static constexpr uint8_t FifoStatus = 0x1b; + static constexpr uint8_t FifoData = 0x78; + }; + + LSM6DSV(I2CImpl i2c, SlimeVR::Logging::Logger &logger) + : LSM6DSOutputHandler(i2c, logger) { + } + + bool initialize() + { + // perform initialization step + i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset); + delay(20); + i2c.writeReg(Regs::HAODRCFG::reg, Regs::HAODRCFG::value); + i2c.writeReg(Regs::Ctrl1XLODR::reg, Regs::Ctrl1XLODR::value); + i2c.writeReg(Regs::Ctrl2GODR::reg, Regs::Ctrl2GODR::value); + i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value); + i2c.writeReg(Regs::Ctrl6GFS::reg, Regs::Ctrl6GFS::value); + i2c.writeReg(Regs::Ctrl8XLFS::reg, Regs::Ctrl8XLFS::value); + i2c.writeReg(Regs::FifoCtrl3BDR::reg, Regs::FifoCtrl3BDR::value); + i2c.writeReg(Regs::FifoCtrl4Mode::reg, Regs::FifoCtrl4Mode::value); + return true; + } + + float getDirectTemp() const + { + return LSM6DSOutputHandler::template getDirectTemp(); + } + + template + void bulkRead(AccelCall &&processAccelSample, GyroCall &&processGyroSample) { + LSM6DSOutputHandler::template bulkRead(processAccelSample, processGyroSample, GyrTs, AccTs); + } + +}; + +} // namespace \ No newline at end of file diff --git a/src/sensors/softfusion/drivers/mpu6050.h b/src/sensors/softfusion/drivers/mpu6050.h new file mode 100644 index 000000000..6546784c8 --- /dev/null +++ b/src/sensors/softfusion/drivers/mpu6050.h @@ -0,0 +1,185 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 furrycoding & 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 +#include +#include + +#include + + +namespace SlimeVR::Sensors::SoftFusion::Drivers +{ + +// Driver uses acceleration range at 8g +// and gyroscope range at 1000dps +// Gyroscope ODR = accel ODR = 250Hz + +template +struct MPU6050 +{ + struct FifoSample { + uint8_t accel_x_h, accel_x_l; + uint8_t accel_y_h, accel_y_l; + uint8_t accel_z_h, accel_z_l; + + // We don't need temperature in FIFO + // uint8_t temp_h, temp_l; + + uint8_t gyro_x_h, gyro_x_l; + uint8_t gyro_y_h, gyro_y_l; + uint8_t gyro_z_h, gyro_z_l; + }; + #define MPU6050_FIFO_VALUE(fifo, name) (((int16_t)fifo->name##_h << 8) | ((int16_t)fifo->name##_l)) + + static constexpr uint8_t Address = 0x68; + static constexpr auto Name = "MPU-6050"; + static constexpr auto Type = ImuID::MPU6050; + + static constexpr float Freq = 250; + + static constexpr float GyrTs = 1.0 / Freq; + static constexpr float AccTs = 1.0 / Freq; + static constexpr float MagTs = 1.0 / Freq; + + static constexpr float GyroSensitivity = 32.8f; + static constexpr float AccelSensitivity = 4096.0f; + + I2CImpl i2c; + SlimeVR::Logging::Logger &logger; + MPU6050(I2CImpl i2c, SlimeVR::Logging::Logger &logger) + : i2c(i2c), logger(logger) {} + + struct Regs { + struct WhoAmI { + static constexpr uint8_t reg = 0x75; + static constexpr uint8_t value = 0x68; + }; + + struct UserCtrl { + static constexpr uint8_t reg = 0x6A; + static constexpr uint8_t fifoResetValue = (1 << MPU6050_USERCTRL_FIFO_EN_BIT) | (1 << MPU6050_USERCTRL_FIFO_RESET_BIT); + }; + + struct GyroConfig { + static constexpr uint8_t reg = 0x1b; + static constexpr uint8_t value = 0b10 << 3; // 1000dps + }; + + struct AccelConfig { + static constexpr uint8_t reg = 0x1c; + static constexpr uint8_t value = 0b10 << 3; // 8g + }; + + static constexpr uint8_t OutTemp = MPU6050_RA_TEMP_OUT_H; + + static constexpr uint8_t IntStatus = MPU6050_RA_INT_STATUS; + + static constexpr uint8_t FifoCount = MPU6050_RA_FIFO_COUNTH; + static constexpr uint8_t FifoData = MPU6050_RA_FIFO_R_W; + }; + + inline uint16_t byteSwap(uint16_t value) const { + // Swap bytes because MPU is big-endian + return (value >> 8) | (value << 8); + } + + void resetFIFO() { + i2c.writeReg(Regs::UserCtrl::reg, Regs::UserCtrl::fifoResetValue); + } + + bool initialize() + { + // Reset + i2c.writeReg(MPU6050_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1: reset with 100ms delay (also disables sleep) + delay(100); + i2c.writeReg(MPU6050_RA_SIGNAL_PATH_RESET, 0x07); // full SIGNAL_PATH_RESET: with another 100ms delay + delay(100); + + // Configure + i2c.writeReg(MPU6050_RA_PWR_MGMT_1, 0x01); // 0000 0001 PWR_MGMT_1:Clock Source Select PLL_X_gyro + i2c.writeReg(MPU6050_RA_USER_CTRL, 0x00); // 0000 0000 USER_CTRL: Disable FIFO / I2C master / DMP + i2c.writeReg(MPU6050_RA_INT_ENABLE, 0x10); // 0001 0000 INT_ENABLE: only FIFO overflow interrupt + i2c.writeReg(Regs::GyroConfig::reg, Regs::GyroConfig::value); + i2c.writeReg(Regs::AccelConfig::reg, Regs::AccelConfig::value); + i2c.writeReg(MPU6050_RA_CONFIG, 0x02); // 0000 0010 CONFIG: No EXT_SYNC_SET, DLPF set to 98Hz(also lowers gyro output rate to 1KHz) + i2c.writeReg(MPU6050_RA_SMPLRT_DIV, 0x03); // 0000 0011 SMPLRT_DIV: Divides the internal sample rate 250Hz (Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)) + + i2c.writeReg(MPU6050_RA_FIFO_EN, 0x78); // 0111 1000 FIFO_EN: All gyro axes + Accel + + resetFIFO(); + + return true; + } + + float getDirectTemp() const + { + auto value = byteSwap(i2c.readReg16(Regs::OutTemp)); + float result = (static_cast(value) / 340.0f) + 36.53f; + return result; + } + + template + void bulkRead(AccelCall &&processAccelSample, GyroCall &&processGyroSample) { + const auto status = i2c.readReg(Regs::IntStatus); + + if (status & (1 << MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) { + // Overflows make it so we lose track of which packet is which + // This necessitates a reset + logger.debug("Fifo overrun, resetting..."); + resetFIFO(); + return; + } + + std::array readBuffer; // max 10 packages of 12byte values (sample) of data form fifo + auto byteCount = byteSwap(i2c.readReg16(Regs::FifoCount)); + + auto readBytes = min(static_cast(byteCount), readBuffer.size()) / sizeof(FifoSample) * sizeof(FifoSample); + if (!readBytes) { + return; + } + + i2c.readBytes(Regs::FifoData, readBytes, readBuffer.data()); + for (auto i = 0u; i < readBytes; i += sizeof(FifoSample)) { + const FifoSample *sample = reinterpret_cast(&readBuffer[i]); + + int16_t xyz[3]; + + xyz[0] = MPU6050_FIFO_VALUE(sample, accel_x); + xyz[1] = MPU6050_FIFO_VALUE(sample, accel_y); + xyz[2] = MPU6050_FIFO_VALUE(sample, accel_z); + processAccelSample(xyz, AccTs); + + xyz[0] = MPU6050_FIFO_VALUE(sample, gyro_x); + xyz[1] = MPU6050_FIFO_VALUE(sample, gyro_y); + xyz[2] = MPU6050_FIFO_VALUE(sample, gyro_z); + processGyroSample(xyz, GyrTs); + } + } + + +}; + +} // namespace diff --git a/src/sensors/softfusion/i2cimpl.h b/src/sensors/softfusion/i2cimpl.h new file mode 100644 index 000000000..7bc892301 --- /dev/null +++ b/src/sensors/softfusion/i2cimpl.h @@ -0,0 +1,72 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 Tailsy13 & 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 +#include "I2Cdev.h" + + +namespace SlimeVR::Sensors::SoftFusion +{ + +struct I2CImpl +{ + static constexpr size_t MaxTransactionLength = I2C_BUFFER_LENGTH - 2; + + I2CImpl(uint8_t devAddr) + : m_devAddr(devAddr) {} + + uint8_t readReg(uint8_t regAddr) const { + uint8_t buffer = 0; + I2Cdev::readByte(m_devAddr, regAddr, &buffer); + return buffer; + } + + uint16_t readReg16(uint8_t regAddr) const { + uint16_t buffer = 0; + I2Cdev::readBytes(m_devAddr, regAddr, sizeof(buffer), reinterpret_cast(&buffer)); + return buffer; + } + + void writeReg(uint8_t regAddr, uint8_t value) const { + I2Cdev::writeByte(m_devAddr, regAddr, value); + } + + void writeReg16(uint8_t regAddr, uint16_t value) const { + I2Cdev::writeBytes(m_devAddr, regAddr, sizeof(value), reinterpret_cast(&value)); + } + + void readBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const { + I2Cdev::readBytes(m_devAddr, regAddr, size, buffer); + } + + void writeBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const { + I2Cdev::writeBytes(m_devAddr, regAddr, size, buffer); + } + + private: + uint8_t m_devAddr; +}; + +} diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h new file mode 100644 index 000000000..ec076a757 --- /dev/null +++ b/src/sensors/softfusion/softfusionsensor.h @@ -0,0 +1,531 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 Tailsy13 & 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 "../sensor.h" +#include "../SensorFusionRestDetect.h" + +#include "GlobalVars.h" + +namespace SlimeVR::Sensors +{ + +template typename T, typename I2CImpl> +class SoftFusionSensor : public Sensor +{ + using imu = T; + using RawVectorT = std::array; + static constexpr auto UpsideDownCalibrationInit = true; + static constexpr auto GyroCalibDelaySeconds = 5; + static constexpr auto GyroCalibSeconds = 5; + static constexpr auto SampleRateCalibDelaySeconds = 1; + static constexpr auto SampleRateCalibSeconds = 5; + + static constexpr auto AccelCalibDelaySeconds = 3; + static constexpr auto AccelCalibRestSeconds = 3; + + static constexpr double GScale = ((32768. / imu::GyroSensitivity) / 32768.) * (PI / 180.0); + static constexpr double AScale = CONST_EARTH_GRAVITY / imu::AccelSensitivity; + + static constexpr bool HasMotionlessCalib = requires(imu& i){ typename imu::MotionlessCalibrationData; }; + static constexpr size_t MotionlessCalibDataSize() { + if constexpr(HasMotionlessCalib) { + return sizeof(typename imu::MotionlessCalibrationData); + } + else { + return 0; + } + } + + bool detected() const + { + const auto value = m_sensor.i2c.readReg(imu::Regs::WhoAmI::reg); + if (imu::Regs::WhoAmI::value != value) { + m_Logger.error("Sensor not detected, expected reg 0x%02x = 0x%02x but got 0x%02x", + imu::Regs::WhoAmI::reg, imu::Regs::WhoAmI::value, value); + return false; + } + + return true; + } + + + void sendTempIfNeeded() + { + uint32_t now = micros(); + constexpr float maxSendRateHz = 2.0f; + constexpr uint32_t sendInterval = 1.0f/maxSendRateHz * 1e6; + uint32_t elapsed = now - m_lastTemperaturePacketSent; + if (elapsed >= sendInterval) { + const float temperature = m_sensor.getDirectTemp(); + m_lastTemperaturePacketSent = now - (elapsed - sendInterval); + networkConnection.sendTemperature(sensorId, temperature); + } + } + + void recalcFusion() + { + m_fusion = SensorFusionRestDetect(m_calibration.G_Ts, m_calibration.A_Ts, m_calibration.M_Ts); + } + + void processAccelSample(const int16_t xyz[3], const sensor_real_t timeDelta) + { + sensor_real_t accelData[] = { + static_cast(xyz[0]), + static_cast(xyz[1]), + static_cast(xyz[2]) }; + + float tmp[3]; + for (uint8_t i = 0; i < 3; i++) + tmp[i] = (accelData[i] - m_calibration.A_B[i]); + + accelData[0] = (m_calibration.A_Ainv[0][0] * tmp[0] + m_calibration.A_Ainv[0][1] * tmp[1] + m_calibration.A_Ainv[0][2] * tmp[2]) * AScale; + accelData[1] = (m_calibration.A_Ainv[1][0] * tmp[0] + m_calibration.A_Ainv[1][1] * tmp[1] + m_calibration.A_Ainv[1][2] * tmp[2]) * AScale; + accelData[2] = (m_calibration.A_Ainv[2][0] * tmp[0] + m_calibration.A_Ainv[2][1] * tmp[1] + m_calibration.A_Ainv[2][2] * tmp[2]) * AScale; + + m_fusion.updateAcc(accelData, m_calibration.A_Ts); + } + + void processGyroSample(const int16_t xyz[3], const sensor_real_t timeDelta) + { + const sensor_real_t scaledData[] = { + static_cast(GScale * (static_cast(xyz[0]) - m_calibration.G_off[0])), + static_cast(GScale * (static_cast(xyz[1]) - m_calibration.G_off[1])), + static_cast(GScale * (static_cast(xyz[2]) - m_calibration.G_off[2]))}; + m_fusion.updateGyro(scaledData, m_calibration.G_Ts); + } + + void eatSamplesForSeconds(const uint32_t seconds) { + const auto targetDelay = millis() + 1000 * seconds; + auto lastSecondsRemaining = seconds; + while (millis() < targetDelay) + { + #ifdef ESP8266 + ESP.wdtFeed(); + #endif + auto currentSecondsRemaining = (targetDelay - millis()) / 1000; + if (currentSecondsRemaining != lastSecondsRemaining) { + m_Logger.info("%d...", currentSecondsRemaining + 1); + lastSecondsRemaining = currentSecondsRemaining; + } + m_sensor.bulkRead( + [](const int16_t xyz[3], const sensor_real_t timeDelta) { }, + [](const int16_t xyz[3], const sensor_real_t timeDelta) { } + ); + } + } + + std::pair eatSamplesReturnLast(const uint32_t milliseconds) + { + RawVectorT accel = {0}; + RawVectorT gyro = {0}; + const auto targetDelay = millis() + milliseconds; + while (millis() < targetDelay) + { + m_sensor.bulkRead( + [&](const int16_t xyz[3], const sensor_real_t timeDelta) { + accel[0] = xyz[0]; + accel[1] = xyz[1]; + accel[2] = xyz[2]; + }, + [&](const int16_t xyz[3], const sensor_real_t timeDelta) { + gyro[0] = xyz[0]; + gyro[1] = xyz[1]; + gyro[2] = xyz[2]; + } + ); + } + return std::make_pair(accel, gyro); + } + +public: + static constexpr auto TypeID = imu::Type; + static constexpr uint8_t Address = imu::Address; + + SoftFusionSensor(uint8_t id, uint8_t addrSuppl, float rotation, uint8_t sclPin, uint8_t sdaPin, uint8_t) + : Sensor(imu::Name, imu::Type, id, imu::Address + addrSuppl, rotation, sclPin, sdaPin), + m_fusion(imu::GyrTs, imu::AccTs, imu::MagTs), m_sensor(I2CImpl(imu::Address + addrSuppl), m_Logger) {} + ~SoftFusionSensor(){} + + void motionLoop() override final + { + sendTempIfNeeded(); + + // read fifo updating fusion + uint32_t now = micros(); + constexpr uint32_t targetPollIntervalMicros = 6000; + uint32_t elapsed = now - m_lastPollTime; + if (elapsed >= targetPollIntervalMicros) { + m_lastPollTime = now - (elapsed - targetPollIntervalMicros); + m_sensor.bulkRead( + [&](const int16_t xyz[3], const sensor_real_t timeDelta) { processAccelSample(xyz, timeDelta); }, + [&](const int16_t xyz[3], const sensor_real_t timeDelta) { processGyroSample(xyz, timeDelta); } + ); + optimistic_yield(100); + if (!m_fusion.isUpdated()) return; + hadData = true; + m_fusion.clearUpdated(); + } + + + // send new fusion values when time is up + now = micros(); + constexpr float maxSendRateHz = 120.0f; + constexpr uint32_t sendInterval = 1.0f/maxSendRateHz * 1e6; + elapsed = now - m_lastRotationPacketSent; + if (elapsed >= sendInterval) { + m_lastRotationPacketSent = now - (elapsed - sendInterval); + + setFusedRotation(m_fusion.getQuaternionQuat()); + setAcceleration(m_fusion.getLinearAccVec()); + optimistic_yield(100); + } + } + + void motionSetup() override final + { + if (!detected()) { + m_status = SensorStatus::SENSOR_ERROR; + return; + } + + SlimeVR::Configuration::CalibrationConfig sensorCalibration = configuration.getCalibration(sensorId); + + // If no compatible calibration data is found, the calibration data will just be zero-ed out + if (sensorCalibration.type == SlimeVR::Configuration::CalibrationConfigType::SFUSION + && (sensorCalibration.data.sfusion.ImuType == imu::Type) + && (sensorCalibration.data.sfusion.MotionlessDataLen == MotionlessCalibDataSize())) { + m_calibration = sensorCalibration.data.sfusion; + recalcFusion(); + } + else if (sensorCalibration.type == SlimeVR::Configuration::CalibrationConfigType::NONE) { + m_Logger.warn("No calibration data found for sensor %d, ignoring...", sensorId); + m_Logger.info("Calibration is advised"); + } + else { + m_Logger.warn("Incompatible calibration data found for sensor %d, ignoring...", sensorId); + m_Logger.info("Please recalibrate"); + } + + bool initResult = false; + + if constexpr(HasMotionlessCalib) { + typename imu::MotionlessCalibrationData calibData; + std::memcpy(&calibData, m_calibration.MotionlessData, sizeof(calibData)); + initResult = m_sensor.initialize(calibData); + } + else { + initResult = m_sensor.initialize(); + } + + if (!initResult) { + m_Logger.error("Sensor failed to initialize!"); + m_status = SensorStatus::SENSOR_ERROR; + return; + } + + m_status = SensorStatus::SENSOR_OK; + working = true; + [[maybe_unused]] auto lastRawSample = eatSamplesReturnLast(1000); + if constexpr(UpsideDownCalibrationInit) { + auto gravity = static_cast(AScale * static_cast(lastRawSample.first[2])); + m_Logger.info("Gravity read: %.1f (need < -7.5 to start calibration)", gravity); + if (gravity < -7.5f) { + ledManager.on(); + m_Logger.info("Flip front in 5 seconds to start calibration"); + lastRawSample = eatSamplesReturnLast(5000); + gravity = static_cast(AScale * static_cast(lastRawSample.first[2])); + if (gravity > 7.5f) { + m_Logger.debug("Starting calibration..."); + startCalibration(0); + } + else { + m_Logger.info("Flip not detected. Skipping calibration."); + } + + ledManager.off(); + } + } + } + + void startCalibration(int calibrationType) override final + { + if (calibrationType == 0) { + // ALL + calibrateSampleRate(); + calibrateGyroOffset(); + if constexpr(HasMotionlessCalib) { + typename imu::MotionlessCalibrationData calibData; + m_sensor.motionlessCalibration(calibData); + std::memcpy(m_calibration.MotionlessData, &calibData, sizeof(calibData)); + } + calibrateAccel(); + } + else if (calibrationType == 1) + { + calibrateSampleRate(); + } + else if (calibrationType == 2) + { + calibrateGyroOffset(); + } + else if (calibrationType == 3) + { + calibrateAccel(); + } + else if (calibrationType == 4) { + if constexpr(HasMotionlessCalib) { + typename imu::MotionlessCalibrationData calibData; + m_sensor.motionlessCalibration(calibData); + std::memcpy(m_calibration.MotionlessData, &calibData, sizeof(calibData)); + } else { + m_Logger.info("Sensor doesn't provide any custom motionless calibration"); + } + } + + saveCalibration(); + } + + void saveCalibration() + { + m_Logger.debug("Saving the calibration data"); + SlimeVR::Configuration::CalibrationConfig calibration; + calibration.type = SlimeVR::Configuration::CalibrationConfigType::SFUSION; + calibration.data.sfusion = m_calibration; + configuration.setCalibration(sensorId, calibration); + configuration.save(); + } + + void calibrateGyroOffset() + { + // Wait for sensor to calm down before calibration + m_Logger.info("Put down the device and wait for baseline gyro reading calibration (%d seconds)", GyroCalibDelaySeconds); + ledManager.on(); + eatSamplesForSeconds(GyroCalibDelaySeconds); + ledManager.off(); + + m_calibration.temperature = m_sensor.getDirectTemp(); + m_Logger.trace("Calibration temperature: %f", m_calibration.temperature); + + ledManager.pattern(100, 100, 3); + ledManager.on(); + m_Logger.info("Gyro calibration started..."); + + int32_t sumXYZ[3] = {0}; + const auto targetCalib = millis() + 1000 * GyroCalibSeconds; + uint32_t sampleCount = 0; + + while (millis() < targetCalib) + { + #ifdef ESP8266 + ESP.wdtFeed(); + #endif + m_sensor.bulkRead( + [](const int16_t xyz[3], const sensor_real_t timeDelta) { }, + [&sumXYZ, &sampleCount](const int16_t xyz[3], const sensor_real_t timeDelta) { + sumXYZ[0] += xyz[0]; + sumXYZ[1] += xyz[1]; + sumXYZ[2] += xyz[2]; + ++sampleCount; + } + ); + } + + ledManager.off(); + m_calibration.G_off[0] = ((double)sumXYZ[0]) / sampleCount; + m_calibration.G_off[1] = ((double)sumXYZ[1]) / sampleCount; + m_calibration.G_off[2] = ((double)sumXYZ[2]) / sampleCount; + + m_Logger.info("Gyro offset after %d samples: %f %f %f", sampleCount, UNPACK_VECTOR_ARRAY(m_calibration.G_off)); + } + + void calibrateAccel() + { + auto magneto = std::make_unique(); + m_Logger.info("Put the device into 6 unique orientations (all sides), leave it still and do not hold/touch for %d seconds each", AccelCalibRestSeconds); + ledManager.on(); + eatSamplesForSeconds(AccelCalibDelaySeconds); + ledManager.off(); + + RestDetectionParams calibrationRestDetectionParams; + calibrationRestDetectionParams.restMinTime = AccelCalibRestSeconds; + calibrationRestDetectionParams.restThAcc = 0.25f; + + RestDetection calibrationRestDetection( + calibrationRestDetectionParams, + imu::GyrTs, + imu::AccTs + ); + + constexpr uint16_t expectedPositions = 6; + constexpr uint16_t numSamplesPerPosition = 96; + + uint16_t numPositionsRecorded = 0; + uint16_t numCurrentPositionSamples = 0; + bool waitForMotion = true; + + auto accelCalibrationChunk = std::make_unique(numSamplesPerPosition * 3); + ledManager.pattern(100, 100, 6); + ledManager.on(); + m_Logger.info("Gathering accelerometer data..."); + m_Logger.info("Waiting for position %i, you can leave the device as is...", numPositionsRecorded + 1); + bool samplesGathered = false; + while (!samplesGathered) { + #ifdef ESP8266 + ESP.wdtFeed(); + #endif + m_sensor.bulkRead( + [&](const int16_t xyz[3], const sensor_real_t timeDelta) { + const sensor_real_t scaledData[] = { + static_cast(AScale * static_cast(xyz[0])), + static_cast(AScale * static_cast(xyz[1])), + static_cast(AScale * static_cast(xyz[2]))}; + + calibrationRestDetection.updateAcc(imu::AccTs, scaledData); + if (waitForMotion) { + if (!calibrationRestDetection.getRestDetected()) { + waitForMotion = false; + } + return; + } + + if (calibrationRestDetection.getRestDetected()) { + const uint16_t i = numCurrentPositionSamples * 3; + accelCalibrationChunk[i + 0] = xyz[0]; + accelCalibrationChunk[i + 1] = xyz[1]; + accelCalibrationChunk[i + 2] = xyz[2]; + numCurrentPositionSamples++; + + if (numCurrentPositionSamples >= numSamplesPerPosition) { + for (int i = 0; i < numSamplesPerPosition; i++) { + magneto->sample( + accelCalibrationChunk[i * 3 + 0], + accelCalibrationChunk[i * 3 + 1], + accelCalibrationChunk[i * 3 + 2] + ); + } + numPositionsRecorded++; + numCurrentPositionSamples = 0; + if (numPositionsRecorded < expectedPositions) { + ledManager.pattern(50, 50, 2); + ledManager.on(); + m_Logger.info("Recorded, waiting for position %i...", numPositionsRecorded + 1); + waitForMotion = true; + } + } + } else { + numCurrentPositionSamples = 0; + } + + if (numPositionsRecorded >= expectedPositions) + { + samplesGathered = true; + } + }, + [](const int16_t xyz[3], const sensor_real_t timeDelta) { } + ); + } + ledManager.off(); + m_Logger.debug("Calculating accelerometer calibration data..."); + accelCalibrationChunk.reset(); + + float A_BAinv[4][3]; + magneto->current_calibration(A_BAinv); + + m_Logger.debug("Finished calculating accelerometer calibration"); + m_Logger.debug("Accelerometer calibration matrix:"); + m_Logger.debug("{"); + for (int i = 0; i < 3; i++) { + m_calibration.A_B[i] = A_BAinv[0][i]; + m_calibration.A_Ainv[0][i] = A_BAinv[1][i]; + m_calibration.A_Ainv[1][i] = A_BAinv[2][i]; + m_calibration.A_Ainv[2][i] = A_BAinv[3][i]; + m_Logger.debug(" %f, %f, %f, %f", A_BAinv[0][i], A_BAinv[1][i], A_BAinv[2][i], A_BAinv[3][i]); + } + m_Logger.debug("}"); + } + + void calibrateSampleRate() + { + m_Logger.debug("Calibrating IMU sample rate in %d second(s)...", SampleRateCalibDelaySeconds); + ledManager.on(); + eatSamplesForSeconds(SampleRateCalibDelaySeconds); + + uint32_t accelSamples = 0; + uint32_t gyroSamples = 0; + + const auto calibTarget = millis() + 1000 * SampleRateCalibSeconds; + m_Logger.debug("Counting samples now..."); + uint32_t currentTime; + while ((currentTime = millis()) < calibTarget) + { + m_sensor.bulkRead( + [&accelSamples](const int16_t xyz[3], const sensor_real_t timeDelta) { accelSamples++; }, + [&gyroSamples](const int16_t xyz[3], const sensor_real_t timeDelta) { gyroSamples++; } + ); + } + + const auto millisFromStart = currentTime - (calibTarget - 1000 * SampleRateCalibSeconds); + m_Logger.debug("Collected %d gyro, %d acc samples during %d ms", gyroSamples, accelSamples, millisFromStart); + m_calibration.A_Ts = millisFromStart / (accelSamples * 1000.0); + m_calibration.G_Ts = millisFromStart / (gyroSamples * 1000.0) ; + + m_Logger.debug("Gyro frequency %fHz, accel frequency: %fHz", 1.0/m_calibration.G_Ts, 1.0/m_calibration.A_Ts); + ledManager.off(); + + //fusion needs to be recalculated + recalcFusion(); + } + + SensorStatus getSensorState() override final + { + return m_status; + } + + SensorFusionRestDetect m_fusion; + T m_sensor; + SlimeVR::Configuration::SoftFusionCalibrationConfig m_calibration = { + // let's create here transparent calibration that doesn't affect input data + .ImuType = {imu::Type}, + .MotionlessDataLen = {MotionlessCalibDataSize()}, + .A_B = {0.0, 0.0, 0.0}, + .A_Ainv = {{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}}, + .M_B = {0.0, 0.0, 0.0}, + .M_Ainv = {{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}}, + .G_off = { 0.0, 0.0, 0.0 }, + .temperature = 0.0, + .A_Ts = imu::AccTs, + .G_Ts = imu::GyrTs, + .M_Ts = imu::MagTs, + .G_Sens = {1.0, 1.0, 1.0}, + .MotionlessData = {} + }; + + SensorStatus m_status = SensorStatus::SENSOR_OFFLINE; + uint32_t m_lastPollTime = micros(); + uint32_t m_lastRotationPacketSent = 0; + uint32_t m_lastTemperaturePacketSent = 0; +}; + +} // namespace diff --git a/src/serial/serialcommands.cpp b/src/serial/serialcommands.cpp index a85469502..72934b42f 100644 --- a/src/serial/serialcommands.cpp +++ b/src/serial/serialcommands.cpp @@ -134,14 +134,14 @@ namespace SerialCommands { statusManager.getStatus(), WiFiNetwork::getWiFiState() ); - for (auto sensor : sensorManager.getSensors()) { + for (auto &sensor : sensorManager.getSensors()) { logger.info( "Sensor[%d]: %s (%.3f %.3f %.3f %.3f) is working: %s, had data: %s", sensor->getSensorId(), getIMUNameByType(sensor->getSensorType()), UNPACK_QUATERNION(sensor->getFusedRotation()), sensor->isWorking() ? "true" : "false", - sensor->hadData ? "true" : "false" + sensor->getHadData() ? "true" : "false" ); } } @@ -180,8 +180,8 @@ namespace SerialCommands { Serial.printf( str.c_str(), BOARD, - IMU, - SECOND_IMU, + static_cast(sensorManager.getSensorType(0)), + static_cast(sensorManager.getSensorType(1)), IMU_ROTATION, SECOND_IMU_ROTATION, BATTERY_MONITOR, @@ -210,16 +210,16 @@ namespace SerialCommands { statusManager.getStatus(), WiFiNetwork::getWiFiState() ); - Sensor* sensor0 = sensorManager.getSensors()[0]; + auto& sensor0 = sensorManager.getSensors()[0]; sensor0->motionLoop(); logger.info( "[TEST] Sensor[0]: %s (%.3f %.3f %.3f %.3f) is working: %s, had data: %s", getIMUNameByType(sensor0->getSensorType()), UNPACK_QUATERNION(sensor0->getFusedRotation()), sensor0->isWorking() ? "true" : "false", - sensor0->hadData ? "true" : "false" + sensor0->getHadData() ? "true" : "false" ); - if(!sensor0->hadData) { + if(!sensor0->getHadData()) { logger.error("[TEST] Sensor[0] didn't send any data yet!"); } else { logger.info("[TEST] Sensor[0] sent some data, looks working."); @@ -293,22 +293,22 @@ namespace SerialCommands { void cmdTemperatureCalibration(CmdParser* parser) { if (parser->getParamCount() > 1) { if (parser->equalCmdParam(1, "PRINT")) { - for (auto sensor : sensorManager.getSensors()) { + for (auto &sensor : sensorManager.getSensors()) { sensor->printTemperatureCalibrationState(); } return; } else if (parser->equalCmdParam(1, "DEBUG")) { - for (auto sensor : sensorManager.getSensors()) { + for (auto &sensor : sensorManager.getSensors()) { sensor->printDebugTemperatureCalibrationState(); } return; } else if (parser->equalCmdParam(1, "RESET")) { - for (auto sensor : sensorManager.getSensors()) { + for (auto &sensor : sensorManager.getSensors()) { sensor->resetTemperatureCalibrationState(); } return; } else if (parser->equalCmdParam(1, "SAVE")) { - for (auto sensor : sensorManager.getSensors()) { + for (auto &sensor : sensorManager.getSensors()) { sensor->saveTemperatureCalibration(); } return;