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

fix icm42688 (#493)

Browse Source 
* fix_icm42688 test fullscale
Still issues with snapping

* fix temp scale, adjust buffer

* change to lowres

* fix semikolon

* Apply suggestions

Co-Authored-By: gorbit99 <5338164+gorbit99@users.noreply.github.com>

* formating

---------

Co-authored-by: gorbit99 <5338164+gorbit99@users.noreply.github.com>
This commit is contained in:
unlogisch04
2025-12-02 20:11:35 +01:00
committed by GitHub
parent fcd49515e1
commit 8469b6fac6
1 changed files with 78 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

100
src/sensors/softfusion/drivers/icm42688.h
View File

@@ -30,6 +30,8 @@
#include "callbacks.h"
#include "vqf.h"
constexpr static bool DEBUG_ICM42688_HIRES = false;
namespace SlimeVR::Sensors::SoftFusion::Drivers {
// Driver uses acceleration range at 8g
@@ -44,16 +46,22 @@ struct ICM42688 {
static constexpr float GyrTs = 1.0 / 200.0;
static constexpr float AccTs = 1.0 / 100.0;
static constexpr float TempTs = 1.0 / 500.0;
static constexpr float TempTs = 1.0 / 200.0;
static constexpr float MagTs = 1.0 / 100;
static constexpr float GyroSensitivity = 32.8f;
static constexpr float AccelSensitivity = 4096.0f;
// When 20-bits data format is used, the only FSR settings that are
// operational are ±2000dps for gyroscope and ±16g for accelerometer, even if the
// FSR selection register settings are configured for other FSR values. The
// corresponding sensitivity scale factor values are 131 LSB/dps for gyroscope and
// 8192 LSB/g for accelerometer.
static constexpr float GyroSensitivity = (DEBUG_ICM42688_HIRES ? 131.0f : 32.8f);
static constexpr float AccelSensitivity
= (DEBUG_ICM42688_HIRES ? 8192.0f : 4096.0f);
static constexpr float TemperatureBias = 25.0f;
static constexpr float TemperatureSensitivity = 2.07f;
static constexpr float TemperatureSensitivity
= (DEBUG_ICM42688_HIRES ? 132.48f : 2.07f);
static constexpr float TemperatureZROChange = 20.0f;
static constexpr VQFParams SensorVQFParams{};
@@ -88,7 +96,9 @@ struct ICM42688 {
static constexpr uint8_t reg = 0x5f;
static constexpr uint8_t value
= 0b1 | (0b1 << 1) | (0b1 << 2)
| (0b0 << 4); // fifo accel en=1, gyro=1, temp=1, hires=1
| (DEBUG_ICM42688_HIRES ? (0b1 << 4) : (0b0 << 4));
// fifo accel en=1, gyro=1, temp=1,
// hires=DEBUG_ICM42688_HIRES
};
struct GyroConfig {
static constexpr uint8_t reg = 0x4f;
@@ -115,7 +125,7 @@ struct ICM42688 {
};
#pragma pack(push, 1)
struct FifoEntryAligned {
struct FifoEntryAlignedHires {
union {
struct {
int16_t accel[3];
@@ -128,10 +138,65 @@ struct ICM42688 {
} part;
uint8_t raw[19];
};
void getGyro(int32_t out[3]) {
// 6.1 Packet Structure for high resolution mode
// https://invensense.tdk.com/wp-content/uploads/2020/04/ds-000347_icm-42688-p-datasheet.pdf
// When 20-bits data format is used, gyroscope data consists of 19-bits
// of actual data and the LSB is always set to 0
out[0] = static_cast<int32_t>(part.gyro[0]) << 3 | ((part.xlsb & 0xe) >> 1);
out[1] = static_cast<int32_t>(part.gyro[1]) << 3 | ((part.ylsb & 0xe) >> 1);
out[2] = static_cast<int32_t>(part.gyro[2]) << 3 | ((part.zlsb & 0xe) >> 1);
}
void getAccel(int32_t out[3]) {
// accelerometer data consists of 18-bits of actual data and the two
// lowest order bits are always set to 0
out[0] = static_cast<int32_t>(part.accel[0]) << 2
| (static_cast<int32_t>((part.xlsb) & 0xf0) >> 6);
out[1] = static_cast<int32_t>(part.accel[1]) << 2
| (static_cast<int32_t>((part.ylsb) & 0xf0) >> 6);
out[2] = static_cast<int32_t>(part.accel[2]) << 2
| (static_cast<int32_t>((part.zlsb) & 0xf0) >> 6);
}
};
struct FifoEntryAlignedDefault {
union {
struct {
int16_t accel[3];
int16_t gyro[3];
int8_t temp;
uint16_t timestamp;
} part;
uint8_t raw[15];
};
void getGyro(int32_t out[3]) {
out[0] = static_cast<int32_t>(part.gyro[0]);
out[1] = static_cast<int32_t>(part.gyro[1]);
out[2] = static_cast<int32_t>(part.gyro[2]);
}
void getAccel(int32_t out[3]) {
out[0] = static_cast<int32_t>(part.accel[0]);
out[1] = static_cast<int32_t>(part.accel[1]);
out[2] = static_cast<int32_t>(part.accel[2]);
}
};
#pragma pack(pop)
static_assert(sizeof(FifoEntryAlignedHires) == 19);
static_assert(sizeof(FifoEntryAlignedDefault) == 15);
using FifoEntryAligned = std::conditional<
DEBUG_ICM42688_HIRES,
FifoEntryAlignedHires,
FifoEntryAlignedDefault>::type;
static constexpr size_t FullFifoEntrySize = sizeof(FifoEntryAligned) + 1;
// max 4 readings in highres mode 8 readings delay too high 6 seems to be the
// edge to work reliably. Tested on ESP8266 with 2 IMU
static constexpr size_t MaxReadings = DEBUG_ICM42688_HIRES ? 4 : 8;
bool initialize() {
// perform initialization step
@@ -155,7 +220,8 @@ struct ICM42688 {
bool bulkRead(DriverCallbacks<int32_t>&& callbacks) {
const auto fifo_bytes = m_RegisterInterface.readReg16(Regs::FifoCount);
std::array<uint8_t, FullFifoEntrySize * 8> read_buffer; // max 8 readings
std::array<uint8_t, FullFifoEntrySize * MaxReadings> read_buffer;
const auto bytes_to_read = std::min(
static_cast<size_t>(read_buffer.size()),
static_cast<size_t>(fifo_bytes)
@@ -171,22 +237,13 @@ struct ICM42688 {
sizeof(FifoEntryAligned)
); // skip fifo header
const int32_t gyroData[3]{
static_cast<int32_t>(entry.part.gyro[0]) << 4 | (entry.part.xlsb & 0xf),
static_cast<int32_t>(entry.part.gyro[1]) << 4 | (entry.part.ylsb & 0xf),
static_cast<int32_t>(entry.part.gyro[2]) << 4 | (entry.part.zlsb & 0xf),
};
int32_t gyroData[3];
entry.getGyro(gyroData);
callbacks.processGyroSample(gyroData, GyrTs);
if (entry.part.accel[0] != -32768) {
const int32_t accelData[3]{
static_cast<int32_t>(entry.part.accel[0]) << 4
| (static_cast<int32_t>(entry.part.xlsb) & 0xf0 >> 4),
static_cast<int32_t>(entry.part.accel[1]) << 4
| (static_cast<int32_t>(entry.part.ylsb) & 0xf0 >> 4),
static_cast<int32_t>(entry.part.accel[2]) << 4
| (static_cast<int32_t>(entry.part.zlsb) & 0xf0 >> 4),
};
int32_t accelData[3];
entry.getAccel(accelData);
callbacks.processAccelSample(accelData, AccTs);
}
@@ -197,7 +254,6 @@ struct ICM42688 {
);
}
}
return fifo_bytes > bytes_to_read;
}
};