github/SlimeVR-Tracker-ESP
1
1
Fork 0
mirror of https://github.com/SlimeVR/SlimeVR-Tracker-ESP.git synced 2026-04-05 17:51:57 +02:00
Code Issues 76 Packages Projects Releases Wiki Activity

SoftFusion sensor framework with BMI, ICM, LSM6, MPU sensor implementations (#322)

Browse Source 
* Update readme to mention BMI270 support.

* Soft fusion sensor initial code, wip

* Soft fusion ICM-42688-P lazy WIP implementation.

* sfusion: Cleanup, implemented sensor frequency calibration

* icm42688: add more comments, basic driver (no hw filtering) should be working

* sfustion: compilation fix

* sfusion: start calibration when upside down

* cleanup: remove confusing had data flag

* sensor manager: use unique_ptr instead of raw pointers

* sfusion: big refactoring wip

* sfusion: make aux work, at least sfusion sensors should now be functional

* sfusion: lightweight implementation of BMI270 sensor, no sensitivity cal yet

* sfusion: BMI270: added CRT and gyro zx factor. should be functionally equivalent to the old driver

* Added lsm6dsv

* Trying to work around esp32c3 compilation problem, not liking that solution

* sfusion: fix problems found after rebase

* Update README.md

* Bump Arduino core to 3.0 to match GCC12

* Remove fast pin swapping that is no longer compatible with arduino core v3

* Bring back fast pin swapping

* Update platformio-tools.ini

* Fix accel timescale (calibration no longer takes forever)

* Fix non-sfusion sensors

* Added LSM6DSO and DSR support and refactored DSV support

* Removed template float param from the implementation

* sfusion: port MPU6050 driver wip, not expecting to be functional yet

* sfusion: add headers specifying main code owners

* connection: fix warning

* update README.md

* fshelper: fixed ESP8266 regression caused by abstracting FS access

* sfusion: fix error on merge

* bno080: differentiate bno080, bno085, bno086 again

* sfusion: final touches

* restore hadData functionality, implementing it in every sensor, made configured flag bno-only

* fix address supplement in non-sfusion sensors, do i2c bus reset for all sensors

* sfusion: make MPU6050 driver use normal MPU6050 ImuID, change eatSamplesAndReturn function to take ms instead of seconds

* sfusion: hotfix, don't apply sensorOffset, it's applied in sensor base

* Log FIFO overruns on LSMs

* Reset the soft watchdog while eating or collecting calibration samples

Resolves an issue where the soft watchdog would trigger.

* Fix missing word in comment, switch to constexpr

* Update esp32/esp8266

---------

Co-authored-by: Gorbit99 <gorbitgames@gmail.com>
Co-authored-by: nekomona <nekomona@nekomona.com>
Co-authored-by: nekomona <nekomona@163.com>
Co-authored-by: unlogisch04 <98281608+unlogisch04@users.noreply.github.com>
Co-authored-by: kounocom <meia@kouno.xyz>
Co-authored-by: Kubuxu <oss@kubuxu.com>
This commit is contained in:
Przemyslaw Romaniak
2024-06-25 12:57:18 +02:00
committed by GitHub
parent 83b075b804
commit ea00bebedd
51 changed files with 2834 additions and 952 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
README.md
View File

@@ -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.

192
lib/ICM42688/ICM42688.h
View File

@@ -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

63
lib/ICM42688/MMC5983MA.h
View File

@@ -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

46
platformio-tools.ini
View File

@@ -7,66 +7,84 @@ 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
board = esp32-c3-devkitm-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

20
platformio.ini
View File

@@ -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

4
src/configuration/CalibrationConfig.cpp
View File

@@ -37,8 +37,8 @@ namespace SlimeVR {
return "MPU9250";
case ICM20948:
return "ICM20948";
case ICM42688:
return "ICM42688";
case SFUSION:
return "SoftFusion (common)";
default:
return "UNKNOWN";
}

35
src/configuration/CalibrationConfig.h
View File

@@ -25,6 +25,7 @@
#define SLIMEVR_CONFIGURATION_CALIBRATIONCONFIG_H
#include <stdint.h>
#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;
};
}

31
src/configuration/Configuration.cpp
View File

@@ -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;
}
}

50
src/consts.h
View File

@@ -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

10
src/defines_sensitivity.h
View File

@@ -50,11 +50,11 @@
struct SensitivityOffsetXYZ { const char* mac; unsigned char sensorId; double spins; double x; double y; double z; };
const SensitivityOffsetXYZ sensitivityOffsets[] = {
// example values
{ "A4:E5:7C:B6:00:01", SENSORID_PRIMARY, .spins = 10, .x = 2.63, .y = 37.82, .z = 31.11 },
{ "A4:E5:7C:B6:00:02", SENSORID_PRIMARY, .spins = 10, .x = -2.38, .y = -26.8, .z = -42.78 },
{ "A4:E5:7C:B6:00:03", SENSORID_PRIMARY, .spins = 10, .x = 11, .y = 2.2, .z = -1 },
{ "A4:E5:7C:B6:00:04", SENSORID_PRIMARY, .spins = 10, .x = -7, .y = -53.7, .z = -57 },
{ "A4:E5:7C:B6:00:05", SENSORID_PRIMARY, .spins = 10, .x = -10.63, .y = -8.25, .z = -18.6 },
{ .mac = "A4:E5:7C:B6:00:01", .sensorId = SENSORID_PRIMARY, .spins = 10, .x = 2.63, .y = 37.82, .z = 31.11 },
{ .mac = "A4:E5:7C:B6:00:02", .sensorId = SENSORID_PRIMARY, .spins = 10, .x = -2.38, .y = -26.8, .z = -42.78 },
{ .mac = "A4:E5:7C:B6:00:03", .sensorId = SENSORID_PRIMARY, .spins = 10, .x = 11, .y = 2.2, .z = -1 },
{ .mac = "A4:E5:7C:B6:00:04", .sensorId = SENSORID_PRIMARY, .spins = 10, .x = -7, .y = -53.7, .z = -57 },
{ .mac = "A4:E5:7C:B6:00:05", .sensorId = SENSORID_PRIMARY, .spins = 10, .x = -10.63, .y = -8.25, .z = -18.6 },
};
#endif

6
src/main.cpp
View File

@@ -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

4
src/motionprocessing/RestDetection.h
View File

@@ -103,7 +103,7 @@ public:
}
#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 @@ public:
#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;
}

18
src/network/connection.cpp
View File

@@ -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<uint8_t>(sensor.getSensorState())));
MUST(sendByte(static_cast<uint8_t>(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<int>(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<Sensor *> & sensors) {
void Connection::updateSensorState(std::vector<std::unique_ptr<Sensor>> & sensors) {
if (millis() - m_LastSensorInfoPacketTimestamp <= 1000) {
return;
}
@@ -520,7 +520,7 @@ void Connection::updateSensorState(std::vector<Sensor *> & 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<Sensor *> & sensors = sensorManager.getSensors();
auto & sensors = sensorManager.getSensors();
updateSensorState(sensors);
maybeRequestFeatureFlags();

4
src/network/connection.h
View File

@@ -125,7 +125,7 @@ public:
bool endBundle();
private:
void updateSensorState(std::vector<Sensor *> & sensors);
void updateSensorState(std::vector<std::unique_ptr<Sensor>> & sensors);
void maybeRequestFeatureFlags();
bool beginPacket();
@@ -156,7 +156,7 @@ private:
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");

2
src/sensors/EmptySensor.h
View File

@@ -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{};

4
src/sensors/ErroneousSensor.h
View File

@@ -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;
};
}
}

6
src/sensors/SensorFusion.cpp
View File

@@ -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;

6
src/sensors/SensorFusion.h
View File

@@ -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();

4
src/sensors/SensorFusionRestDetect.cpp
View File

@@ -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);

4
src/sensors/SensorFusionRestDetect.h
View File

@@ -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

126
src/sensors/SensorManager.cpp
View File

@@ -22,17 +22,24 @@
*/
#include "SensorManager.h"
#include <i2cscan.h>
#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<SoftFusion::Drivers::LSM6DS3TRC, SoftFusion::I2CImpl>;
using SoftFusionICM42688 = SoftFusionSensor<SoftFusion::Drivers::ICM42688, SoftFusion::I2CImpl>;
using SoftFusionBMI270 = SoftFusionSensor<SoftFusion::Drivers::BMI270, SoftFusion::I2CImpl>;
using SoftFusionLSM6DSV = SoftFusionSensor<SoftFusion::Drivers::LSM6DSV, SoftFusion::I2CImpl>;
using SoftFusionLSM6DSO = SoftFusionSensor<SoftFusion::Drivers::LSM6DSO, SoftFusion::I2CImpl>;
using SoftFusionLSM6DSR = SoftFusionSensor<SoftFusion::Drivers::LSM6DSR, SoftFusion::I2CImpl>;
using SoftFusionMPU6050 = SoftFusionSensor<SoftFusion::Drivers::MPU6050, SoftFusion::I2CImpl>;
// 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<ImuType>(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();
}

70
src/sensors/SensorManager.h
View File

@@ -27,8 +27,14 @@
#include "globals.h"
#include "sensor.h"
#include "EmptySensor.h"
#include "ErroneousSensor.h"
#include "logging/Logger.h"
#include <i2cscan.h>
#include <memory>
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<Sensor *> & getSensors() { return m_Sensors; };
std::vector<std::unique_ptr<Sensor>> & 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<Sensor *> 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<std::unique_ptr<Sensor>> m_Sensors;
template <typename ImuType>
std::unique_ptr<Sensor> 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> 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<ErroneousSensor>(sensorID, ImuType::TypeID);
}
else {
m_Logger.debug("Optional sensor %d not found at address 0x%02X", sensorID + 1, address);
sensor = std::make_unique<EmptySensor>(sensorID);
}
return sensor;
}
uint8_t intPin = extraParam;
sensor = std::make_unique<ImuType>(sensorID, addrSuppl, rotation, sclPin, sdaPin, intPin);
sensor->motionSetup();
return sensor;
}
uint8_t activeSCL = 0;
uint8_t activeSDA = 0;
bool running = false;

1
src/sensors/bmi160sensor.cpp
View File

@@ -332,6 +332,7 @@ void BMI160Sensor::motionLoop() {
#endif
optimistic_yield(100);
if (!sfusion.isUpdated()) return;
hadData = true;
sfusion.clearUpdated();
}
}

15
src/sensors/bmi160sensor.h
View File

@@ -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;

2
src/sensors/bno055sensor.cpp
View File

@@ -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));

7
src/sensors/bno055sensor.h
View File

@@ -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;

4
src/sensors/bno080sensor.cpp
View File

@@ -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);

28
src/sensors/bno080sensor.h
View File

@@ -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 @@ public:
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 @@ private:
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

2
src/sensors/icm20948sensor.cpp
View File

@@ -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()

7
src/sensors/icm20948sensor.h
View File

@@ -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 {

345
src/sensors/icm42688sensor.cpp
View File

@@ -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 <i2cscan.h>
#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]);
}

69
src/sensors/icm42688sensor.h
View File

@@ -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 <ICM42688.h>
#include <MMC5983MA.h>
#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

2
src/sensors/mpu6050sensor.cpp
View File

@@ -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);

7
src/sensors/mpu6050sensor.h
View File

@@ -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;

2
src/sensors/mpu9250sensor.cpp
View File

@@ -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);

7
src/sensors/mpu9250sensor.h
View File

@@ -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

35
src/sensors/sensor.cpp
View File

@@ -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";
}

16
src/sensors/sensor.h
View File

@@ -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 @@ public:
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 @@ public:
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 @@ protected:
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 @@ private:
void printTemperatureCalibrationUnsupported();
};
const char * getIMUNameByType(int imuType);
const char * getIMUNameByType(ImuID imuType);
#endif // SLIMEVR_SENSOR_H_

40
src/sensors/sensoraddresses.h
View File

@@ -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
#define PRIMARY_IMU_ADDRESS_ONE 0
#define PRIMARY_IMU_ADDRESS_TWO 1
#define SECONDARY_IMU_ADDRESS_ONE 0
#define SECONDARY_IMU_ADDRESS_TWO 1

419
src/sensors/softfusion/drivers/bmi270.h Normal file
View File

@@ -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 <cstdint>
#include <array>
#include <algorithm>
#include <limits>
#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 <typename I2CImpl>
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<sizeof(bmi270_firmware);)
{
// tell the device current position
// this thing is little endian, but it requires address in bizzare form
// LSB register is only 4 bits, while MSB register is 8bits
// also value requested is in words (16bit) not in bytes (8bit)
const uint16_t pos_words = 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<uint8_t*>(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<int8_t>(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<uint8_t, 3> 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<int16_t>(i2c.readReg16(Regs::TempData));
return static_cast<float>(value) * TempStep + 23.0f;
}
using FifoBuffer = std::array<uint8_t, I2CImpl::MaxTransactionLength>;
FifoBuffer read_buffer;
template<typename T>
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 <typename AccelCall, typename GyroCall>
void bulkRead(AccelCall &&processAccelSample, GyroCall &&processGyroSample) {
const auto fifo_bytes = i2c.readReg16(Regs::FifoCount);
const auto bytes_to_read = std::min(static_cast<size_t>(read_buffer.size()),
static_cast<size_t>(fifo_bytes));
i2c.readBytes(Regs::FifoData, bytes_to_read, read_buffer.data());
for (uint32_t i=0u; i<bytes_to_read;) {
const uint8_t header = getFromFifo<uint8_t>(i, read_buffer);
if ((header & Fifo::ModeMask) == Fifo::SkipFrame && (i - bytes_to_read) >= 1) {
getFromFifo<uint8_t>(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<uint16_t>(i, read_buffer);
gyro[1] = getFromFifo<uint16_t>(i, read_buffer);
gyro[2] = getFromFifo<uint16_t>(i, read_buffer);
using ShortLimit = std::numeric_limits<int16_t>;
// apply zx factor, todo: this awful line should be simplified and validated
gyro[0] = std::clamp(static_cast<int32_t>(gyro[0]) - static_cast<int16_t>((static_cast<int32_t>(zxFactor) * gyro[2]) / 512),
static_cast<int32_t>(ShortLimit::min()), static_cast<int32_t>(ShortLimit::max()));
processGyroSample(gyro, GyrTs);
}
if (header & Fifo::AccelDataBit) {
int16_t accel[3];
accel[0] = getFromFifo<uint16_t>(i, read_buffer);
accel[1] = getFromFifo<uint16_t>(i, read_buffer);
accel[2] = getFromFifo<uint16_t>(i, read_buffer);
processAccelSample(accel, AccTs);
}
}
}
}
};
} // namespace

478
src/sensors/softfusion/drivers/bmi270fw.h Normal file
View File

@@ -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 <cstdint>
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
};
}

164
src/sensors/softfusion/drivers/icm42688.h Normal file
View File

@@ -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 <cstdint>
#include <array>
#include <algorithm>
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 <typename I2CImpl>
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<int16_t>(i2c.readReg16(Regs::TempData));
float result = ((float)value / 132.48f) + 25.0f;
return result;
}
template <typename AccelCall, typename GyroCall>
void bulkRead(AccelCall &&processAccelSample, GyroCall &&processGyroSample) {
const auto fifo_bytes = i2c.readReg16(Regs::FifoCount);
std::array<uint8_t, FullFifoEntrySize * 8> read_buffer; // max 8 readings
const auto bytes_to_read = std::min(static_cast<size_t>(read_buffer.size()),
static_cast<size_t>(fifo_bytes)) / FullFifoEntrySize * FullFifoEntrySize;
i2c.readBytes(Regs::FifoData, bytes_to_read, read_buffer.data());
for (auto i=0u; i<bytes_to_read; i+=FullFifoEntrySize) {
FifoEntryAligned entry;
memcpy(entry.raw, &read_buffer[i+0x1], sizeof(FifoEntryAligned)); // skip fifo header
processGyroSample(entry.part.gyro, GyrTs);
if (entry.part.accel[0] != -32768) {
processAccelSample(entry.part.accel, AccTs);
}
}
}
};
} // namespace

99
src/sensors/softfusion/drivers/lsm6ds-common.h Normal file
View File

@@ -0,0 +1,99 @@
/*
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 <cstdint>
#include <array>
#include <algorithm>
namespace SlimeVR::Sensors::SoftFusion::Drivers
{
template <typename I2CImpl>
struct LSM6DSOutputHandler
{
LSM6DSOutputHandler(I2CImpl i2c, SlimeVR::Logging::Logger &logger)
: i2c(i2c), logger(logger)
{}
I2CImpl i2c;
SlimeVR::Logging::Logger &logger;
template<typename Regs>
float getDirectTemp() const
{
const auto value = static_cast<int16_t>(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 <typename AccelCall, typename GyroCall, typename Regs>
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<uint8_t, FullFifoEntrySize * 8> read_buffer; // max 8 readings
const auto bytes_to_read = std::min(static_cast<size_t>(read_buffer.size()),
static_cast<size_t>(fifo_bytes)) / FullFifoEntrySize * FullFifoEntrySize;
i2c.readBytes(Regs::FifoData, bytes_to_read, read_buffer.data());
for (auto i=0u; i<bytes_to_read; i+=FullFifoEntrySize) {
FifoEntryAligned entry;
uint8_t tag = read_buffer[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

139
src/sensors/softfusion/drivers/lsm6ds3trc.h Normal file
View File

@@ -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 <cstdint>
#include <array>
#include <algorithm>
namespace SlimeVR::Sensors::SoftFusion::Drivers
{
// Driver uses acceleration range at 8g
// and gyroscope range at 1000dps
// Gyroscope ODR = 416Hz, accel ODR = 416Hz
template <typename I2CImpl>
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<int16_t>(i2c.readReg16(Regs::OutTemp));
float result = ((float)value / 256.0f) + 25.0f;
return result;
}
template <typename AccelCall, typename GyroCall>
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<int16_t, 60> read_buffer; // max 10 packages of 6 16bit values of data form fifo
const auto bytes_to_read = std::min(static_cast<size_t>(read_buffer.size()), static_cast<size_t>(unread_entries)) \
* sizeof(uint16_t) / single_measurement_bytes * single_measurement_bytes;
i2c.readBytes(Regs::FifoData, bytes_to_read, reinterpret_cast<uint8_t *>(read_buffer.data()));
for (uint16_t i=0; i<bytes_to_read/sizeof(uint16_t); i+=single_measurement_words) {
processGyroSample(reinterpret_cast<const int16_t *>(&read_buffer[i]), GyrTs);
processAccelSample(reinterpret_cast<const int16_t *>(&read_buffer[i+3]), AccTs);
}
}
};
} // namespace

120
src/sensors/softfusion/drivers/lsm6dso.h Normal file
View File

@@ -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 <cstdint>
#include <array>
#include <algorithm>
#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 <typename I2CImpl>
struct LSM6DSO : LSM6DSOutputHandler<I2CImpl>
{
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<I2CImpl>::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<I2CImpl>(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<I2CImpl>::template getDirectTemp<Regs>();
}
template <typename AccelCall, typename GyroCall>
void bulkRead(AccelCall &&processAccelSample, GyroCall &&processGyroSample) {
LSM6DSOutputHandler<I2CImpl>::template bulkRead<AccelCall, GyroCall, Regs>(processAccelSample, processGyroSample, GyrTs, AccTs);
}
};
} // namespace

120
src/sensors/softfusion/drivers/lsm6dsr.h Normal file
View File

@@ -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 <cstdint>
#include <array>
#include <algorithm>
#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 <typename I2CImpl>
struct LSM6DSR : LSM6DSOutputHandler<I2CImpl>
{
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<I2CImpl>::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<I2CImpl>(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<I2CImpl>::template getDirectTemp<Regs>();
}
template <typename AccelCall, typename GyroCall>
void bulkRead(AccelCall &&processAccelSample, GyroCall &&processGyroSample) {
LSM6DSOutputHandler<I2CImpl>::template bulkRead<AccelCall, GyroCall, Regs>(processAccelSample, processGyroSample, GyrTs, AccTs);
}
};
} // namespace

135
src/sensors/softfusion/drivers/lsm6dsv.h Normal file
View File

@@ -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 <cstdint>
#include <array>
#include <algorithm>
#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 <typename I2CImpl>
struct LSM6DSV : LSM6DSOutputHandler<I2CImpl>
{
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<I2CImpl>::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<I2CImpl>(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<I2CImpl>::template getDirectTemp<Regs>();
}
template <typename AccelCall, typename GyroCall>
void bulkRead(AccelCall &&processAccelSample, GyroCall &&processGyroSample) {
LSM6DSOutputHandler<I2CImpl>::template bulkRead<AccelCall, GyroCall, Regs>(processAccelSample, processGyroSample, GyrTs, AccTs);
}
};
} // namespace

185
src/sensors/softfusion/drivers/mpu6050.h Normal file
View File

@@ -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 <cstdint>
#include <array>
#include <algorithm>
#include <MPU6050.h>
namespace SlimeVR::Sensors::SoftFusion::Drivers
{
// Driver uses acceleration range at 8g
// and gyroscope range at 1000dps
// Gyroscope ODR = accel ODR = 250Hz
template <typename I2CImpl>
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<int16_t>(value) / 340.0f) + 36.53f;
return result;
}
template <typename AccelCall, typename GyroCall>
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<uint8_t, 12 * 10> readBuffer; // max 10 packages of 12byte values (sample) of data form fifo
auto byteCount = byteSwap(i2c.readReg16(Regs::FifoCount));
auto readBytes = min(static_cast<size_t>(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<FifoSample *>(&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

72
src/sensors/softfusion/i2cimpl.h Normal file
View File

@@ -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 <cstdint>
#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<uint8_t*>(&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<uint8_t*>(&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;
};
}

531
src/sensors/softfusion/softfusionsensor.h Normal file
View File

@@ -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 <template<typename I2CImpl> typename T, typename I2CImpl>
class SoftFusionSensor : public Sensor
{
using imu = T<I2CImpl>;
using RawVectorT = std::array<int16_t, 3>;
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<sensor_real_t>(xyz[0]),
static_cast<sensor_real_t>(xyz[1]),
static_cast<sensor_real_t>(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<sensor_real_t>(GScale * (static_cast<sensor_real_t>(xyz[0]) - m_calibration.G_off[0])),
static_cast<sensor_real_t>(GScale * (static_cast<sensor_real_t>(xyz[1]) - m_calibration.G_off[1])),
static_cast<sensor_real_t>(GScale * (static_cast<sensor_real_t>(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<RawVectorT, RawVectorT> 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<sensor_real_t>(AScale * static_cast<sensor_real_t>(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<sensor_real_t>(AScale * static_cast<sensor_real_t>(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<MagnetoCalibration>();
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<float[]>(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<sensor_real_t>(AScale * static_cast<sensor_real_t>(xyz[0])),
static_cast<sensor_real_t>(AScale * static_cast<sensor_real_t>(xyz[1])),
static_cast<sensor_real_t>(AScale * static_cast<sensor_real_t>(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<I2CImpl> 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

22
src/serial/serialcommands.cpp
View File

@@ -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<int>(sensorManager.getSensorType(0)),
static_cast<int>(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;