Skip to content

Commit adba3fd

Browse files
gorbit99gorbit99
authored
Mag support (Attempt 2) (#458)
* WhoAmI check working * In theory this should be setting up the mag * Not sure how that happened * Add magnetometer status to GET TEST and GET INFO * Formatting * Formatting 2
1 parent 1c31f8e commit adba3fd

File tree

11 files changed

+521
-3
lines changed

11 files changed

+521
-3
lines changed

‎src/sensors/SensorToggles.cpp

Lines changed: 24 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -25,3 +25,27 @@ bool SensorToggleState::getToggle(SensorToggles toggle) const {
2525
}
2626
return false;
2727
}
28+
29+
void SensorToggleState::onToggleChange(
30+
std::function<void(SensorToggles, bool)>&& callback
31+
) {
32+
this->callback = callback;
33+
}
34+
35+
void SensorToggleState::emitToggleChange(SensorToggles toggle, bool state) const {
36+
if (callback) {
37+
(*callback)(toggle, state);
38+
}
39+
}
40+
41+
const char* SensorToggleState::toggleToString(SensorToggles toggle) {
42+
switch (toggle) {
43+
case SensorToggles::MagEnabled:
44+
return "MagEnabled";
45+
case SensorToggles::CalibrationEnabled:
46+
return "CalibrationEnabled";
47+
case SensorToggles::TempGradientCalibrationEnabled:
48+
return "TempGradientCalibrationEnabled";
49+
}
50+
return "Unknown";
51+
}

‎src/sensors/SensorToggles.h

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -24,6 +24,8 @@
2424
#pragma once
2525

2626
#include <cstdint>
27+
#include <functional>
28+
#include <optional>
2729

2830
#include "../debug.h"
2931

@@ -38,7 +40,14 @@ class SensorToggleState {
3840
void setToggle(SensorToggles toggle, bool state);
3941
[[nodiscard]] bool getToggle(SensorToggles toggle) const;
4042

43+
void onToggleChange(std::function<void(SensorToggles, bool)>&& callback);
44+
45+
static const char* toggleToString(SensorToggles toggle);
46+
4147
private:
48+
std::optional<std::function<void(SensorToggles, bool)>> callback;
49+
void emitToggleChange(SensorToggles toggle, bool state) const;
50+
4251
bool magEnabled = !USE_6_AXIS;
4352
bool calibrationEnabled = true;
4453
bool tempGradientCalibrationEnabled = true;

‎src/sensors/bno080sensor.cpp

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -135,6 +135,13 @@ void BNO080Sensor::motionSetup() {
135135
configured = true;
136136
m_tpsCounter.reset();
137137
m_dataCounter.reset();
138+
139+
toggles.onToggleChange([&](SensorToggles toggle, bool) {
140+
if (toggle == SensorToggles::MagEnabled) {
141+
// TODO: maybe handle this more gracefully, I'm sure it's possible
142+
motionSetup();
143+
}
144+
});
138145
}
139146

140147
void BNO080Sensor::motionLoop() {

‎src/sensors/sensor.cpp

Lines changed: 9 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -91,6 +91,8 @@ void Sensor::resetTemperatureCalibrationState() {
9191
printTemperatureCalibrationUnsupported();
9292
};
9393

94+
const char* Sensor::getAttachedMagnetometer() const { return nullptr; }
95+
9496
SlimeVR::Configuration::SensorConfigBits Sensor::getSensorConfigData() {
9597
return SlimeVR::Configuration::SensorConfigBits{
9698
.magEnabled = toggles.getToggle(SensorToggles::MagEnabled),
@@ -157,12 +159,16 @@ void Sensor::markRestCalibrationComplete(bool completed) {
157159
}
158160

159161
void Sensor::setFlag(SensorToggles toggle, bool state) {
160-
assert(isFlagSupported(toggle));
162+
if (!isFlagSupported(toggle)) {
163+
m_Logger.error(
164+
"Toggle %s isn't supported by this sensor!",
165+
SensorToggleState::toggleToString(toggle)
166+
);
167+
return;
168+
}
161169

162170
toggles.setToggle(toggle, state);
163171

164172
configuration.setSensorToggles(sensorId, toggles);
165173
configuration.save();
166-
167-
motionSetup();
168174
}

‎src/sensors/sensor.h

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -85,6 +85,11 @@ class Sensor {
8585
virtual void printDebugTemperatureCalibrationState();
8686
virtual void resetTemperatureCalibrationState();
8787
virtual void saveTemperatureCalibration();
88+
// TODO: currently only for softfusionsensor, bmi160 and others should get
89+
// an overload too
90+
virtual const char* getAttachedMagnetometer() const;
91+
// TODO: realistically each sensor should print its own state instead of
92+
// having 15 getters for things only the serial commands use
8893
bool isWorking() { return working; };
8994
bool getHadData() const { return hadData; };
9095
bool isValid() { return m_hwInterface != nullptr; };

‎src/sensors/softfusion/drivers/icm45base.h

Lines changed: 201 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,7 @@
2626

2727
#include "../../../sensorinterface/RegisterInterface.h"
2828
#include "callbacks.h"
29+
#include "sensors/softfusion/magdriver.h"
2930

3031
namespace SlimeVR::Sensors::SoftFusion::Drivers {
3132

@@ -104,6 +105,77 @@ struct ICM45Base {
104105

105106
static constexpr uint8_t FifoCount = 0x12;
106107
static constexpr uint8_t FifoData = 0x14;
108+
109+
// Indirect Register Access
110+
111+
static constexpr uint32_t IRegWaitTimeMicros = 4;
112+
113+
enum class Bank : uint8_t {
114+
IMemSram = 0x00,
115+
IPregBar = 0xa0,
116+
IPregSys1 = 0xa4,
117+
IPregSys2 = 0xa5,
118+
IPregTop1 = 0xa2,
119+
};
120+
121+
static constexpr uint8_t IRegAddr = 0x7c;
122+
static constexpr uint8_t IRegData = 0x7e;
123+
124+
// Mag Support
125+
126+
struct IOCPadScenarioAuxOvrd {
127+
static constexpr uint8_t reg = 0x30;
128+
static constexpr uint8_t value = (0b1 << 4) // Enable AUX1 override
129+
| (0b01 << 2) // Enable I2CM master
130+
| (0b1 << 1) // Enable AUX1 enable override
131+
| (0b1 << 0); // Enable AUX1
132+
};
133+
134+
struct I2CMCommand0 {
135+
static constexpr Bank bank = Bank::IPregTop1;
136+
static constexpr uint8_t reg = 0x06;
137+
};
138+
139+
struct I2CMDevProfile0 {
140+
static constexpr Bank bank = Bank::IPregTop1;
141+
static constexpr uint8_t reg = 0x0e;
142+
};
143+
144+
struct I2CMDevProfile1 {
145+
static constexpr Bank bank = Bank::IPregTop1;
146+
static constexpr uint8_t reg = 0x0f;
147+
};
148+
149+
struct I2CMWrData0 {
150+
static constexpr Bank bank = Bank::IPregTop1;
151+
static constexpr uint8_t reg = 0x33;
152+
};
153+
154+
struct I2CMRdData0 {
155+
static constexpr Bank bank = Bank::IPregTop1;
156+
static constexpr uint8_t reg = 0x1b;
157+
};
158+
159+
struct DmpExtSenOdrCfg {
160+
// TODO: todo
161+
};
162+
163+
struct I2CMControl {
164+
static constexpr Bank bank = Bank::IPregTop1;
165+
static constexpr uint8_t reg = 0x16;
166+
};
167+
168+
struct I2CMStatus {
169+
static constexpr Bank bank = Bank::IPregTop1;
170+
static constexpr uint8_t reg = 0x18;
171+
172+
static constexpr uint8_t SDAErr = 0b1 << 5;
173+
static constexpr uint8_t SCLErr = 0b1 << 4;
174+
static constexpr uint8_t SRSTErr = 0b1 << 3;
175+
static constexpr uint8_t TimeoutErr = 0b1 << 2;
176+
static constexpr uint8_t Done = 0b1 << 1;
177+
static constexpr uint8_t Busy = 0b1 << 0;
178+
};
107179
};
108180

109181
#pragma pack(push, 1)
@@ -149,6 +221,11 @@ struct ICM45Base {
149221
BaseRegs::PwrMgmt0::value
150222
);
151223

224+
m_RegisterInterface.writeReg(
225+
BaseRegs::IOCPadScenarioAuxOvrd::reg,
226+
BaseRegs::IOCPadScenarioAuxOvrd::value
227+
);
228+
152229
read_buffer.resize(FullFifoEntrySize * MaxReadings);
153230

154231
delay(1);
@@ -231,6 +308,130 @@ struct ICM45Base {
231308
}
232309
}
233310
}
311+
312+
template <typename Reg>
313+
uint8_t readBankRegister() {
314+
uint8_t buffer;
315+
readBankRegister<Reg>(&buffer, sizeof(buffer));
316+
return buffer;
317+
}
318+
319+
template <typename Reg, typename T>
320+
void readBankRegister(T* buffer, size_t length) {
321+
uint8_t data[] = {
322+
static_cast<uint8_t>(Reg::bank),
323+
Reg::reg,
324+
};
325+
326+
auto* bufferBytes = reinterpret_cast<uint8_t*>(buffer);
327+
m_RegisterInterface.writeBytes(BaseRegs::IRegAddr, sizeof(data), data);
328+
delayMicroseconds(BaseRegs::IRegWaitTimeMicros);
329+
for (size_t i = 0; i < length * sizeof(T); i++) {
330+
bufferBytes[i] = m_RegisterInterface.readReg(BaseRegs::IRegData);
331+
delayMicroseconds(BaseRegs::IRegWaitTimeMicros);
332+
}
333+
}
334+
335+
template <typename Reg>
336+
void writeBankRegister() {
337+
writeBankRegister<Reg>(&Reg::value, sizeof(Reg::value));
338+
}
339+
340+
template <typename Reg, typename T>
341+
void writeBankRegister(T* buffer, size_t length) {
342+
auto* bufferBytes = reinterpret_cast<uint8_t*>(buffer);
343+
344+
uint8_t data[] = {
345+
static_cast<uint8_t>(Reg::bank),
346+
Reg::reg,
347+
bufferBytes[0],
348+
};
349+
350+
m_RegisterInterface.writeBytes(BaseRegs::IRegAddr, sizeof(data), data);
351+
delayMicroseconds(BaseRegs::IRegWaitTimeMicros);
352+
for (size_t i = 1; i < length * sizeof(T); i++) {
353+
m_RegisterInterface.writeReg(BaseRegs::IRegData, bufferBytes[i]);
354+
delayMicroseconds(BaseRegs::IRegWaitTimeMicros);
355+
}
356+
}
357+
358+
template <typename Reg>
359+
void writeBankRegister(uint8_t value) {
360+
writeBankRegister<Reg>(&value, sizeof(value));
361+
}
362+
363+
void setAuxId(uint8_t deviceId) {
364+
writeBankRegister<typename BaseRegs::I2CMDevProfile1>(deviceId);
365+
}
366+
367+
uint8_t readAux(uint8_t address) {
368+
writeBankRegister<typename BaseRegs::I2CMDevProfile0>(address);
369+
370+
writeBankRegister<typename BaseRegs::I2CMCommand0>(
371+
(0b1 << 7) // Last transaction
372+
| (0b0 << 6) // Channel 0
373+
| (0b01 << 4) // Read with register
374+
| (0b0001 << 0) // Read 1 byte
375+
);
376+
writeBankRegister<typename BaseRegs::I2CMControl>(
377+
(0b0 << 6) // No restarts
378+
| (0b0 << 3) // Fast mode
379+
| (0b1 << 0) // Start transaction
380+
);
381+
382+
uint8_t lastStatus;
383+
while ((lastStatus = readBankRegister<typename BaseRegs::I2CMStatus>())
384+
& BaseRegs::I2CMStatus::Busy)
385+
;
386+
387+
if (lastStatus != BaseRegs::I2CMStatus::Done) {
388+
m_Logger.error(
389+
"Aux read from address 0x%02x returned status 0x%02x",
390+
address,
391+
lastStatus
392+
);
393+
}
394+
395+
return readBankRegister<typename BaseRegs::I2CMRdData0>();
396+
}
397+
398+
void writeAux(uint8_t address, uint8_t value) {
399+
writeBankRegister<typename BaseRegs::I2CMDevProfile0>(address);
400+
writeBankRegister<typename BaseRegs::I2CMWrData0>(value);
401+
writeBankRegister<typename BaseRegs::I2CMCommand0>(
402+
(0b1 << 7) // Last transaction
403+
| (0b0 << 6) // Channel 0
404+
| (0b01 << 4) // Read with register
405+
| (0b0001 << 0) // Read 1 byte
406+
);
407+
writeBankRegister<typename BaseRegs::I2CMControl>(
408+
(0b0 << 6) // No restarts
409+
| (0b0 << 3) // Fast mode
410+
| (0b1 << 0) // Start transaction
411+
);
412+
413+
uint8_t lastStatus;
414+
while ((lastStatus = readBankRegister<typename BaseRegs::I2CMStatus>())
415+
& BaseRegs::I2CMStatus::Busy)
416+
;
417+
418+
if (lastStatus != BaseRegs::I2CMStatus::Done) {
419+
m_Logger.error(
420+
"Aux write to address 0x%02x with value 0x%02x returned status 0x%02x",
421+
address,
422+
value,
423+
lastStatus
424+
);
425+
}
426+
}
427+
428+
void startAuxPolling(uint8_t dataReg, MagDataWidth dataWidth) {
429+
// TODO:
430+
}
431+
432+
void stopAuxPolling() {
433+
// TODO:
434+
}
234435
};
235436

236437
}; // namespace SlimeVR::Sensors::SoftFusion::Drivers

‎src/sensors/softfusion/imuconsts.h

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -56,4 +56,13 @@ struct IMUConsts {
5656
return IMU::TempTs;
5757
}
5858
}
59+
60+
static constexpr bool SupportsMags = requires(IMU& i) { i.readAux(0x00); };
61+
static constexpr bool Supports9ByteMag = []() constexpr {
62+
if constexpr (requires { IMU::Supports9ByteMag; }) {
63+
return IMU::Supports9ByteMag;
64+
} else {
65+
return true;
66+
}
67+
}();
5968
};

0 commit comments

Comments
 (0)