From 7533030e136ca6befcffe38836e45b220acacef6 Mon Sep 17 00:00:00 2001 From: DevMiner Date: Fri, 8 Mar 2024 13:37:53 +0100 Subject: [PATCH 1/3] impl: basic magnetometer toggling --- src/sensors/sensor.cpp | 6 ++++++ src/sensors/sensor.h | 8 +++++++- 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/src/sensors/sensor.cpp b/src/sensors/sensor.cpp index 011fe93ac..d1672fa8a 100644 --- a/src/sensors/sensor.cpp +++ b/src/sensors/sensor.cpp @@ -67,6 +67,12 @@ void Sensor::printDebugTemperatureCalibrationState() { printTemperatureCalibrati void Sensor::saveTemperatureCalibration() { printTemperatureCalibrationUnsupported(); }; void Sensor::resetTemperatureCalibrationState() { printTemperatureCalibrationUnsupported(); }; +bool Sensor::toggleMagnetometer(bool enabled) { + m_Logger.error("Toggling the magnetometer is not supported or implemented for IMU %s", getIMUNameByType(sensorType)); + + return false; +}; + const char * getIMUNameByType(int imuType) { switch(imuType) { case IMU_MPU9250: diff --git a/src/sensors/sensor.h b/src/sensors/sensor.h index da6ce90b4..8a2cc8340 100644 --- a/src/sensors/sensor.h +++ b/src/sensors/sensor.h @@ -66,6 +66,12 @@ class Sensor virtual void printDebugTemperatureCalibrationState(); virtual void resetTemperatureCalibrationState(); virtual void saveTemperatureCalibration(); + + // TODO: Replace with feature flags + virtual bool supportsTogglingMagnetometer() { return false; }; + virtual bool hasMagnetometerEnabled() { return false; }; + virtual bool toggleMagnetometer(bool enabled); + bool isWorking() { return working; }; @@ -106,7 +112,7 @@ class Sensor Vector3 acceleration{}; SlimeVR::Logging::Logger m_Logger; - + public: uint8_t sclPin = 0; uint8_t sdaPin = 0; From a7e6e6a516fb4e30d54a0f3b4d6305652e9538b6 Mon Sep 17 00:00:00 2001 From: DevMiner Date: Fri, 8 Mar 2024 13:42:12 +0100 Subject: [PATCH 2/3] impl(bno055): magnetometer toggling --- lib/bno055_adafruit/Adafruit_BNO055.h | 2 ++ src/sensors/bno055sensor.cpp | 31 +++++++++++++++++++++++++++ src/sensors/bno055sensor.h | 4 ++++ 3 files changed, 37 insertions(+) diff --git a/lib/bno055_adafruit/Adafruit_BNO055.h b/lib/bno055_adafruit/Adafruit_BNO055.h index 824d17600..77c4bd476 100644 --- a/lib/bno055_adafruit/Adafruit_BNO055.h +++ b/lib/bno055_adafruit/Adafruit_BNO055.h @@ -308,6 +308,8 @@ class Adafruit_BNO055 { void enterSuspendMode(); void enterNormalMode(); + adafruit_bno055_opmode_t getMode() { return _mode; } + private: byte read8(adafruit_bno055_reg_t); bool readLen(adafruit_bno055_reg_t, byte *buffer, uint8_t len); diff --git a/src/sensors/bno055sensor.cpp b/src/sensors/bno055sensor.cpp index 9161511e9..6f11e10f3 100644 --- a/src/sensors/bno055sensor.cpp +++ b/src/sensors/bno055sensor.cpp @@ -24,6 +24,27 @@ #include "globals.h" #include "GlobalVars.h" +/* + Info: + + Check the datasheet for the BNO055 for more information on the different + operation modes. + + OPERATION_MODE_IMUPLUS = OPR_MODE 0x08 + In the IMU mode the relative orientation of the BNO055 in space is + calculated from the accelerometer and gyroscope data. The calculation + is fast. + + OPERATION_MODE_NDOF = OPR_MODE 0x0C + This is a fusion mode with 9 degrees of freedom where the fused + absolute orientation data is calculated from accelerometer, gyroscope + and the magnetometer. The advantages of combining all three sensors are + a fast calculation, resulting in high output data rate, and high + robustness from magnetic field distortions. In this mode the + Fast Magnetometer calibration is turned ON and thereby resulting in + quick calibration of the magnetometer and higher output data accuracy. +*/ + void BNO055Sensor::motionSetup() { imu = Adafruit_BNO055(sensorId, addr); delay(3000); @@ -76,3 +97,13 @@ void BNO055Sensor::motionLoop() { void BNO055Sensor::startCalibration(int calibrationType) { } + +bool BNO055Sensor::hasMagnetometerEnabled() { + return imu.getMode() == Adafruit_BNO055::OPERATION_MODE_NDOF; +} + +bool BNO055Sensor::toggleMagnetometer(bool enabled) { + imu.setMode(enabled ? Adafruit_BNO055::OPERATION_MODE_NDOF : Adafruit_BNO055::OPERATION_MODE_IMUPLUS); + + return true; +} diff --git a/src/sensors/bno055sensor.h b/src/sensors/bno055sensor.h index 0694643a0..29c25326a 100644 --- a/src/sensors/bno055sensor.h +++ b/src/sensors/bno055sensor.h @@ -38,6 +38,10 @@ class BNO055Sensor : public Sensor void motionLoop() override final; void startCalibration(int calibrationType) override final; + bool supportsTogglingMagnetometer() { return true; }; + bool hasMagnetometerEnabled() override final; + bool toggleMagnetometer(bool enabled) override final; + private: Adafruit_BNO055 imu; }; From 2be265980928fd76eb27ab9f8848fd6c3a93a758 Mon Sep 17 00:00:00 2001 From: DevMiner Date: Fri, 8 Mar 2024 13:49:06 +0100 Subject: [PATCH 3/3] impl(network): toggling mags --- src/network/connection.cpp | 51 ++++++++++++++++++++++++++++++++++++++ src/network/connection.h | 3 +++ src/network/packets.h | 1 + 3 files changed, 55 insertions(+) diff --git a/src/network/connection.cpp b/src/network/connection.cpp index 1f60ad876..109c93d8a 100644 --- a/src/network/connection.cpp +++ b/src/network/connection.cpp @@ -299,6 +299,15 @@ void Connection::sendSensorInfo(Sensor* sensor) { MUST(sendByte((uint8_t)sensor->getSensorState())); MUST(sendByte(sensor->getSensorType())); + // 0b00000000 + // ^^ + // |`- supportsMagToggle + // `-- hasMagEnabled + const bool supportsMagToggle = sensor->supportsTogglingMagnetometer(); + const bool hasMagEnabled = sensor->hasMagnetometerEnabled(); + const uint8_t magInfo = (supportsMagToggle << 1) | hasMagEnabled; + MUST(sendByte(magInfo)); + MUST(endPacket()); } @@ -409,6 +418,19 @@ void Connection::sendTrackerDiscovery() { MUST(endPacket()); } +void Connection::sendToggleMagnetometerResult(uint8_t sensorId, bool result) { + MUST(m_Connected); + + MUST(beginPacket()); + + MUST(sendPacketType(PACKET_TOGGLE_MAGNETOMETER)); + MUST(sendPacketNumber()); + MUST(sendByte(sensorId)); + MUST(sendByte(result)); + + MUST(endPacket()); +} + #if ENABLE_INSPECTION void Connection::sendInspectionRawIMUData( uint8_t sensorId, @@ -709,6 +731,35 @@ void Connection::update() { #endif } + break; + + case PACKET_TOGGLE_MAGNETOMETER: + constexpr size_t PACKET_TOGGLE_MAGNETOMETER_SIZE = 4 + 8 + 1 + 1; + + // Packet type (4) + Packet number (8) + sensor ID (1) + enabled (1) + if (len < PACKET_TOGGLE_MAGNETOMETER_SIZE) { + m_Logger.warn("Invalid toggle magnetometer packet: too short"); + break; + } + + const auto sensorId = m_Packet[12]; + const auto enabled = m_Packet[13]; + + const auto sensor = sensorManager.getSensors().at(sensorId); + if (sensor == nullptr) { + m_Logger.warn("Invalid sensor ID in toggle magnetometer packet"); + break; + } + + bool result = false; + if (sensor->supportsTogglingMagnetometer()) { + result = sensor->toggleMagnetometer(enabled); + } else { + m_Logger.warn("Received toggle magnetometer packet for sensor that doesn't support it"); + } + + sendToggleMagnetometerResult(sensorId, result); + break; } } diff --git a/src/network/connection.h b/src/network/connection.h index 7b8f7b6a2..76bb3278a 100644 --- a/src/network/connection.h +++ b/src/network/connection.h @@ -158,6 +158,9 @@ class Connection { // PACKET_SENSOR_INFO 15 void sendSensorInfo(Sensor* sensor); + // PACKET_TOGGLE_MAGNETOMETER 21 + void sendToggleMagnetometerResult(uint8_t sensorId, bool result); + bool m_Connected = false; SlimeVR::Logging::Logger m_Logger = SlimeVR::Logging::Logger("UDPConnection"); diff --git a/src/network/packets.h b/src/network/packets.h index e962a6af0..d91e1da71 100644 --- a/src/network/packets.h +++ b/src/network/packets.h @@ -47,6 +47,7 @@ #define PACKET_TEMPERATURE 20 // #define PACKET_USER_ACTION 21 // Joycon buttons only currently #define PACKET_FEATURE_FLAGS 22 +#define PACKET_TOGGLE_MAGNETOMETER 23 #define PACKET_BUNDLE 100