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

Add ICM-42688 imu and MMC5983MA magnetometer (#242)

Browse Source
This commit is contained in:
sctanf
2023-09-22 09:30:25 -05:00
committed by GitHub
parent 3789a4cdb8
commit 1ed8d63e65
12 changed files with 720 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

192
lib/ICM42688/ICM42688.h Normal file
View File

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

63
lib/ICM42688/MMC5983MA.h Normal file
View File

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

2
src/configuration/CalibrationConfig.cpp
View File

@@ -37,6 +37,8 @@ namespace SlimeVR {
return "MPU9250";
case ICM20948:
return "ICM20948";
case ICM42688:
return "ICM42688";
default:
return "UNKNOWN";
}

16
src/configuration/CalibrationConfig.h
View File

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

19
src/configuration/Configuration.cpp
View File

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

1
src/consts.h
View File

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

2
src/main.cpp
View File

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

4
src/sensors/SensorManager.cpp
View File

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

352
src/sensors/icm42688sensor.cpp Normal file
View File

@@ -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 <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::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]);
}

67
src/sensors/icm42688sensor.h Normal file
View File

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

2
src/sensors/sensor.cpp
View File

@@ -87,6 +87,8 @@ const char * getIMUNameByType(int imuType) {
return "BMI160";
case IMU_ICM20948:
return "ICM20948";
case IMU_ICM42688:
return "ICM42688";
}
return "Unknown";
}

4
src/sensors/sensoraddresses.h
View File

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