performance GPS driver

Author: ClemensElflein

boards/XCORE/board_ex.h

@@ -59,14 +59,6 @@
 // 255.255.255.0
 #define FALLBACK_NETMASK 0xFFFFFF00
 
-// Flash information for the bootloader
-#define BOOT_ADDRESS 0x8020000
-// Available flash pages for user program
-#define FLASH_PAGE_COUNT 7
-// Size of each flash page in bytes
-#define FLASH_PAGE_SIZE_BYTES 0x20000
-// Size of flash memory for the user program
-#define PROGRAM_FLASH_SIZE_BYTES (FLASH_PAGE_COUNT * FLASH_PAGE_SIZE_BYTES)
 
 #define SPID_IMU SPID3
 #ifndef __ASSEMBLER__

main.cpp

@@ -126,6 +126,7 @@ int main(void) {
 
   // chThdCreateStatic(testWa, sizeof(testWa), NORMALPRIO, testThreadFunc, NULL);
   gps.set_state_callback(etl::delegate<void(const xbot::driver::gps::GpsInterface::GpsState &new_state)>::create<gpsUpdated>());
+  gps.set_datum(49.0, 10.0, 150);
   gps.start_driver(&UARTD6, 921600);
 
   // Subscribe to global events and dispatch to our services

src/boot_service_discovery.c

@@ -68,6 +68,14 @@ static void multicast_sender_thread(void *p) {
         recvfrom(sockfd, boardAdvertisementRequestBuffer,
                  sizeof(boardAdvertisementRequestBuffer) - 1, 0, NULL, NULL);
     if (received > 0) {
+      // check, if the command was "RESET", if so, we reset
+      if(strncmp(boardAdvertisementBuffer, "RESET", sizeof(boardAdvertisementBuffer)) == 0) {
+        // Set the boot address to bootloader and reset the system
+        MODIFY_REG(SYSCFG->UR2, SYSCFG_UR2_BOOT_ADD0,
+           (0x8000000 >> 16) << SYSCFG_UR2_BOOT_ADD0_Pos);
+        NVIC_SystemReset();
+        while(1);
+      }
       // Send the multicast message
       sendto(sockfd, boardAdvertisementBuffer, strlen(boardAdvertisementBuffer),
              0, (struct sockaddr *)&multicast_addr, sizeof(multicast_addr));

src/drivers/gps/gps_interface.cpp

@@ -5,6 +5,7 @@
 #include "gps_interface.h"
 
 #include <etl/algorithm.h>
+#include <ulog.h>
 
 #include <cmath>
 
@@ -14,18 +15,26 @@ namespace gps {
 
 GpsInterface::GpsInterface() {
   datum_lat_ = datum_long_ = NAN;
+  chMtxObjectInit(&processing_mutex);
 }
 bool GpsInterface::start_driver(UARTDriver *uart, uint32_t baudrate) {
   chDbgAssert(stopped_, "don't start the driver twice");
   chDbgAssert(uart != nullptr, "need to provide a driver");
   if(!stopped_) {
     return false;
   }
-  stopped_ = false;
   this->uart = uart;
   uart_config.speed = baudrate;
-  chThdCreateStatic(&wa, sizeof(wa), NORMALPRIO, GpsInterface::threadHelper, this);
-  return uartStart(uart, &uart_config) == MSG_OK;
+  bool uartStarted = uartStart(uart, &uart_config) == MSG_OK;
+  if(!uartStarted) {
+    return false;
+  }
+
+  stopped_ = false;
+  processing_thread_ = chThdCreateStatic(&wa, sizeof(wa), NORMALPRIO, threadHelper, this);
+  // Allow higher prio in order to not lose buffers. Active time in this thread is very minimal (it's just swapping buffers)
+  chThdCreateStatic(&recvThdWa, sizeof(recvThdWa), NORMALPRIO+1, receivingThreadHelper, this);
+  return true;
 }
 
 void GpsInterface::set_state_callback(const GpsInterface::StateCallback &function) {
@@ -45,23 +54,59 @@ bool GpsInterface::send_raw(const void *data, size_t size) {
 }
 
 void GpsInterface::threadFunc() {
-  size_t bytes_to_process = 1;
+  // size_t bytes_to_process = 1;
   while(!stopped_) {
+    // Wait for data to arrive
+    chEvtWaitAny(ALL_EVENTS);
+    // Lock the processing buffer and process the data
+    chMtxLock(&processing_mutex);
     // try to read as much as possible in order to not interrupt on single bytes
-    size_t read = sizeof(gps_buffer);
-    uartReceiveTimeout(uart, &read, gps_buffer, TIME_MS2I(100));
-    size_t processed = 0;
-    while(processed < read) {
-      size_t in = etl::min(bytes_to_process, read-processed);
-      bytes_to_process = process_bytes(gps_buffer+processed, in);
-      processed+=in;
+    // size_t processed = 0;
+    // while(processed < processing_buffer_len) {
+      // const size_t in = etl::min(bytes_to_process, processing_buffer_len-processed);
+      // bytes_to_process = process_bytes(processing_buffer+processed, in);
+      // processed+=in;
+    // }
+    process_bytes(processing_buffer, processing_buffer_len);
+    chMtxUnlock(&processing_mutex);
+  }
+}
+
+
+void GpsInterface::receivingThreadFunc() {
+  while(!stopped_) {
+    // try to read as much as possible in order to not interrupt on single bytes
+    size_t read = RECV_BUFFER_SIZE;
+    // Get the pointer to the receiving buffer (it's not the processing buffer)
+    uint8_t* buffer = (processing_buffer == recv_buffer1) ? recv_buffer2 : recv_buffer1;
+    memset(buffer, 0, RECV_BUFFER_SIZE);
+    uartReceiveTimeout(uart, &read, buffer, TIME_S2I(1));
+    // swap buffers and instantly restart DMA
+    bool locked = chMtxTryLock(&processing_mutex);
+    if(!locked) {
+      ULOG_WARNING("GPS processing too slow to keep up with incoming data");
+      chMtxLock(&processing_mutex);
+    }
+    // Swap the buffers
+    processing_buffer = buffer;
+    processing_buffer_len = read;
+    chMtxUnlock(&processing_mutex);
+    // Signal the processing thread
+    if(processing_thread_) {
+      chEvtSignal(processing_thread_, 1);
     }
   }
 }
+
+
 void GpsInterface::threadHelper(void *instance) {
   auto *gps_interface = static_cast<GpsInterface *>(instance);
   gps_interface->threadFunc();
 }
+void GpsInterface::receivingThreadHelper(void *instance) {
+  auto *gps_interface = static_cast<GpsInterface *>(instance);
+  gps_interface->receivingThreadFunc();
+}
 
 void GpsInterface::send_rtcm(const uint8_t *data, size_t size) {
   send_raw(data, size);

src/drivers/gps/gps_interface.h

@@ -6,6 +6,7 @@
 #define OPEN_MOWER_ROS_GPS_INTERFACE_H
 
 #include <etl/delegate.h>
+
 #include "ch.h"
 #include "hal.h"
 
@@ -22,13 +23,7 @@ class GpsInterface {
    * The final GPS state we're interested in.
    */
   struct GpsState {
-    enum FixType {
-      NO_FIX = 0,
-      DR_ONLY = 1,
-      FIX_2D = 2,
-      FIX_3D = 3,
-      GNSS_DR_COMBINED = 4
-    };
+    enum FixType { NO_FIX = 0, DR_ONLY = 1, FIX_2D = 2, FIX_3D = 3, GNSS_DR_COMBINED = 4 };
 
     enum RTKType { RTK_NONE = 0, RTK_FLOAT = 1, RTK_FIX = 2 };
 
@@ -65,7 +60,7 @@ class GpsInterface {
   typedef etl::delegate<void(const GpsState &new_state)> StateCallback;
 
  public:
-  bool start_driver(UARTDriver* uart, uint32_t baudrate);
+  bool start_driver(UARTDriver *uart, uint32_t baudrate);
   void set_state_callback(const GpsInterface::StateCallback &function);
 
   void set_datum(double datum_lat, double datum_long, double datum_height);
@@ -94,21 +89,29 @@ class GpsInterface {
   virtual size_t process_bytes(const uint8_t *buffer, size_t len) = 0;
 
  private:
-  uint8_t gps_buffer[512]{};
-  uint8_t gps_buffer2[512]{};
- uint8_t current_buffer = 0;
-  size_t gps_buffer_count = 0;
-  size_t requested_bytes = 0;
+ static constexpr size_t RECV_BUFFER_SIZE=512;
+ // Keep two buffers for streaming data while doing processing
+  uint8_t recv_buffer1[RECV_BUFFER_SIZE]{};
+  uint8_t recv_buffer2[RECV_BUFFER_SIZE]{};
+  uint8_t* processing_buffer = recv_buffer1;
+  size_t processing_buffer_len = 0;
   UARTDriver *uart{};
- UARTConfig uart_config{};
+  UARTConfig uart_config{};
 
- THD_WORKING_AREA(wa, 1024);
+  THD_WORKING_AREA(wa, 1024){};
+  THD_WORKING_AREA(recvThdWa, 100){};
+  thread_t* processing_thread_ = nullptr;
+  // mutex held while processing data
+  // i.e. receiving thread cannot swap buffers when this is held
+  ch_mutex processing_mutex{};
 
   bool stopped_ = true;
 
- void threadFunc();
+  void threadFunc();
+  void receivingThreadFunc();
 
- static void threadHelper(void* instance);
+  static void threadHelper(void *instance);
+  static void receivingThreadHelper(void *instance);
 };
 }  // namespace gps
 }  // namespace driver