#570 Added decoding of lighthouse 2 data from FPGA

src/modules/interface/lighthouse/lighthouse_core.h

@@ -31,10 +31,20 @@
 #include <stdint.h>
 
 typedef struct {
+  bool isSyncFrame;
+  uint8_t sensor;
+
+  // V1 base station data --------
   uint32_t timestamp;
-  uint32_t sensor;
   uint16_t width;
-  bool isSyncFrame;
+
+  // V2 base station data --------
+  uint32_t beamData;
+  uint32_t offset;
+  bool channelFound;
+  // Channel is zero indexed (0-15) here, while it is one indexed in the base station config (1 - 16)
+  uint8_t channel;
+  uint8_t slowbit;
 } lighthouseUartFrame_t;
 
 void lighthouseCoreTask(void *param);

src/modules/src/lighthouse/lighthouse_core.c

@@ -90,7 +90,12 @@ static bool getUartFrameRaw(lighthouseUartFrame_t *frame) {
   frame->isSyncFrame = (syncCounter == UART_FRAME_LENGTH);
 
   frame->sensor = data[0] & 0x03;
+  frame->channelFound = (data[0] & 0x80) == 0;
+  frame->channel = (data[0] >> 3) & 0x0f;
+  frame->slowbit = (data[0] >> 2) & 0x01;
   memcpy(&frame->width, &data[1], 2);
+  memcpy(&frame->offset, &data[3], 3);
+  memcpy(&frame->beamData, &data[6], 3);
   memcpy(&frame->timestamp, &data[9], 3);
 
   bool isPaddingZero = (((data[5] | data[8]) & 0xfe) == 0);

test/modules/src/lighthouse/test_lighthouse_core.c

@@ -118,10 +118,45 @@ void testThatWidthIsDecodedInUartFrame() {
 }
 
 
+void testThatOffsetIsDecodedInUartFrame() {
+  // Fixture
+  unsigned char sequence[] = {0, 0, 0, 3, 2, 1, 0, 0, 0, 0, 0, 0};
+  uint32_t expected = 0x10203;
+  uart1SetSequence(sequence, sizeof(sequence));
+
+  // Test
+  bool frameOk = getUartFrame(&frame);
+
+  // Assert
+  uint32_t actual = frame.offset;
+  TEST_ASSERT_EQUAL_UINT32(expected, actual);
+
+  // Verify the padding data was not affected
+  TEST_ASSERT_TRUE(frameOk)
+}
+
+
+void testThatBeamDataIsDecodedInUartFrame() {
+  // Fixture
+  unsigned char sequence[] = {0, 0, 0, 0, 0, 0, 3, 2, 1, 0, 0, 0};
+  uint32_t expected = 0x10203;
+  uart1SetSequence(sequence, sizeof(sequence));
+
+  // Test
+  bool frameOk = getUartFrame(&frame);
+
+  // Assert
+  uint32_t actual = frame.beamData;
+  TEST_ASSERT_EQUAL_UINT32(expected, actual);
+
+  // Verify the padding data was not affected
+  TEST_ASSERT_TRUE(frameOk)
+}
+
 void testThatSensorIsDecodedInUartFrame() {
   // Fixture
-  unsigned char sequence[] = {2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
-  uint8_t expected = 0x2;
+  unsigned char sequence[] = {3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+  uint8_t expected = 0x3;
   uart1SetSequence(sequence, sizeof(sequence));
 
   // Test
@@ -130,6 +165,61 @@ void testThatSensorIsDecodedInUartFrame() {
   // Assert
   uint32_t actual = frame.sensor;
   TEST_ASSERT_EQUAL_UINT32(expected, actual);
+
+  // Verify we did not get data in other fields
+  TEST_ASSERT_TRUE(frame.channelFound);
+}
+
+void testThatLackOfChannelIsDecodedInUartFrame() {
+  // Fixture
+  unsigned char sequence[] = {0x80, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+  uart1SetSequence(sequence, sizeof(sequence));
+
+  // Test
+  getUartFrame(&frame);
+
+  // Assert
+  TEST_ASSERT_FALSE(frame.channelFound);
+
+  // Verify we did not get data in other fields
+  TEST_ASSERT_EQUAL_UINT8(0, frame.channel);
+  TEST_ASSERT_FALSE(frame.slowbit);
+}
+
+
+void testThatChannelIsDecodedInUartFrame() {
+  // Fixture
+  unsigned char sequence[] = {0x78, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+  uint8_t expected = 0x0f;
+  uart1SetSequence(sequence, sizeof(sequence));
+
+  // Test
+  getUartFrame(&frame);
+
+  // Assert
+  TEST_ASSERT_EQUAL_UINT8(expected, frame.channel);
+
+  // Verify we did not get data in other fields
+  TEST_ASSERT_TRUE(frame.channelFound);
+  TEST_ASSERT_FALSE(frame.slowbit);
+}
+
+
+void testThatSlowBitIsDecodedInUartFrame() {
+  // Fixture
+  unsigned char sequence[] = {0x04, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+  uint8_t expected = 0x0f;
+  uart1SetSequence(sequence, sizeof(sequence));
+
+  // Test
+  getUartFrame(&frame);
+
+  // Assert
+  TEST_ASSERT_TRUE(frame.slowbit);
+
+  // Verify we did not get data in other fields
+  TEST_ASSERT_TRUE(frame.channelFound);
+  TEST_ASSERT_EQUAL_UINT8(0, frame.channel);
 }
 
 // Test support ----------------------------------------------------------------------------------------------------