Merge pull request #1308 from bitcraze/krichardsson/lh-uart

Optimize UART read for lighthouse

src/drivers/interface/uart1.h

@@ -95,11 +95,24 @@ bool uart1GetDataWithDefaultTimeout(uint8_t *c);
  *
  * @param[in] size  Number of bytes to read
  * @param[out] data  Pointer to data
- * 
+ *
  * @return number of bytes read
  */
 void uart1GetBytesWithDefaultTimeout(uint32_t size, uint8_t* data);
 
+/**
+ * @brief Get the number of bytes available in the UART1 in queue
+ *
+ * @return uint32_t  Number of bytes available
+ */
+uint32_t uart1bytesAvailable();
+
+/**
+ * @brief Get the maximum number of bytes that can be stored in the UART1 in queue
+ *
+ * @return uint32_t  The maximum length of the in queue
+ */
+uint32_t uart1QueueMaxLength();
 
 /**
  * Sends raw data using a lock. Should be used from

src/drivers/src/uart1.c

@@ -48,8 +48,9 @@
  */
 //#define ENABLE_UART1_DMA
 
+#define QUEUE_LENGTH 64
 static xQueueHandle uart1queue;
-STATIC_MEM_QUEUE_ALLOC(uart1queue, 64, sizeof(uint8_t));
+STATIC_MEM_QUEUE_ALLOC(uart1queue, QUEUE_LENGTH, sizeof(uint8_t));
 
 static bool isInit = false;
 static bool hasOverrun = false;
@@ -263,6 +264,16 @@ void uart1Getchar(char * ch)
   xQueueReceive(uart1queue, ch, portMAX_DELAY);
 }
 
+uint32_t uart1bytesAvailable()
+{
+  return uxQueueMessagesWaiting(uart1queue);
+}
+
+uint32_t uart1QueueMaxLength()
+{
+  return QUEUE_LENGTH;
+}
+
 bool uart1DidOverrun()
 {
   bool result = hasOverrun;

src/modules/src/lighthouse/lighthouse_core.c

@@ -229,14 +229,30 @@ void lighthouseCoreSetSystemType(const lighthouseBaseStationType_t type)
   lighthouseUpdateSystemType();
 }
 
+#define OPTIMIZE_UART1_ACCESS 1
 TESTABLE_STATIC bool getUartFrameRaw(lighthouseUartFrame_t *frame) {
   static char data[UART_FRAME_LENGTH];
   int syncCounter = 0;
 
+  #ifdef OPTIMIZE_UART1_ACCESS
+    // Wait until there is enough data available in the queue before reading
+    // to optimize the CPU usage. Locking on the queue (as is done in uart1GetDataWithTimeout()) seems to take a lot
+    // of time and the vTaskDelay() solution uses much less CPU.
+  while (uart1bytesAvailable() < UART_FRAME_LENGTH) {
+    vTaskDelay(1);
+    lighthouseTransmitProcessTimeout();
+  }
+  #endif
+
   for(int i = 0; i < UART_FRAME_LENGTH; i++) {
+  #ifdef OPTIMIZE_UART1_ACCESS
+    uart1Getchar((char*)&data[i]);
+  #else
     while(!uart1GetDataWithTimeout((uint8_t*)&data[i], 2)) {
       lighthouseTransmitProcessTimeout();
     }
+  #endif
+
     if ((unsigned char)data[i] == 0xff) {
       syncCounter += 1;
     }
@@ -527,6 +543,8 @@ void lighthouseCoreTask(void *param) {
   lighthouseStorageInitializeGeoDataFromStorage();
   lighthouseStorageInitializeCalibDataFromStorage();
 
+  ASSERT(uart1QueueMaxLength() >= UART_FRAME_LENGTH);
+
   if (lighthouseDeckFlasherCheckVersionAndBoot() == false) {
     DEBUG_PRINT("FPGA not booted. Lighthouse disabled!\n");
     while(1) {

test/modules/src/lighthouse/test_lighthouse_core.c

@@ -41,6 +41,8 @@ void vTaskDelay(const uint32_t ignore) {}
 static int nrOfCallsToStorageFetchForCalib = 0;
 static size_t mockStorageFetchForCalib(char* key, void* buffer, size_t length, int cmock_num_calls);
 
+static const uint32_t FRAME_LENGTH = 12;
+
 void setUp(void) {
     nrOfCallsToStorageFetchForCalib = 0;
     uart1SetSequence(emptySequence, 0);
@@ -74,6 +76,8 @@ void testThatUartSyncFramesAreSkipped() {
                               0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
   int expectedRead = 24;
   uart1SetSequence(sequence, sizeof(sequence));
+  uart1bytesAvailable_ExpectAndReturn(FRAME_LENGTH);
+  uart1bytesAvailable_ExpectAndReturn(FRAME_LENGTH);
 
   // Test
   do {
@@ -91,6 +95,7 @@ void testThatCorruptUartFramesAreDetectedWithOnesInFirstPadding() {
   // Fixture
   unsigned char sequence[] = {0, 0, 0, 0, 0, 2, 0, 0, 0, 0, 0, 0};
   uart1SetSequence(sequence, sizeof(sequence));
+  uart1bytesAvailable_ExpectAndReturn(FRAME_LENGTH);
 
   // Test
   bool actual = getUartFrameRaw(&frame);
@@ -104,6 +109,7 @@ void testThatCorruptUartFramesAreDetectedWithOnesInSecondPadding() {
   // Fixture
   unsigned char sequence[] = {0, 0, 0, 0, 0, 0, 0, 0, 128, 0, 0, 0};
   uart1SetSequence(sequence, sizeof(sequence));
+  uart1bytesAvailable_ExpectAndReturn(FRAME_LENGTH);
 
   // Test
   bool actual = getUartFrameRaw(&frame);
@@ -118,6 +124,7 @@ void testThatTimeStampIsDecodedInUartFrame() {
   unsigned char sequence[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 2, 1};
   uint32_t expected = 0x010203;
   uart1SetSequence(sequence, sizeof(sequence));
+  uart1bytesAvailable_ExpectAndReturn(FRAME_LENGTH);
 
   // Test
   getUartFrameRaw(&frame);
@@ -133,6 +140,7 @@ void testThatWidthIsDecodedInUartFrame() {
   unsigned char sequence[] = {0, 1, 2, 0, 0, 0, 0, 0, 0, 0, 0, 0};
   uint32_t expected = 0x0201;
   uart1SetSequence(sequence, sizeof(sequence));
+  uart1bytesAvailable_ExpectAndReturn(FRAME_LENGTH);
 
   // Test
   getUartFrameRaw(&frame);
@@ -150,6 +158,7 @@ void testThatOffsetIsDecodedInUartFrame() {
   // The offset is converted from a 6 MHz to 24 MHz clock when read
   uint32_t expected = 0x10203 * 4;
   uart1SetSequence(sequence, sizeof(sequence));
+  uart1bytesAvailable_ExpectAndReturn(FRAME_LENGTH);
 
   // Test
   bool frameOk = getUartFrameRaw(&frame);
@@ -168,6 +177,7 @@ void testThatBeamDataIsDecodedInUartFrame() {
   unsigned char sequence[] = {0, 0, 0, 0, 0, 0, 3, 2, 1, 0, 0, 0};
   uint32_t expected = 0x10203;
   uart1SetSequence(sequence, sizeof(sequence));
+  uart1bytesAvailable_ExpectAndReturn(FRAME_LENGTH);
 
   // Test
   bool frameOk = getUartFrameRaw(&frame);
@@ -185,6 +195,7 @@ void testThatSensorIsDecodedInUartFrame() {
   unsigned char sequence[] = {3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
   uint8_t expected = 0x3;
   uart1SetSequence(sequence, sizeof(sequence));
+  uart1bytesAvailable_ExpectAndReturn(FRAME_LENGTH);
 
   // Test
   getUartFrameRaw(&frame);
@@ -201,6 +212,7 @@ void testThatLackOfChannelIsDecodedInUartFrame() {
   // Fixture
   unsigned char sequence[] = {0x80, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
   uart1SetSequence(sequence, sizeof(sequence));
+  uart1bytesAvailable_ExpectAndReturn(FRAME_LENGTH);
 
   // Test
   getUartFrameRaw(&frame);
@@ -219,6 +231,7 @@ void testThatChannelIsDecodedInUartFrame() {
   unsigned char sequence[] = {0x78, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
   uint8_t expected = 0x0f;
   uart1SetSequence(sequence, sizeof(sequence));
+  uart1bytesAvailable_ExpectAndReturn(FRAME_LENGTH);
 
   // Test
   getUartFrameRaw(&frame);
@@ -237,6 +250,7 @@ void testThatSlowBitIsDecodedInUartFrame() {
   unsigned char sequence[] = {0x04, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
   uint8_t expected = 0x0f;
   uart1SetSequence(sequence, sizeof(sequence));
+  uart1bytesAvailable_ExpectAndReturn(FRAME_LENGTH);
 
   // Test
   getUartFrameRaw(&frame);
@@ -259,7 +273,7 @@ static void uart1ReadCallback(char* ch, int cmock_num_calls) {
     uart1BytesRead++;
 }
 
-static bool uart1GetDataWithTimeoutCallback(char* ch, const uint32_t timeoutTicks, int cmock_num_calls) {
+static bool uart1GetcharCallback(char* ch, int cmock_num_calls) {
     uart1ReadCallback(ch, cmock_num_calls);
     return true;
 }
@@ -270,5 +284,5 @@ static void uart1SetSequence(char* sequence, int length) {
     uart1SequenceLength = length;
 
     uart1Getchar_StubWithCallback(uart1ReadCallback);
-    uart1GetDataWithTimeout_StubWithCallback(uart1GetDataWithTimeoutCallback);
+    uart1Getchar_StubWithCallback(uart1GetcharCallback);
 }