From d516bbfa9e35f167dba72920f6f044041d3fd744 Mon Sep 17 00:00:00 2001 From: sctanf <36978460+sctanf@users.noreply.github.com> Date: Sat, 13 May 2023 14:00:51 -0500 Subject: [PATCH 1/6] add icm only --- lib/ICM42688/ICM42688.h | 192 ++++++++++++++ lib/ICM42688/MMC5983MA.h | 63 +++++ src/configuration/CalibrationConfig.cpp | 2 + src/configuration/CalibrationConfig.h | 16 +- src/configuration/Configuration.cpp | 19 ++ src/consts.h | 1 + src/main.cpp | 2 +- src/sensors/SensorManager.cpp | 5 + src/sensors/icm42688sensor.cpp | 316 ++++++++++++++++++++++++ src/sensors/icm42688sensor.h | 64 +++++ src/sensors/sensor.cpp | 2 + src/sensors/sensoraddresses.h | 4 +- 12 files changed, 682 insertions(+), 4 deletions(-) create mode 100644 lib/ICM42688/ICM42688.h create mode 100644 lib/ICM42688/MMC5983MA.h create mode 100644 src/sensors/icm42688sensor.cpp create mode 100644 src/sensors/icm42688sensor.h diff --git a/lib/ICM42688/ICM42688.h b/lib/ICM42688/ICM42688.h new file mode 100644 index 000000000..115879390 --- /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 000000000..bdec5b77e --- /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 75555ad34..9aeb3eec7 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 5b2a3859f..0fe4d91f8 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 a0264d4b5..8fe169733 100644 --- a/src/configuration/Configuration.cpp +++ b/src/configuration/Configuration.cpp @@ -442,6 +442,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 14a4b8316..2c96c4361 100644 --- a/src/consts.h +++ b/src/consts.h @@ -33,6 +33,7 @@ #define IMU_BNO086 7 #define IMU_BMI160 8 #define IMU_ICM20948 9 +#define IMU_ICM42688 10 #define BOARD_SLIMEVR_LEGACY 1 #define BOARD_SLIMEVR_DEV 2 diff --git a/src/main.cpp b/src/main.cpp index 623445432..6fcc4f044 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 afac9fffa..72c3eb9a2 100644 --- a/src/sensors/SensorManager.cpp +++ b/src/sensors/SensorManager.cpp @@ -30,6 +30,7 @@ #include "mpu6050sensor.h" #include "bmi160sensor.h" #include "icm20948sensor.h" +#include "icm42688sensor.h" #include "ErroneousSensor.h" #include "sensoraddresses.h" @@ -71,6 +72,8 @@ namespace SlimeVR m_Sensor1 = new MPU6050Sensor(sensorID, IMU, firstIMUAddress, IMU_ROTATION); #elif IMU == IMU_ICM20948 m_Sensor1 = new ICM20948Sensor(sensorID, firstIMUAddress, IMU_ROTATION); +#elif IMU == IMU_ICM42688 + m_Sensor1 = new ICM42688Sensor(sensorID, firstIMUAddress, IMU_ROTATION); #endif } @@ -109,6 +112,8 @@ namespace SlimeVR m_Sensor2 = new MPU6050Sensor(sensorID, SECOND_IMU, secondIMUAddress, SECOND_IMU_ROTATION); #elif SECOND_IMU == IMU_ICM20948 m_Sensor2 = new ICM20948Sensor(sensorID, secondIMUAddress, SECOND_IMU_ROTATION); +#elif SECOND_IMU == IMU_ICM42688 + m_Sensor2 = new ICM42688Sensor(sensorID, secondIMUAddress, SECOND_IMU_ROTATION); #endif } diff --git a/src/sensors/icm42688sensor.cpp b/src/sensors/icm42688sensor.cpp new file mode 100644 index 000000000..820fa75f4 --- /dev/null +++ b/src/sensors/icm42688sensor.cpp @@ -0,0 +1,316 @@ +/* + 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" + +// >>>> Ascale = AFS_8G, Gscale = GFS_2000DPS, AODR = AODR_200Hz, GODR = GODR_1kHz, aMode = aMode_LN, gMode = gMode_LN; + +//#if defined(_MAHONY_H_) || defined(_MADGWICK_H_) +constexpr float gscale = (2000. / 32768.0) * (PI / 180.0); //gyro 2000 LSB per d/s -> rad/s +//#endif + +#define MAG_CORR_RATIO 0.02 + +//#define ACCEL_SENSITIVITY_2G 16384.0f +#define ACCEL_SENSITIVITY_8G 4096.0f + +// Accel scale conversion steps: LSB/G -> G -> m/s^2 +//constexpr float ASCALE_2G = ((32768. / ACCEL_SENSITIVITY_2G) / 32768.) * CONST_EARTH_GRAVITY; +constexpr float ASCALE_8G = (8. / 32768.) * CONST_EARTH_GRAVITY; + +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); + + 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 + + // 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"); + } + } + + deltat = 1.0 / 1000.0; // gyro fifo 1khz + + 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(); + + 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(); + + #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_8G; + Axyz[1] = raw1 * ASCALE_8G; + Axyz[2] = raw2 * ASCALE_8G; +} + +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::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]); + + 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]; + 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("}"); + 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"); +} + +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() { + // reading big endian int16 + 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 000000000..c18e14348 --- /dev/null +++ b/src/sensors/icm42688sensor.h @@ -0,0 +1,64 @@ +/* + 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 "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; + + // 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 parseAccelData(); + void parseGyroData(); + void parseMagData(); +}; + +#endif diff --git a/src/sensors/sensor.cpp b/src/sensors/sensor.cpp index 147ce825e..1ea3fe836 100644 --- a/src/sensors/sensor.cpp +++ b/src/sensors/sensor.cpp @@ -74,6 +74,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 b8cb8f0f3..567120d45 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 From 18da4d346244ebd1304590eceb927d81a498f14d Mon Sep 17 00:00:00 2001 From: sctanf <36978460+sctanf@users.noreply.github.com> Date: Sun, 14 May 2023 12:19:36 -0500 Subject: [PATCH 2/6] add mmc, cleanup --- src/sensors/icm42688sensor.cpp | 95 +++++++++++++++++++++++----------- src/sensors/icm42688sensor.h | 4 ++ 2 files changed, 69 insertions(+), 30 deletions(-) diff --git a/src/sensors/icm42688sensor.cpp b/src/sensors/icm42688sensor.cpp index 820fa75f4..90f62af1f 100644 --- a/src/sensors/icm42688sensor.cpp +++ b/src/sensors/icm42688sensor.cpp @@ -30,20 +30,8 @@ #include "mahony.h" // #include "madgwick.h" -// >>>> Ascale = AFS_8G, Gscale = GFS_2000DPS, AODR = AODR_200Hz, GODR = GODR_1kHz, aMode = aMode_LN, gMode = gMode_LN; - -//#if defined(_MAHONY_H_) || defined(_MADGWICK_H_) -constexpr float gscale = (2000. / 32768.0) * (PI / 180.0); //gyro 2000 LSB per d/s -> rad/s -//#endif - -#define MAG_CORR_RATIO 0.02 - -//#define ACCEL_SENSITIVITY_2G 16384.0f -#define ACCEL_SENSITIVITY_8G 4096.0f - -// Accel scale conversion steps: LSB/G -> G -> m/s^2 -//constexpr float ASCALE_2G = ((32768. / ACCEL_SENSITIVITY_2G) / 32768.) * CONST_EARTH_GRAVITY; -constexpr float ASCALE_8G = (8. / 32768.) * CONST_EARTH_GRAVITY; +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 @@ -56,6 +44,24 @@ void ICM42688Sensor::motionSetup() { 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 @@ -68,6 +74,14 @@ void ICM42688Sensor::motionSetup() { 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(); @@ -104,8 +118,6 @@ void ICM42688Sensor::motionSetup() { } } - deltat = 1.0 / 1000.0; // gyro fifo 1khz - 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 @@ -130,6 +142,11 @@ void ICM42688Sensor::motionLoop() { 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) { @@ -170,9 +187,9 @@ void ICM42688Sensor::accel_read() { 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_8G; - Axyz[1] = raw1 * ASCALE_8G; - Axyz[2] = raw2 * ASCALE_8G; + Axyz[0] = raw0 * ascale; + Axyz[1] = raw1 * ascale; + Axyz[2] = raw2 * ascale; } void ICM42688Sensor::gyro_read() { @@ -186,6 +203,18 @@ void ICM42688Sensor::gyro_read() { 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..."); @@ -228,6 +257,8 @@ void ICM42688Sensor::startCalibration(int calibrationType) { 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); @@ -239,7 +270,9 @@ void ICM42688Sensor::startCalibration(int calibrationType) { delete magneto_acc; float M_BAinv[4][3]; - magneto_mag->current_calibration(M_BAinv); + if (magExists) { + magneto_mag->current_calibration(M_BAinv); + } delete magneto_mag; m_Logger.debug("Finished Calculate Calibration data"); @@ -254,16 +287,18 @@ void ICM42688Sensor::startCalibration(int calibrationType) { 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("}"); - 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]); + 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("}"); m_Logger.debug("Saving the calibration data"); @@ -278,6 +313,7 @@ void ICM42688Sensor::startCalibration(int calibrationType) { 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() { @@ -309,7 +345,6 @@ void ICM42688Sensor::parseAccelData() { } void ICM42688Sensor::parseGyroData() { - // reading big endian int16 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 index c18e14348..3da019f7a 100644 --- a/src/sensors/icm42688sensor.h +++ b/src/sensors/icm42688sensor.h @@ -28,6 +28,7 @@ #include "logging/Logger.h" #include +#include #include "I2Cdev.h" class ICM42688Sensor : public Sensor @@ -41,6 +42,8 @@ class ICM42688Sensor : public Sensor private: uint8_t addr; + 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 @@ -55,6 +58,7 @@ class ICM42688Sensor : public Sensor void accel_read(); void gyro_read(); + void mag_read(); void parseAccelData(); void parseGyroData(); From 3375fb3154e4dad012c08b81efd44586d515389a Mon Sep 17 00:00:00 2001 From: sctanf <36978460+sctanf@users.noreply.github.com> Date: Sun, 14 May 2023 12:20:53 -0500 Subject: [PATCH 3/6] todo, mag axes --- src/sensors/icm42688sensor.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/sensors/icm42688sensor.cpp b/src/sensors/icm42688sensor.cpp index 90f62af1f..ce698d3a7 100644 --- a/src/sensors/icm42688sensor.cpp +++ b/src/sensors/icm42688sensor.cpp @@ -164,6 +164,7 @@ void ICM42688Sensor::motionLoop() { 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_) From 99b3c059159d37c48dc0888320295e66b88dae1d Mon Sep 17 00:00:00 2001 From: sctanf <36978460+sctanf@users.noreply.github.com> Date: Mon, 15 May 2023 16:31:22 -0500 Subject: [PATCH 4/6] make it work --- src/sensors/icm42688sensor.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/sensors/icm42688sensor.h b/src/sensors/icm42688sensor.h index 3da019f7a..f33a70a23 100644 --- a/src/sensors/icm42688sensor.h +++ b/src/sensors/icm42688sensor.h @@ -41,7 +41,6 @@ class ICM42688Sensor : public Sensor void startCalibration(int calibrationType) override final; private: - uint8_t addr; uint8_t addr_mag = 0x30; bool magExists = false; From edf9ff9970412c156b50f96d64e3db6de684374c Mon Sep 17 00:00:00 2001 From: sctanf <36978460+sctanf@users.noreply.github.com> Date: Fri, 22 Sep 2023 09:32:13 -0500 Subject: [PATCH 5/6] Update icm42688sensor.h --- src/sensors/icm42688sensor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/sensors/icm42688sensor.h b/src/sensors/icm42688sensor.h index f33a70a23..dffb8e7a5 100644 --- a/src/sensors/icm42688sensor.h +++ b/src/sensors/icm42688sensor.h @@ -34,7 +34,7 @@ class ICM42688Sensor : public Sensor { public: - ICM42688Sensor(uint8_t id, uint8_t address, float rotation) : Sensor("ICM42688Sensor", IMU_ICM42688, id, address, rotation){}; + ICM42688Sensor(uint8_t id, uint8_t address, float rotation, uint8_t sclPin, uint8_t sdaPin) : Sensor("ICM42688Sensor", IMU_ICM42688, id, address, rotation, sclPin, sdaPin){}; ~ICM42688Sensor(){}; void motionSetup() override final; void motionLoop() override final; From 1f13fcd355d97824af24c908d773b471c3db6582 Mon Sep 17 00:00:00 2001 From: sctanf <36978460+sctanf@users.noreply.github.com> Date: Fri, 22 Sep 2023 11:38:29 -0500 Subject: [PATCH 6/6] more fixes, use vqf maybe --- src/sensors/icm42688sensor.cpp | 34 +++++++++++++++------------------- src/sensors/icm42688sensor.h | 12 +++++++----- 2 files changed, 22 insertions(+), 24 deletions(-) diff --git a/src/sensors/icm42688sensor.cpp b/src/sensors/icm42688sensor.cpp index ce698d3a7..6884ba2a4 100644 --- a/src/sensors/icm42688sensor.cpp +++ b/src/sensors/icm42688sensor.cpp @@ -20,7 +20,6 @@ */ #include "icm42688sensor.h" -#include "network/network.h" #include "globals.h" #include "helper_3dmath.h" #include @@ -44,7 +43,7 @@ void ICM42688Sensor::motionSetup() { m_Logger.info("Connected to ICM42688 (reported device ID 0x%02x) at address 0x%02x", temp, addr); - if (I2CSCAN::isI2CExist(addr_mag)) { + 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); @@ -54,10 +53,11 @@ void ICM42688Sensor::motionSetup() { 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; } - deltat = 1.0 / 1000.0; // gyro fifo 1khz - if (magExists) { I2Cdev::writeByte(addr_mag, MMC5983MA_CONTROL_1, 0x80); // Reset MMC now } @@ -136,7 +136,6 @@ void ICM42688Sensor::motionLoop() { 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(); @@ -164,22 +163,20 @@ void ICM42688Sensor::motionLoop() { 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 + // TODO: mag axes will be different, make sure to change them??? + sfusion.updateGyro(Gxyz); } - - quaternion.set(-q[2], q[1], q[3], q[0]); - quaternion *= sensorOffset; + sfusion.updateAcc(Axyz); - if(!lastQuatSent.equalsWithEpsilon(quaternion)) { - newData = true; - lastQuatSent = quaternion; - } + if (magExists) + sfusion.updateMag(Mxyz); + + fusedRotation = sfusion.getQuaternionQuat(); + fusedRotation *= sensorOffset; + acceleration = sfusion.getLinearAccVec(); + setFusedRotationReady(); + setAccelerationReady(); } void ICM42688Sensor::accel_read() { @@ -310,7 +307,6 @@ void ICM42688Sensor::startCalibration(int calibrationType) { configuration.save(); ledManager.off(); - Network::sendCalibrationFinished(CALIBRATION_TYPE_EXTERNAL_ALL, 0); m_Logger.debug("Saved the calibration data"); m_Logger.info("Calibration data gathered"); diff --git a/src/sensors/icm42688sensor.h b/src/sensors/icm42688sensor.h index dffb8e7a5..dc3b936cb 100644 --- a/src/sensors/icm42688sensor.h +++ b/src/sensors/icm42688sensor.h @@ -31,10 +31,14 @@ #include #include "I2Cdev.h" +#include "SensorFusion.h" + class ICM42688Sensor : public Sensor { public: - ICM42688Sensor(uint8_t id, uint8_t address, float rotation, uint8_t sclPin, uint8_t sdaPin) : Sensor("ICM42688Sensor", IMU_ICM42688, id, address, rotation, sclPin, sdaPin){}; + 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; @@ -45,13 +49,11 @@ class ICM42688Sensor : public Sensor 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::Sensors::SensorFusion sfusion; SlimeVR::Configuration::ICM42688CalibrationConfig m_Calibration;