many fixes to ugv comms code

main/e32_driver.cc

@@ -76,10 +76,10 @@ esp_err_t E32_Driver::Init(Config config) {
   }
   initialized_ = true;
 
-  ReadParams(params_);
-  if (ret != ESP_OK) {
-    goto error;
-  }
+  // ReadParams(params_);
+  // if (ret != ESP_OK) {
+    // goto error;
+  // }
 
   return ESP_OK;
 
@@ -313,6 +313,10 @@ int E32_Driver::Read(std::string& data, TickType_t ticks_to_wait) {
   return total_read;
 }
 
+void E32_Driver::Flush() {
+  uart_flush(config_.uart_port);
+}
+
 int E32_Driver::RawWrite(const uint8_t* data, size_t data_size) {
   int written =
       uart_write_bytes(config_.uart_port, (const char*)data, data_size);

main/e32_driver.hh

@@ -73,6 +73,8 @@ class E32_Driver {
            TickType_t ticks_to_wait = portMAX_DELAY);
   int Read(std::string& data, TickType_t ticks_to_wait = portMAX_DELAY);
 
+  void Flush();
+
  private:
   bool   initialized_;
   Config config_;

main/ugv_comms.cc

@@ -138,22 +138,39 @@ void CommsClass::RunTask() {
 
 #ifdef COMMS_SX127X
     ret = sx127x_recv_packet(lora, &rx_packet, delay_ticks);
-#else
-    rx_len = lora.Read(rx_buf, rx_buf_len, delay_ticks);
-#endif
-
-    current_tick = xTaskGetTickCount();
-
-#ifdef COMMS_SX127X
     if (ret == ESP_OK) {
       HandlePacket(&rx_packet);
 
       sx127x_packet_rx_free(&rx_packet);
     }
 #else
-    HandleRxData(rx_buf, rx_len);
+    if (packet_len <= 0) { // need to receive packet size first
+      rx_len = lora.Read(rx_buf, 1, delay_ticks);
+      if (rx_len > 0) {
+        packet_len = rx_buf[0];
+        if (packet_len > MAX_PACKET_LEN) { // should not be possible
+          ESP_LOGE(TAG, "invalid packet len received: %d", packet_len);
+          packet_len = -1;
+        } else {
+          ESP_LOGI(TAG, "rx packet len: %d", packet_len);
+        }
+      }
+    } else { // receiving packet data now
+      rx_len = lora.Read(rx_buf, packet_len, PACKET_RX_TIMEOUT);
+      if (rx_len < packet_len) {
+        ESP_LOGE(TAG, "timeout for packet rx");
+        lora.Flush();
+      } else {
+        ESP_LOGI(TAG, "rx data: %.*s", packet_len, rx_buf);
+        HandlePacket(rx_buf, packet_len);
+      }
+      packet_len = -1; // wait for new packet len
+    }
+    // TODO: checksum?
 #endif
 
+    current_tick = xTaskGetTickCount();
+
     if (current_tick < next_send) {
       continue;
     }
@@ -185,42 +202,21 @@ void CommsClass::HandlePacket(sx127x_rx_packet_t *rx_packet) {
            rx_packet->data_len, rx_packet->data);
 
   xSemaphoreTake(mutex, portMAX_DELAY);
-  last_packet_tick = xTaskGetTickCount();
   last_packet_rssi = rx_packet->rssi;
   last_packet_snr  = rx_packet->snr;
   xSemaphoreGive(mutex);
 
   HandlePacket(rx_packet->data, rx_packet->data_len);
 }
-#else
-void CommsClass::HandleRxData(const uint8_t *data, int size) {
-  if (size <= 0) {
-    return;
-  }
-  int i = 0;
-  while (i < size) {
-    if (packet_len <= 0) {
-      packet_len = data[i++];
-      packet_buf.clear();
-    } else {
-      int remaining = packet_len - packet_buf.size();
-      int available = size - i;
-      int read = (remaining > available) ? available : remaining;
-      packet_buf.append((const char *)&data[i], read);
-      i += read;
-      if (packet_buf.size() >= packet_len) {
-        HandlePacket((uint8_t *)packet_buf.c_str(), packet_buf.size());
-        packet_buf.clear();
-        packet_len = -1;
-      }
-    }
-  }
-}
 #endif
 
 void CommsClass::HandlePacket(const uint8_t *data, size_t data_size) {
   using messages::GroundMessage;
 
+  xSemaphoreTake(mutex, pdMS_TO_TICKS(10));
+  last_packet_tick = xTaskGetTickCount();
+  xSemaphoreGive(mutex);
+
   GroundMessage ground_message;
 
   bool pb_ret = ground_message.ParseFromArray(data, data_size);

main/ugv_comms.hh

@@ -20,6 +20,7 @@ namespace messages = uas::ugv::messages;
 class CommsClass {
  public:
   static constexpr int MAX_PACKET_LEN = 255;
+  static constexpr TickType_t PACKET_RX_TIMEOUT = pdMS_TO_TICKS(200);
 
   CommsClass();
 
@@ -45,7 +46,6 @@ class CommsClass {
 #else
   e32::E32_Driver lora;
   int             packet_len;
-  std::string     packet_buf;
 #endif
   TaskHandle_t task_handle;
 
@@ -53,9 +53,6 @@ class CommsClass {
 
 #ifdef COMMS_SX127X
   void HandlePacket(sx127x_rx_packet_t* packet);
-#else
-
-  void HandleRxData(const uint8_t* data, int size);
 #endif
 
   void HandlePacket(const uint8_t* data, size_t size);