From b3b5b757eb88cf1dea1206843370c72f3e7170c5 Mon Sep 17 00:00:00 2001 From: ThreadOfFate <37180802+ThreadOfFate@users.noreply.github.com> Date: Fri, 24 Mar 2023 22:38:16 +0000 Subject: [PATCH] Fix AUX trackers inconsistent tracker ID issue. (#233) --- src/network/udpclient.cpp | 5 +-- src/sensors/SensorManager.cpp | 60 ++++++++++++++++------------------- src/sensors/sensoraddresses.h | 35 ++++++++++++++++++++ 3 files changed, 66 insertions(+), 34 deletions(-) create mode 100644 src/sensors/sensoraddresses.h diff --git a/src/network/udpclient.cpp b/src/network/udpclient.cpp index 7685473db..b7465cddb 100644 --- a/src/network/udpclient.cpp +++ b/src/network/udpclient.cpp @@ -665,9 +665,10 @@ void ServerConnection::update(Sensor * const sensor, Sensor * const sensor2) { udpClientLogger.warn("Wrong sensor info packet"); break; } - if(incomingPacket[4] == 0) { + if(incomingPacket[4] == sensor->getSensorId()) { sensorStateNotified1 = incomingPacket[5]; - } else if(incomingPacket[4] == 1) { + } + else if(incomingPacket[4] == sensor2->getSensorId()) { sensorStateNotified2 = incomingPacket[5]; } break; diff --git a/src/sensors/SensorManager.cpp b/src/sensors/SensorManager.cpp index 67ed4ca19..afac9fffa 100644 --- a/src/sensors/SensorManager.cpp +++ b/src/sensors/SensorManager.cpp @@ -31,6 +31,7 @@ #include "bmi160sensor.h" #include "icm20948sensor.h" #include "ErroneousSensor.h" +#include "sensoraddresses.h" namespace SlimeVR { @@ -41,37 +42,35 @@ namespace SlimeVR uint8_t firstIMUAddress = 0; uint8_t secondIMUAddress = 0; + bool sharedIMUAddresses = (PRIMARY_IMU_ADDRESS_ONE == SECONDARY_IMU_ADDRESS_ONE && PRIMARY_IMU_ADDRESS_TWO == SECONDARY_IMU_ADDRESS_TWO); { -#if IMU == IMU_BNO080 || IMU == IMU_BNO085 || IMU == IMU_BNO086 - firstIMUAddress = I2CSCAN::pickDevice(0x4A, 0x4B, true); -#elif IMU == IMU_BNO055 - firstIMUAddress = I2CSCAN::pickDevice(0x29, 0x28, true); -#elif IMU == IMU_MPU9250 || IMU == IMU_BMI160 || IMU == IMU_MPU6500 || IMU == IMU_MPU6050 || IMU == IMU_ICM20948 - firstIMUAddress = I2CSCAN::pickDevice(0x68, 0x69, true); -#else -#error Unsupported primary IMU -#endif + firstIMUAddress = I2CSCAN::pickDevice(PRIMARY_IMU_ADDRESS_ONE, PRIMARY_IMU_ADDRESS_TWO, true); + uint8_t sensorID = 0; + if(sharedIMUAddresses && firstIMUAddress != PRIMARY_IMU_ADDRESS_ONE) + { + sensorID = 1; + } if (firstIMUAddress == 0) { - m_Sensor1 = new ErroneousSensor(0, IMU); + m_Sensor1 = new ErroneousSensor(sensorID, IMU); } else { m_Logger.trace("Primary IMU found at address 0x%02X", firstIMUAddress); #if IMU == IMU_BNO080 || IMU == IMU_BNO085 || IMU == IMU_BNO086 - m_Sensor1 = new BNO080Sensor(0, IMU, firstIMUAddress, IMU_ROTATION, PIN_IMU_INT); + m_Sensor1 = new BNO080Sensor(sensorID, IMU, firstIMUAddress, IMU_ROTATION, PIN_IMU_INT); #elif IMU == IMU_BNO055 - m_Sensor1 = new BNO055Sensor(0, firstIMUAddress, IMU_ROTATION); + m_Sensor1 = new BNO055Sensor(sensorID, firstIMUAddress, IMU_ROTATION); #elif IMU == IMU_MPU9250 - m_Sensor1 = new MPU9250Sensor(0, firstIMUAddress, IMU_ROTATION); + m_Sensor1 = new MPU9250Sensor(sensorID, firstIMUAddress, IMU_ROTATION); #elif IMU == IMU_BMI160 - m_Sensor1 = new BMI160Sensor(0, firstIMUAddress, IMU_ROTATION); + m_Sensor1 = new BMI160Sensor(sensorID, firstIMUAddress, IMU_ROTATION); #elif IMU == IMU_MPU6500 || IMU == IMU_MPU6050 - m_Sensor1 = new MPU6050Sensor(0, IMU, firstIMUAddress, IMU_ROTATION); + m_Sensor1 = new MPU6050Sensor(sensorID, IMU, firstIMUAddress, IMU_ROTATION); #elif IMU == IMU_ICM20948 - m_Sensor1 = new ICM20948Sensor(0, firstIMUAddress, IMU_ROTATION); + m_Sensor1 = new ICM20948Sensor(sensorID, firstIMUAddress, IMU_ROTATION); #endif } @@ -79,15 +78,12 @@ namespace SlimeVR } { -#if SECOND_IMU == IMU_BNO080 || SECOND_IMU == IMU_BNO085 || SECOND_IMU == IMU_BNO086 - secondIMUAddress = I2CSCAN::pickDevice(0x4B, 0x4A, false); -#elif SECOND_IMU == IMU_BNO055 - secondIMUAddress = I2CSCAN::pickDevice(0x28, 0x29, false); -#elif SECOND_IMU == IMU_MPU9250 || SECOND_IMU == IMU_BMI160 || SECOND_IMU == IMU_MPU6500 || SECOND_IMU == IMU_MPU6050 || SECOND_IMU == IMU_ICM20948 - secondIMUAddress = I2CSCAN::pickDevice(0x69, 0x68, false); -#else -#error Unsupported secondary IMU -#endif + secondIMUAddress = I2CSCAN::pickDevice(SECONDARY_IMU_ADDRESS_TWO, SECONDARY_IMU_ADDRESS_ONE, false); + uint8_t sensorID = 1; + if(sharedIMUAddresses && secondIMUAddress != SECONDARY_IMU_ADDRESS_TWO) + { + sensorID = 0; + } if (secondIMUAddress == firstIMUAddress) { @@ -95,24 +91,24 @@ namespace SlimeVR } else if (secondIMUAddress == 0) { - m_Sensor2 = new ErroneousSensor(1, SECOND_IMU); + m_Sensor2 = new ErroneousSensor(sensorID, SECOND_IMU); } else { m_Logger.trace("Secondary IMU found at address 0x%02X", secondIMUAddress); #if SECOND_IMU == IMU_BNO080 || SECOND_IMU == IMU_BNO085 || SECOND_IMU == IMU_BNO086 - m_Sensor2 = new BNO080Sensor(1, SECOND_IMU, secondIMUAddress, SECOND_IMU_ROTATION, PIN_IMU_INT_2); + m_Sensor2 = new BNO080Sensor(sensorID, SECOND_IMU, secondIMUAddress, SECOND_IMU_ROTATION, PIN_IMU_INT_2); #elif SECOND_IMU == IMU_BNO055 - m_Sensor2 = new BNO055Sensor(1, secondIMUAddress, SECOND_IMU_ROTATION); + m_Sensor2 = new BNO055Sensor(sensorID, secondIMUAddress, SECOND_IMU_ROTATION); #elif SECOND_IMU == IMU_MPU9250 - m_Sensor2 = new MPU9250Sensor(1, secondIMUAddress, SECOND_IMU_ROTATION); + m_Sensor2 = new MPU9250Sensor(sensorID, secondIMUAddress, SECOND_IMU_ROTATION); #elif SECOND_IMU == IMU_BMI160 - m_Sensor2 = new BMI160Sensor(1, secondIMUAddress, SECOND_IMU_ROTATION); + m_Sensor2 = new BMI160Sensor(sensorID, secondIMUAddress, SECOND_IMU_ROTATION); #elif SECOND_IMU == IMU_MPU6500 || SECOND_IMU == IMU_MPU6050 - m_Sensor2 = new MPU6050Sensor(1, SECOND_IMU, secondIMUAddress, SECOND_IMU_ROTATION); + m_Sensor2 = new MPU6050Sensor(sensorID, SECOND_IMU, secondIMUAddress, SECOND_IMU_ROTATION); #elif SECOND_IMU == IMU_ICM20948 - m_Sensor2 = new ICM20948Sensor(1, secondIMUAddress, SECOND_IMU_ROTATION); + m_Sensor2 = new ICM20948Sensor(sensorID, secondIMUAddress, SECOND_IMU_ROTATION); #endif } diff --git a/src/sensors/sensoraddresses.h b/src/sensors/sensoraddresses.h new file mode 100644 index 000000000..b8cb8f0f3 --- /dev/null +++ b/src/sensors/sensoraddresses.h @@ -0,0 +1,35 @@ +//This is useful if you want to use an address shifter +#define DEFAULT_IMU_ADDRESS true + +//We use fixed address values instead of scanning to keep the sensorID consistent and to avoid issues with some breakout board with multiple active ic2 addresses +#if DEFAULT_IMU_ADDRESS + + #if IMU == IMU_BNO080 || IMU == IMU_BNO085 || IMU == IMU_BNO086 + #define PRIMARY_IMU_ADDRESS_ONE 0x4A + #define PRIMARY_IMU_ADDRESS_TWO 0x4B + #elif IMU == IMU_BNO055 + #define PRIMARY_IMU_ADDRESS_ONE 0x29 + #define PRIMARY_IMU_ADDRESS_TWO 0x28 + #elif IMU == IMU_MPU9250 || IMU == IMU_BMI160 || IMU == IMU_MPU6500 || IMU == IMU_MPU6050 || IMU == IMU_ICM20948 + #define PRIMARY_IMU_ADDRESS_ONE 0x68 + #define PRIMARY_IMU_ADDRESS_TWO 0x69 + #endif + + #if SECOND_IMU == IMU_BNO080 || SECOND_IMU == IMU_BNO085 || SECOND_IMU == IMU_BNO086 + #define SECONDARY_IMU_ADDRESS_ONE 0x4A + #define SECONDARY_IMU_ADDRESS_TWO 0x4B + #elif SECOND_IMU == IMU_BNO055 + #define SECONDARY_IMU_ADDRESS_ONE 0x29 + #define SECONDARY_IMU_ADDRESS_TWO 0x28 + #elif SECOND_IMU == IMU_MPU9250 || SECOND_IMU == IMU_BMI160 || SECOND_IMU == IMU_MPU6500 || SECOND_IMU == IMU_MPU6050 || SECOND_IMU == IMU_ICM20948 + #define SECONDARY_IMU_ADDRESS_ONE 0x68 + #define SECONDARY_IMU_ADDRESS_TWO 0x69 + #endif + +#else + //If not using the default address you can set custom addresses here + #define PRIMARY_IMU_ADDRESS_ONE 0x69 + #define PRIMARY_IMU_ADDRESS_TWO 0x68 + #define SECONDARY_IMU_ADDRESS_ONE 0x69 + #define SECONDARY_IMU_ADDRESS_TWO 0x68 +#endif \ No newline at end of file