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

Make mag work

Browse Source
This commit is contained in:
gorbit99
2025-07-17 00:28:31 +02:00
parent 0425f66561
commit 7083c7e716
6 changed files with 119 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/sensors/SensorToggles.cpp
View File

@@ -12,6 +12,8 @@ void SensorToggleState::setToggle(SensorToggles toggle, bool state) {
tempGradientCalibrationEnabled = state;
break;
}
emitToggleChange(toggle, state);
}
bool SensorToggleState::getToggle(SensorToggles toggle) const {

7
src/sensors/softfusion/drivers/callbacks.h
View File

@@ -28,7 +28,8 @@
template <typename SampleType>
struct DriverCallbacks {
std::function<void(const SampleType sample[3], float AccTs)> processAccelSample;
std::function<void(const SampleType sample[3], float GyrTs)> processGyroSample;
std::function<void(int16_t sample, float TempTs)> processTempSample;
std::function<void(const SampleType sample[3], float AccTs)> processAccelSample = [](SampleType *, float){};
std::function<void(const SampleType sample[3], float GyrTs)> processGyroSample = [](SampleType *, float){};
std::function<void(int16_t sample, float TempTs)> processTempSample = [](int16_t, float){};
std::function<void(uint8_t *sample, float Magts)> processMagSample = [](uint8_t *, float){};
};

71
src/sensors/softfusion/drivers/icm45base.h
View File

@@ -44,7 +44,8 @@ struct ICM45Base {
static constexpr float AccTs = 1.0 / 102.4;
static constexpr float TempTs = 1.0 / 409.6;
static constexpr float MagTs = 1.0 / 100;
static constexpr uint32_t MagPollingHz = 10;
static constexpr float MagTs = 1.0 / MagPollingHz;
static constexpr float GyroSensitivity = 131.072f;
static constexpr float AccelSensitivity = 16384.0f;
@@ -83,7 +84,7 @@ struct ICM45Base {
struct FifoConfig0 {
static constexpr uint8_t reg = 0x1d;
static constexpr uint8_t value
= (0b01 << 6) | (0b011111); // stream to FIFO mode, FIFO depth
= (0b10 << 6) | (0b011111); // stop on full FIFO mode, FIFO depth
// 8k bytes <-- this disables all APEX
// features, but we don't need them
};
@@ -156,10 +157,6 @@ struct ICM45Base {
static constexpr uint8_t reg = 0x1b;
};
struct DmpExtSenOdrCfg {
// TODO: todo
};
struct I2CMControl {
static constexpr Bank bank = Bank::IPregTop1;
static constexpr uint8_t reg = 0x16;
@@ -239,31 +236,22 @@ struct ICM45Base {
std::vector<uint8_t> read_buffer;
void bulkRead(DriverCallbacks<int32_t>&& callbacks) {
if (magPollingEnabled && millis() - lastMagPollMillis >= MagTs * 1000) {
uint8_t magData[9];
readAux(magDataReg, magData, magDataWidth == MagDataWidth::SixByte ? 6 : 9);
callbacks.processMagSample(magData, 1.0f / MagPollingHz);
lastMagPollMillis += MagTs * 1000;
}
constexpr int16_t InvalidReading = -32768;
size_t fifo_packets = m_RegisterInterface.readReg16(BaseRegs::FifoCount);
if (fifo_packets <= 1) {
if (fifo_packets == 0) {
return;
}
// AN-000364
// 2.16 FIFO EMPTY EVENT IN STREAMING MODE CAN CORRUPT FIFO DATA
//
// Description: When in FIFO streaming mode, a FIFO empty event
// (caused by host reading the last byte of the last FIFO frame) can
// cause FIFO data corruption in the first FIFO frame that arrives
// after the FIFO empty condition. Once the issue is triggered, the
// FIFO state is compromised and cannot recover. FIFO must be set in
// bypass mode to flush out the wrong state
//
// When operating in FIFO streaming mode, if FIFO threshold
// interrupt is triggered with M number of FIFO frames accumulated
// in the FIFO buffer, the host should only read the first M-1
// number of FIFO frames. This prevents the FIFO empty event, that
// can cause FIFO data corruption, from happening.
--fifo_packets;
fifo_packets = std::min(fifo_packets, MaxReadings);
size_t bytes_to_read = fifo_packets * FullFifoEntrySize;
@@ -365,13 +353,21 @@ struct ICM45Base {
}
uint8_t readAux(uint8_t address) {
uint8_t buffer;
readAux(address, &buffer, sizeof(buffer));
return buffer;
}
void readAux(uint8_t address, uint8_t* buffer, size_t length) {
assert(length <= 15);
writeBankRegister<typename BaseRegs::I2CMDevProfile0>(address);
writeBankRegister<typename BaseRegs::I2CMCommand0>(
(0b1 << 7) // Last transaction
| (0b0 << 6) // Channel 0
| (0b01 << 4) // Read with register
| (0b0001 << 0) // Read 1 byte
| (length << 0) // Read "length" bytes
);
writeBankRegister<typename BaseRegs::I2CMControl>(
(0b0 << 6) // No restarts
@@ -392,17 +388,18 @@ struct ICM45Base {
);
}
return readBankRegister<typename BaseRegs::I2CMRdData0>();
readBankRegister<typename BaseRegs::I2CMRdData0>(buffer, length);
}
void writeAux(uint8_t address, uint8_t value) {
writeBankRegister<typename BaseRegs::I2CMDevProfile0>(address);
writeBankRegister<typename BaseRegs::I2CMWrData0>(value);
uint8_t writeData[] = {address, value};
writeBankRegister<typename BaseRegs::I2CMWrData0>(writeData, sizeof(writeData));
writeBankRegister<typename BaseRegs::I2CMCommand0>(
(0b1 << 7) // Last transaction
| (0b0 << 6) // Channel 0
| (0b01 << 4) // Read with register
| (0b0001 << 0) // Read 1 byte
| (0b00 << 4) // Write
| (0b0010 << 0) // Write 2 bytes
);
writeBankRegister<typename BaseRegs::I2CMControl>(
(0b0 << 6) // No restarts
@@ -425,13 +422,19 @@ struct ICM45Base {
}
}
bool magPollingEnabled = false;
uint8_t magDataReg = 0x00;
MagDataWidth magDataWidth;
uint64_t lastMagPollMillis = 0;
void startAuxPolling(uint8_t dataReg, MagDataWidth dataWidth) {
// TODO:
magPollingEnabled = true;
magDataReg = dataReg;
magDataWidth = dataWidth;
lastMagPollMillis = millis();
}
void stopAuxPolling() {
// TODO:
}
void stopAuxPolling() { magPollingEnabled = false; }
};
}; // namespace SlimeVR::Sensors::SoftFusion::Drivers

58
src/sensors/softfusion/magdriver.cpp
View File

@@ -49,6 +49,8 @@ std::vector<MagDefinition> MagDriver::supportedMags{
); // LP filter 2, 8x Oversampling, normal mode
return true;
},
.resolution = 0.025f,
},
MagDefinition{
.name = "IST8306",
@@ -61,15 +63,16 @@ std::vector<MagDefinition> MagDriver::supportedMags{
.dataWidth = MagDataWidth::SixByte,
.dataReg = 0x11,
.setup =
[](MagInterface& interface) {
interface.writeByte(0x32, 0x01); // Soft reset
delay(50);
interface.writeByte(0x30, 0x20); // Noise suppression: low
interface.writeByte(0x41, 0x2d); // Oversampling: 32X
interface.writeByte(0x31, 0x02); // Continuous measurement @ 10Hz
return true;
},
.setup = [](MagInterface& interface) {
interface.writeByte(0x32, 0x01); // Soft reset
delay(50);
interface.writeByte(0x30, 0x20); // Noise suppression: low
interface.writeByte(0x41, 0x2d); // Oversampling: 32X
interface.writeByte(0x31, 0x02); // Continuous measurement @ 10Hz
return true;
},
.resolution = 0.3,
},
};
@@ -121,7 +124,40 @@ void MagDriver::stopPolling() const {
interface.stopPolling();
}
const char* MagDriver::getAttachedMagName() const {
void MagDriver::scaleMagSample(const uint8_t* magSample, float* scaled) const {
#pragma pack(push, 1)
struct MagData6Byte {
int16_t x;
int16_t y;
int16_t z;
};
struct MagData9Byte {
int32_t x : 24;
int32_t y : 24;
int32_t z : 24;
};
#pragma pack(pop)
if (!detectedMag) {
return;
}
if (detectedMag->dataWidth == MagDataWidth::SixByte) {
const auto* data = reinterpret_cast<const MagData6Byte*>(magSample);
scaled[0] = data->x * detectedMag->resolution;
scaled[1] = data->y * detectedMag->resolution;
scaled[2] = data->z * detectedMag->resolution;
} else {
const auto* data = reinterpret_cast<const MagData9Byte*>(magSample);
scaled[0] = data->x * detectedMag->resolution;
scaled[1] = data->y * detectedMag->resolution;
scaled[2] = data->z * detectedMag->resolution;
}
}
const char*
MagDriver::getAttachedMagName() const {
if (!detectedMag) {
return nullptr;
}
@@ -129,4 +165,6 @@ const char* MagDriver::getAttachedMagName() const {
return detectedMag->name;
}
bool MagDriver::isMagAttached() const { return detectedMag.has_value(); }
} // namespace SlimeVR::Sensors::SoftFusion

5
src/sensors/softfusion/magdriver.h
View File

@@ -56,6 +56,8 @@ struct MagDefinition {
uint8_t dataReg;
std::function<bool(MagInterface& interface)> setup;
float resolution;
};
class MagDriver {
@@ -63,8 +65,11 @@ public:
bool init(MagInterface&& interface, bool supports9ByteMags);
void startPolling() const;
void stopPolling() const;
void scaleMagSample(const uint8_t* sample, float* scaled) const;
[[nodiscard]] const char* getAttachedMagName() const;
[[nodiscard]] bool isMagAttached() const;
private:
std::optional<MagDefinition> detectedMag;
MagInterface interface;

32
src/sensors/softfusion/softfusionsensor.h
View File

@@ -150,6 +150,12 @@ class SoftFusionSensor : public Sensor {
}
}
void processMagSample(const uint8_t *sample, const sensor_real_t timeDelta) {
float scaledSample[3];
magDriver.scaleMagSample(sample, scaledSample);
m_fusion.updateMag(scaledSample);
}
public:
static constexpr auto TypeID = SensorType::Type;
static constexpr uint8_t Address = SensorType::Address;
@@ -163,13 +169,13 @@ public:
uint8_t = 0
)
: Sensor(
SensorType::Name,
SensorType::Type,
id,
registerInterface,
rotation,
sensorInterface
)
SensorType::Name,
SensorType::Type,
id,
registerInterface,
rotation,
sensorInterface
)
, m_fusion(
SensorType::SensorVQFParams,
SensorType::GyrTs,
@@ -251,6 +257,9 @@ public:
[&](int16_t sample, float TempTs) {
processTempSample(sample, TempTs);
},
[&](uint8_t *sample, float MagTs) {
processMagSample(sample, MagTs);
},
});
if (!m_fusion.isUpdated()) {
checkSensorTimeout();
@@ -287,7 +296,8 @@ public:
// zero-ed out
if (calibrator.calibrationMatches(sensorCalibration)) {
calibrator.assignCalibration(sensorCalibration);
} else if (sensorCalibration.type == SlimeVR::Configuration::SensorConfigType::NONE) {
} else if (sensorCalibration.type
== SlimeVR::Configuration::SensorConfigType::NONE) {
m_Logger.warn(
"No calibration data found for sensor %d, ignoring...",
sensorId
@@ -333,7 +343,7 @@ public:
SoftFusion::MagInterface{
.readByte
= [&](uint8_t address) { return m_sensor.readAux(address); },
.writeByte = [&](uint8_t address, uint8_t value) {},
.writeByte = [&](uint8_t address, uint8_t value) { m_sensor.writeAux(address, value); },
.setDeviceId
= [&](uint8_t deviceId) { m_sensor.setAuxId(deviceId); },
.startPolling
@@ -365,6 +375,10 @@ public:
}
[[nodiscard]] bool isFlagSupported(SensorToggles toggle) const final {
if (toggle == SensorToggles::MagEnabled) {
return magDriver.isMagAttached();
}
return toggle == SensorToggles::CalibrationEnabled
|| toggle == SensorToggles::TempGradientCalibrationEnabled;
}