Skip to content

Mag Support (Continuation) #465

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
wants to merge 2 commits into
base: main
Choose a base branch
from
+102 −38
Draft

Mag Support (Continuation) #465

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
7083c7e
Make mag work
gorbit99 Jul 16, 2025
5b3dc43
Formatting
gorbit99 Jul 16, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions src/sensors/SensorToggles.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,8 @@ void SensorToggleState::setToggle(SensorToggles toggle, bool state) {
tempGradientCalibrationEnabled = state;
break;
}

emitToggleChange(toggle, state);
}

bool SensorToggleState::getToggle(SensorToggles toggle) const {
Expand Down
11 changes: 8 additions & 3 deletions src/sensors/softfusion/drivers/callbacks.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,12 @@

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 changes: 37 additions & 34 deletions src/sensors/softfusion/drivers/icm45base.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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
};
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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
Expand All @@ -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
Expand All @@ -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
37 changes: 37 additions & 0 deletions src/sensors/softfusion/magdriver.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,8 @@ std::vector<MagDefinition> MagDriver::supportedMags{
); // LP filter 2, 8x Oversampling, normal mode
return true;
},

.resolution = 0.025f,
},
MagDefinition{
.name = "IST8306",
Expand All @@ -70,6 +72,8 @@ std::vector<MagDefinition> MagDriver::supportedMags{
interface.writeByte(0x31, 0x02); // Continuous measurement @ 10Hz
return true;
},

.resolution = 0.3,
},
};

Expand Down Expand Up @@ -121,6 +125,37 @@ void MagDriver::stopPolling() const {
interface.stopPolling();
}

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;
Expand All @@ -129,4 +164,6 @@ const char* MagDriver::getAttachedMagName() const {
return detectedMag->name;
}

bool MagDriver::isMagAttached() const { return detectedMag.has_value(); }

} // namespace SlimeVR::Sensors::SoftFusion
5 changes: 5 additions & 0 deletions src/sensors/softfusion/magdriver.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,15 +56,20 @@ struct MagDefinition {
uint8_t dataReg;

std::function<bool(MagInterface& interface)> setup;

float resolution;
};

class MagDriver {
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;
Expand Down
14 changes: 13 additions & 1 deletion src/sensors/softfusion/softfusionsensor.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -251,6 +257,7 @@ class SoftFusionSensor : public Sensor {
[&](int16_t sample, float TempTs) {
processTempSample(sample, TempTs);
},
[&](uint8_t* sample, float MagTs) { processMagSample(sample, MagTs); },
});
if (!m_fusion.isUpdated()) {
checkSensorTimeout();
Expand Down Expand Up @@ -333,7 +340,8 @@ class SoftFusionSensor : public Sensor {
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
Expand Down Expand Up @@ -365,6 +373,10 @@ class SoftFusionSensor : public Sensor {
}

[[nodiscard]] bool isFlagSupported(SensorToggles toggle) const final {
if (toggle == SensorToggles::MagEnabled) {
return magDriver.isMagAttached();
}

return toggle == SensorToggles::CalibrationEnabled
|| toggle == SensorToggles::TempGradientCalibrationEnabled;
}
Expand Down