Add Per-Sensor Descriptor and Support Multiple IMUs

src/consts.h

@@ -60,10 +60,10 @@
 #define POWER_SAVING_MODERATE 3 // Sleeping and better PS => might miss broadcasts, use at own risk
 #define POWER_SAVING_MAXIMUM 4 // Actual CPU sleeping, currently has issues with disconnecting
 
-#define DEG_0 0.f
-#define DEG_90 -PI / 2
-#define DEG_180 PI
-#define DEG_270 PI / 2
+#define DEG_0 0
+#define DEG_90 -90
+#define DEG_180 180
+#define DEG_270 90
 
 #define CONST_EARTH_GRAVITY 9.80665
 

src/defines.h

@@ -32,6 +32,13 @@
 #define IMU_ROTATION DEG_270
 #define SECOND_IMU_ROTATION DEG_270
 
+#define MAX_IMU_COUNT 2
+
+#ifndef IMU_DESC_STR
+#define IMU_DESC_STR IMU(PRIMARY_IMU_ADDRESS_ONE,IMU_ROTATION,PIN_IMU_SCL,PIN_IMU_SDA,PIN_IMU_INT);\
+                      SECOND_IMU(SECONDARY_IMU_ADDRESS_TWO,IMU_ROTATION,PIN_IMU_SCL,PIN_IMU_SDA,PIN_IMU_INT_2);
+#endif
+
 // Battery monitoring options (comment to disable):
 //   BAT_EXTERNAL for ADC pin, 
 //   BAT_INTERNAL for internal - can detect only low battery, 
@@ -161,4 +168,12 @@
   #define PIN_BATTERY_LEVEL 3
   #define LED_PIN LED_OFF  // RGB LED Protocol would need to be implementetet did not brother for the test, because the board ideal for tracker ifself
 //  #define LED_INVERTED false
+#elif BOARD == BOARD_LOLIN_S2_MINI
+  #define PIN_IMU_SDA 18
+  #define PIN_IMU_SCL 19
+  #define PIN_IMU_INT 16
+  #define PIN_IMU_INT_2 5
+  #define PIN_BATTERY_LEVEL 17
+  #define LED_PIN 2
+  #define LED_INVERTED false
 #endif

src/network/connection.cpp

@@ -427,19 +427,17 @@ void Connection::returnLastPacket(int len) {
 	MUST(endPacket());
 }
 
-void Connection::updateSensorState(Sensor* const sensor1, Sensor* const sensor2) {
+void Connection::updateSensorState(Sensor** const sensors) {
 	if (millis() - m_LastSensorInfoPacketTimestamp <= 1000) {
 		return;
 	}
 
 	m_LastSensorInfoPacketTimestamp = millis();
 
-	if (m_AckedSensorState1 != sensor1->getSensorState()) {
-		sendSensorInfo(sensor1);
-	}
-
-	if (m_AckedSensorState2 != sensor2->getSensorState()) {
-		sendSensorInfo(sensor2);
+	for (int i = 0; i < MAX_IMU_COUNT; i++) {
+		if (m_AckedSensorState[i] != sensors[i]->getSensorState()) {
+			sendSensorInfo(sensors[i]);
+		}
 	}
 }
 
@@ -504,19 +502,17 @@ void Connection::searchForServer() {
 
 void Connection::reset() {
 	m_Connected = false;
-	m_AckedSensorState1 = SensorStatus::SENSOR_OFFLINE;
-	m_AckedSensorState2 = SensorStatus::SENSOR_OFFLINE;
+	std::fill(m_AckedSensorState, m_AckedSensorState+MAX_IMU_COUNT, SensorStatus::SENSOR_OFFLINE);
 
 	m_UDP.begin(m_ServerPort);
 
 	statusManager.setStatus(SlimeVR::Status::SERVER_CONNECTING, true);
 }
 
 void Connection::update() {
-	auto sensor1 = sensorManager.getFirst();
-	auto sensor2 = sensorManager.getSecond();
+	Sensor ** const sensors = sensorManager.getSensors();
 
-	updateSensorState(sensor1, sensor2);
+	updateSensorState(sensors);
 
 	if (!m_Connected) {
 		searchForServer();
@@ -527,8 +523,7 @@ void Connection::update() {
 		statusManager.setStatus(SlimeVR::Status::SERVER_CONNECTING, true);
 
 		m_Connected = false;
-		m_AckedSensorState1 = SensorStatus::SENSOR_OFFLINE;
-		m_AckedSensorState2 = SensorStatus::SENSOR_OFFLINE;
+		std::fill(m_AckedSensorState, m_AckedSensorState+MAX_IMU_COUNT, SensorStatus::SENSOR_OFFLINE);
 		m_Logger.warn("Connection to server timed out");
 
 		return;
@@ -583,10 +578,11 @@ void Connection::update() {
 				break;
 			}
 
-			if (m_Packet[4] == sensor1->getSensorId()) {
-				m_AckedSensorState1 = (SensorStatus)m_Packet[5];
-			} else if (m_Packet[4] == sensor2->getSensorId()) {
-				m_AckedSensorState2 = (SensorStatus)m_Packet[5];
+			for (int i = 0; i < MAX_IMU_COUNT; i++) {
+				if (m_Packet[4] == sensors[i]->getSensorId()) {
+					m_AckedSensorState[i] = (SensorStatus)m_Packet[5];
+					break;
+				}
 			}
 
 			break;

src/network/connection.h

@@ -104,7 +104,7 @@ class Connection {
 #endif
 
 private:
-	void updateSensorState(Sensor* const sensor1, Sensor* const sensor2);
+	void updateSensorState(Sensor** const sensors);
 
 	bool beginPacket();
 	bool endPacket();
@@ -144,10 +144,8 @@ class Connection {
 	unsigned long m_lastConnectionAttemptTimestamp;
 	unsigned long m_LastPacketTimestamp;
 
-	SensorStatus m_AckedSensorState1 = SensorStatus::SENSOR_OFFLINE;
-	SensorStatus m_AckedSensorState2 = SensorStatus::SENSOR_OFFLINE;
+	SensorStatus m_AckedSensorState[MAX_IMU_COUNT] = {SensorStatus::SENSOR_OFFLINE};
 	unsigned long m_LastSensorInfoPacketTimestamp = 0;
-
 	unsigned char m_Buf[8];
 };