diff --git a/lib/ICM42688/ICM42688.h b/lib/ICM42688/ICM42688.h new file mode 100644 index 0000000..1158793 --- /dev/null +++ b/lib/ICM42688/ICM42688.h @@ -0,0 +1,192 @@ +/* 01/14/2022 Copyright Tlera Corporation + + Created by Kris Winer + + This sketch uses SDA/SCL on pins 21/20 (ladybug default), respectively, and it uses the Ladybug STM32L432 Breakout Board. + The ICM42688 is a combo sensor with embedded accel and gyro, here used as 6 DoF in a 9 DoF absolute orientation solution. + + Library may be used freely and without limit with attribution. + +*/ + +#ifndef ICM42688_h +#define ICM42688_h + +/* ICM42688 registers +https://media.digikey.com/pdf/Data%20Sheets/TDK%20PDFs/ICM-42688-P_DS_Rev1.2.pdf +*/ +// User Bank 0 +#define ICM42688_DEVICE_CONFIG 0x11 +#define ICM42688_DRIVE_CONFIG 0x13 +#define ICM42688_INT_CONFIG 0x14 +#define ICM42688_FIFO_CONFIG 0x16 +#define ICM42688_TEMP_DATA1 0x1D +#define ICM42688_TEMP_DATA0 0x1E +#define ICM42688_ACCEL_DATA_X1 0x1F +#define ICM42688_ACCEL_DATA_X0 0x20 +#define ICM42688_ACCEL_DATA_Y1 0x21 +#define ICM42688_ACCEL_DATA_Y0 0x22 +#define ICM42688_ACCEL_DATA_Z1 0x23 +#define ICM42688_ACCEL_DATA_Z0 0x24 +#define ICM42688_GYRO_DATA_X1 0x25 +#define ICM42688_GYRO_DATA_X0 0x26 +#define ICM42688_GYRO_DATA_Y1 0x27 +#define ICM42688_GYRO_DATA_Y0 0x28 +#define ICM42688_GYRO_DATA_Z1 0x29 +#define ICM42688_GYRO_DATA_Z0 0x2A +#define ICM42688_TMST_FSYNCH 0x2B +#define ICM42688_TMST_FSYNCL 0x2C +#define ICM42688_INT_STATUS 0x2D +#define ICM42688_FIFO_COUNTH 0x2E +#define ICM42688_FIFO_COUNTL 0x2F +#define ICM42688_FIFO_DATA 0x30 +#define ICM42688_APEX_DATA0 0x31 +#define ICM42688_APEX_DATA1 0x32 +#define ICM42688_APEX_DATA2 0x33 +#define ICM42688_APEX_DATA3 0x34 +#define ICM42688_APEX_DATA4 0x35 +#define ICM42688_APEX_DATA5 0x36 +#define ICM42688_INT_STATUS2 0x37 +#define ICM42688_INT_STATUS3 0x38 +#define ICM42688_SIGNAL_PATH_RESET 0x4B +#define ICM42688_INTF_CONFIG0 0x4C +#define ICM42688_INTF_CONFIG1 0x4D +#define ICM42688_PWR_MGMT0 0x4E +#define ICM42688_GYRO_CONFIG0 0x4F +#define ICM42688_ACCEL_CONFIG0 0x50 +#define ICM42688_GYRO_CONFIG1 0x51 +#define ICM42688_GYRO_ACCEL_CONFIG0 0x52 +#define ICM42688_ACCEL_CONFIG1 0x53 +#define ICM42688_TMST_CONFIG 0x54 +#define ICM42688_APEX_CONFIG0 0x56 +#define ICM42688_SMD_CONFIG 0x57 +#define ICM42688_FIFO_CONFIG1 0x5F +#define ICM42688_FIFO_CONFIG2 0x60 +#define ICM42688_FIFO_CONFIG3 0x61 +#define ICM42688_FSYNC_CONFIG 0x62 +#define ICM42688_INT_CONFIG0 0x63 +#define ICM42688_INT_CONFIG1 0x64 +#define ICM42688_INT_SOURCE0 0x65 +#define ICM42688_INT_SOURCE1 0x66 +#define ICM42688_INT_SOURCE3 0x68 +#define ICM42688_INT_SOURCE4 0x69 +#define ICM42688_FIFO_LOST_PKT0 0x6C +#define ICM42688_FIFO_LOST_PKT1 0x6D +#define ICM42688_SELF_TEST_CONFIG 0x70 +#define ICM42688_WHO_AM_I 0x75 // should return 0x47 +#define ICM42688_REG_BANK_SEL 0x76 + +// User Bank 1 +#define ICM42688_SENSOR_CONFIG0 0x03 +#define ICM42688_GYRO_CONFIG_STATIC2 0x0B +#define ICM42688_GYRO_CONFIG_STATIC3 0x0C +#define ICM42688_GYRO_CONFIG_STATIC4 0x0D +#define ICM42688_GYRO_CONFIG_STATIC5 0x0E +#define ICM42688_GYRO_CONFIG_STATIC6 0x0F +#define ICM42688_GYRO_CONFIG_STATIC7 0x10 +#define ICM42688_GYRO_CONFIG_STATIC8 0x11 +#define ICM42688_GYRO_CONFIG_STATIC9 0x12 +#define ICM42688_GYRO_CONFIG_STATIC10 0x13 +#define ICM42688_XG_ST_DATA 0x5F +#define ICM42688_YG_ST_DATA 0x60 +#define ICM42688_ZG_ST_DATA 0x61 +#define ICM42688_TMSTAL0 0x63 +#define ICM42688_TMSTAL1 0x64 +#define ICM42688_TMSTAL2 0x62 +#define ICM42688_INTF_CONFIG4 0x7A +#define ICM42688_INTF_CONFIG5 0x7B +#define ICM42688_INTF_CONFIG6 0x7C + +// User Bank 2 +#define ICM42688_ACCEL_CONFIG_STATIC2 0x03 +#define ICM42688_ACCEL_CONFIG_STATIC3 0x04 +#define ICM42688_ACCEL_CONFIG_STATIC4 0x05 +#define ICM42688_XA_ST_DATA 0x3B +#define ICM42688_YA_ST_DATA 0x3C +#define ICM42688_ZA_ST_DATA 0x3D + +// User Bank 4 +#define ICM42688_APEX_CONFIG1 0x40 +#define ICM42688_APEX_CONFIG2 0x41 +#define ICM42688_APEX_CONFIG3 0x42 +#define ICM42688_APEX_CONFIG4 0x43 +#define ICM42688_APEX_CONFIG5 0x44 +#define ICM42688_APEX_CONFIG6 0x45 +#define ICM42688_APEX_CONFIG7 0x46 +#define ICM42688_APEX_CONFIG8 0x47 +#define ICM42688_APEX_CONFIG9 0x48 +#define ICM42688_ACCEL_WOM_X_THR 0x4A +#define ICM42688_ACCEL_WOM_Y_THR 0x4B +#define ICM42688_ACCEL_WOM_Z_THR 0x4C +#define ICM42688_INT_SOURCE6 0x4D +#define ICM42688_INT_SOURCE7 0x4E +#define ICM42688_INT_SOURCE8 0x4F +#define ICM42688_INT_SOURCE9 0x50 +#define ICM42688_INT_SOURCE10 0x51 +#define ICM42688_OFFSET_USER0 0x77 +#define ICM42688_OFFSET_USER1 0x78 +#define ICM42688_OFFSET_USER2 0x79 +#define ICM42688_OFFSET_USER3 0x7A +#define ICM42688_OFFSET_USER4 0x7B +#define ICM42688_OFFSET_USER5 0x7C +#define ICM42688_OFFSET_USER6 0x7D +#define ICM42688_OFFSET_USER7 0x7E +#define ICM42688_OFFSET_USER8 0x7F + +#define ICM42688_ADDRESS 0x68 // Address of ICM42688 accel/gyro when ADO = 0 + +#define AFS_2G 0x03 +#define AFS_4G 0x02 +#define AFS_8G 0x01 +#define AFS_16G 0x00 // default + +#define GFS_2000DPS 0x00 // default +#define GFS_1000DPS 0x01 +#define GFS_500DPS 0x02 +#define GFS_250DPS 0x03 +#define GFS_125DPS 0x04 +#define GFS_62_50DPS 0x05 +#define GFS_31_25DPS 0x06 +#define GFS_15_625DPS 0x07 + +// Low Noise mode +#define AODR_32kHz 0x01 +#define AODR_16kHz 0x02 +#define AODR_8kHz 0x03 +#define AODR_4kHz 0x04 +#define AODR_2kHz 0x05 +#define AODR_1kHz 0x06 // default +//Low Noise or Low Power modes +#define AODR_500Hz 0x0F +#define AODR_200Hz 0x07 +#define AODR_100Hz 0x08 +#define AODR_50Hz 0x09 +#define AODR_25Hz 0x0A +#define AODR_12_5Hz 0x0B +// Low Power mode +#define AODR_6_25Hz 0x0C +#define AODR_3_125Hz 0x0D +#define AODR_1_5625Hz 0x0E + +#define GODR_32kHz 0x01 +#define GODR_16kHz 0x02 +#define GODR_8kHz 0x03 +#define GODR_4kHz 0x04 +#define GODR_2kHz 0x05 +#define GODR_1kHz 0x06 // default +#define GODR_500Hz 0x0F +#define GODR_200Hz 0x07 +#define GODR_100Hz 0x08 +#define GODR_50Hz 0x09 +#define GODR_25Hz 0x0A +#define GODR_12_5Hz 0x0B + +#define aMode_OFF 0x01 +#define aMode_LP 0x02 +#define aMode_LN 0x03 + +#define gMode_OFF 0x00 +#define gMode_SBY 0x01 +#define gMode_LN 0x03 + +#endif diff --git a/lib/ICM42688/MMC5983MA.h b/lib/ICM42688/MMC5983MA.h new file mode 100644 index 0000000..bdec5b7 --- /dev/null +++ b/lib/ICM42688/MMC5983MA.h @@ -0,0 +1,63 @@ +/* 06/14/2020 Copyright Tlera Corporation + + Created by Kris Winer + + This sketch uses SDA/SCL on pins 21/20 (Ladybug default), respectively, and it uses the Ladybug STM32L432 Breakout Board. + The MMC5983MA is a low power magnetometer, here used as 3 DoF in a 9 DoF absolute orientation solution. + + Library may be used freely and without limit with attribution. + +*/ + +#ifndef MMC5983MA_h +#define MMC5983MA_h + +//Register map for MMC5983MA' +//http://www.memsic.com/userfiles/files/DataSheets/Magnetic-Sensors-Datasheets/MMC5983MA_Datasheet.pdf +#define MMC5983MA_XOUT_0 0x00 +#define MMC5983MA_XOUT_1 0x01 +#define MMC5983MA_YOUT_0 0x02 +#define MMC5983MA_YOUT_1 0x03 +#define MMC5983MA_ZOUT_0 0x04 +#define MMC5983MA_ZOUT_1 0x05 +#define MMC5983MA_XYZOUT_2 0x06 +#define MMC5983MA_TOUT 0x07 +#define MMC5983MA_STATUS 0x08 +#define MMC5983MA_CONTROL_0 0x09 +#define MMC5983MA_CONTROL_1 0x0A +#define MMC5983MA_CONTROL_2 0x0B +#define MMC5983MA_CONTROL_3 0x0C +#define MMC5983MA_PRODUCT_ID 0x2F + +#define MMC5983MA_ADDRESS 0x30 + +// Sample rates +#define MODR_ONESHOT 0x00 +#define MODR_1Hz 0x01 +#define MODR_10Hz 0x02 +#define MODR_20Hz 0x03 +#define MODR_50Hz 0x04 +#define MODR_100Hz 0x05 +#define MODR_200Hz 0x06 // BW = 0x01 only +#define MODR_1000Hz 0x07 // BW = 0x11 only + +//Bandwidths +#define MBW_100Hz 0x00 // 8 ms measurement time +#define MBW_200Hz 0x01 // 4 ms +#define MBW_400Hz 0x02 // 2 ms +#define MBW_800Hz 0x03 // 0.5 ms + +// Set/Reset as a function of measurements +#define MSET_1 0x00 // Set/Reset each data measurement +#define MSET_25 0x01 // each 25 data measurements +#define MSET_75 0x02 +#define MSET_100 0x03 +#define MSET_250 0x04 +#define MSET_500 0x05 +#define MSET_1000 0x06 +#define MSET_2000 0x07 + +#define MMC5983MA_mRes (1.0f / 16384.0f) // mag sensitivity if using 18 bit data +#define MMC5983MA_offset 131072.0f // mag range unsigned to signed + +#endif diff --git a/src/configuration/CalibrationConfig.cpp b/src/configuration/CalibrationConfig.cpp index 75555ad..9aeb3ee 100644 --- a/src/configuration/CalibrationConfig.cpp +++ b/src/configuration/CalibrationConfig.cpp @@ -37,6 +37,8 @@ namespace SlimeVR { return "MPU9250"; case ICM20948: return "ICM20948"; + case ICM42688: + return "ICM42688"; default: return "UNKNOWN"; } diff --git a/src/configuration/CalibrationConfig.h b/src/configuration/CalibrationConfig.h index 5b2a385..0fe4d91 100644 --- a/src/configuration/CalibrationConfig.h +++ b/src/configuration/CalibrationConfig.h @@ -76,7 +76,20 @@ namespace SlimeVR { int32_t C[3]; }; - enum CalibrationConfigType { NONE, BMI160, MPU6050, MPU9250, ICM20948 }; + struct ICM42688CalibrationConfig { + // 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]; + }; + + enum CalibrationConfigType { NONE, BMI160, MPU6050, MPU9250, ICM20948, ICM42688 }; const char* calibrationConfigTypeToString(CalibrationConfigType type); @@ -88,6 +101,7 @@ namespace SlimeVR { MPU6050CalibrationConfig mpu6050; MPU9250CalibrationConfig mpu9250; ICM20948CalibrationConfig icm20948; + ICM42688CalibrationConfig icm42688; } data; }; } diff --git a/src/configuration/Configuration.cpp b/src/configuration/Configuration.cpp index 74431a0..773d2e6 100644 --- a/src/configuration/Configuration.cpp +++ b/src/configuration/Configuration.cpp @@ -440,6 +440,25 @@ namespace SlimeVR { m_Logger.info(" A_B : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.mpu6050.A_B)); m_Logger.info(" G_off: %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.mpu6050.G_off)); + break; + + case CalibrationConfigType::ICM42688: + m_Logger.info(" A_B : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.icm42688.A_B)); + + m_Logger.info(" A_Ainv:"); + for (uint8_t i = 0; i < 3; i++) { + m_Logger.info(" %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.icm42688.A_Ainv[i])); + } + + m_Logger.info(" M_B : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.icm42688.M_B)); + + m_Logger.info(" M_Ainv:"); + for (uint8_t i = 0; i < 3; i++) { + m_Logger.info(" %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.icm42688.M_Ainv[i])); + } + + m_Logger.info(" G_off : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.icm42688.G_off)); + break; } } diff --git a/src/consts.h b/src/consts.h index 15f804e..3e60a68 100644 --- a/src/consts.h +++ b/src/consts.h @@ -34,6 +34,7 @@ #define IMU_BNO086 7 #define IMU_BMI160 8 #define IMU_ICM20948 9 +#define IMU_ICM42688 10 #define IMU_DEV_RESERVED 250 // Reserved, should not be used in any release firmware #define BOARD_UNKNOWN 0 diff --git a/src/main.cpp b/src/main.cpp index bc6148d..b4a4dd8 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -71,7 +71,7 @@ void setup() SerialCommands::setUp(); -#if IMU == IMU_MPU6500 || IMU == IMU_MPU6050 || IMU == IMU_MPU9250 || IMU == IMU_BNO055 || IMU == IMU_ICM20948 || IMU == IMU_BMI160 +#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 diff --git a/src/sensors/SensorManager.cpp b/src/sensors/SensorManager.cpp index 6995147..434f8a5 100644 --- a/src/sensors/SensorManager.cpp +++ b/src/sensors/SensorManager.cpp @@ -29,6 +29,7 @@ #include "mpu6050sensor.h" #include "bmi160sensor.h" #include "icm20948sensor.h" +#include "icm42688sensor.h" #include "ErroneousSensor.h" #include "sensoraddresses.h" #include "GlobalVars.h" @@ -104,6 +105,9 @@ namespace SlimeVR 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; diff --git a/src/sensors/icm42688sensor.cpp b/src/sensors/icm42688sensor.cpp new file mode 100644 index 0000000..ce698d3 --- /dev/null +++ b/src/sensors/icm42688sensor.cpp @@ -0,0 +1,352 @@ +/* + 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 "network/network.h" +#include "globals.h" +#include "helper_3dmath.h" +#include +#include "calibration.h" +#include "magneto1.4.h" +#include "GlobalVars.h" +#include "mahony.h" +// #include "madgwick.h" + +constexpr float gscale = (2000. / 32768.0) * (PI / 180.0); // gyro LSB/d/s -> rad/s +constexpr float ascale = (8. / 32768.) * CONST_EARTH_GRAVITY; // accel LSB/G -> m/s^2 + +void ICM42688Sensor::motionSetup() { + // initialize device + uint8_t temp; + I2Cdev::readByte(addr, ICM42688_WHO_AM_I, &temp); + if(!(temp == 0x47 || temp == 0xDB)) { + m_Logger.fatal("Can't connect to ICM42688 (reported device ID 0x%02x) at address 0x%02x", temp, addr); + return; + } + + m_Logger.info("Connected to ICM42688 (reported device ID 0x%02x) at address 0x%02x", temp, addr); + + if (I2CSCAN::isI2CExist(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; + } + } + + deltat = 1.0 / 1000.0; // gyro fifo 1khz + + 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]; + uint16_t stco = 0; + 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 here + #if defined(_MAHONY_H_) + mahonyQuaternionUpdate(q, Axyz[0], Axyz[1], Axyz[2], Gxyz[0], Gxyz[1], Gxyz[2], Mxyz[0], Mxyz[1], Mxyz[2], deltat * 1.0e-6); + #elif defined(_MADGWICK_H_) + madgwickQuaternionUpdate(q, Axyz[0], Axyz[1], Axyz[2], Gxyz[0], Gxyz[1], Gxyz[2], Mxyz[0], Mxyz[1], Mxyz[2], deltat * 1.0e-6); + #endif + } + + quaternion.set(-q[2], q[1], q[3], q[0]); + + quaternion *= sensorOffset; + + if(!lastQuatSent.equalsWithEpsilon(quaternion)) { + newData = true; + lastQuatSent = quaternion; + } +} + +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(); + Network::sendCalibrationFinished(CALIBRATION_TYPE_EXTERNAL_ALL, 0); + m_Logger.debug("Saved the calibration data"); + + m_Logger.info("Calibration data gathered"); + I2Cdev::writeByte(addr, ICM42688_SIGNAL_PATH_RESET, 0x02); // flush FIFO before return +} + +void ICM42688Sensor::parseMagData() { + float temp[3]; + + //apply offsets and scale factors from Magneto + for (unsigned i = 0; i < 3; i++) { + temp[i] = (Mxyz[i] - m_Calibration.M_B[i]); + #if useFullCalibrationMatrix == true + Mxyz[i] = m_Calibration.M_Ainv[i][0] * temp[0] + m_Calibration.M_Ainv[i][1] * temp[1] + m_Calibration.M_Ainv[i][2] * temp[2]; + #else + Mxyz[i] = temp[i]; + #endif + } +} + +void ICM42688Sensor::parseAccelData() { + float temp[3]; + + //apply offsets (bias) and scale factors from Magneto + for (unsigned i = 0; i < 3; i++) { + temp[i] = (Axyz[i] - m_Calibration.A_B[i]); + #if useFullCalibrationMatrix == true + Axyz[i] = m_Calibration.A_Ainv[i][0] * temp[0] + m_Calibration.A_Ainv[i][1] * temp[1] + m_Calibration.A_Ainv[i][2] * temp[2]; + #else + Axyz[i] = temp[i]; + #endif + } +} + +void ICM42688Sensor::parseGyroData() { + Gxyz[0] = (Gxyz[0] - m_Calibration.G_off[0]); + Gxyz[1] = (Gxyz[1] - m_Calibration.G_off[1]); + Gxyz[2] = (Gxyz[2] - m_Calibration.G_off[2]); +} diff --git a/src/sensors/icm42688sensor.h b/src/sensors/icm42688sensor.h new file mode 100644 index 0000000..f33a70a --- /dev/null +++ b/src/sensors/icm42688sensor.h @@ -0,0 +1,67 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2021 Eiren Rain & SlimeVR contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#ifndef SENSORS_ICM42688SENSOR_H +#define SENSORS_ICM42688SENSOR_H + +#include "sensor.h" +#include "logging/Logger.h" + +#include +#include +#include "I2Cdev.h" + +class ICM42688Sensor : public Sensor +{ +public: + ICM42688Sensor(uint8_t id, uint8_t address, float rotation) : Sensor("ICM42688Sensor", IMU_ICM42688, id, address, rotation){}; + ~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 q[4]{1.0f, 0.0f, 0.0f, 0.0f}; // for raw filter + float Axyz[3]{}; + float Gxyz[3]{}; + float Mxyz[3]{}; + Quat correction{0, 0, 0, 0}; + // Loop timing globals + float deltat = 0; // sample time in seconds + + SlimeVR::Configuration::ICM42688CalibrationConfig m_Calibration; + + void accel_read(); + void gyro_read(); + void mag_read(); + + void parseAccelData(); + void parseGyroData(); + void parseMagData(); +}; + +#endif diff --git a/src/sensors/sensor.cpp b/src/sensors/sensor.cpp index 730ea68..011fe93 100644 --- a/src/sensors/sensor.cpp +++ b/src/sensors/sensor.cpp @@ -87,6 +87,8 @@ const char * getIMUNameByType(int imuType) { return "BMI160"; case IMU_ICM20948: return "ICM20948"; + case IMU_ICM42688: + return "ICM42688"; } return "Unknown"; } diff --git a/src/sensors/sensoraddresses.h b/src/sensors/sensoraddresses.h index b8cb8f0..567120d 100644 --- a/src/sensors/sensoraddresses.h +++ b/src/sensors/sensoraddresses.h @@ -10,7 +10,7 @@ #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 + #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 @@ -21,7 +21,7 @@ #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 + #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