Fix AUX trackers inconsistent tracker ID issue. (#233)

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;

src/sensors/SensorManager.cpp

@@ -31,6 +31,7 @@
 #include "bmi160sensor.h"
 #include "icm20948sensor.h"
 #include "ErroneousSensor.h"
+#include "sensoraddresses.h"
 
 namespace SlimeVR
 {
@@ -41,78 +42,73 @@ 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
                 }
 
                 m_Sensor1->motionSetup();
             }
 
             {
-#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)
                 {
                     m_Logger.debug("No secondary IMU connected");
                 }
                 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
                 }
 

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