More work on getting E32 lora to work

components/sx127x_driver/sx127x_driver.h

@@ -75,15 +75,15 @@ typedef struct sx127x_config {
 // clang-format on
 
 typedef struct sx127x_packet_rx {
-  char *  data;
+  uint8_t *data;
-  size_t  data_len;
+  size_t   data_len;
   // rssi value
   int32_t rssi;
   // snr value in steps of .25
-  int8_t  snr;
+  int8_t snr;
 } sx127x_rx_packet_t;
 
-typedef struct sx127x* sx127x_hndl;
+typedef struct sx127x *sx127x_hndl;
 
 sx127x_config_t sx127x_config_default();
 

main/e32_driver.cc

@@ -76,6 +76,11 @@ esp_err_t E32_Driver::Init(Config config) {
   }
   initialized_ = true;
 
+  ReadParams(params_);
+  if (ret != ESP_OK) {
+    goto error;
+  }
+
   return ESP_OK;
 
 error:
@@ -248,7 +253,27 @@ esp_err_t E32_Driver::WriteParams(const Params& params) {
 
 esp_err_t E32_Driver::Write(Address address, Channel channel,
                             const uint8_t* data, size_t data_size) {
+  esp_err_t ret;
+  if (params_.tx_mode == TxFixed) {
+    uint8_t header[3];
+    header[0] = address >> 8;
+    header[1] = address & 0xFF;
+    header[2] = channel;
+
+    ret = uart_write_bytes(config_.uart_port, (char*)header, sizeof(header));
+    if (ret != ESP_OK) {
+      goto error;
+    }
+  }
+  ret = uart_write_bytes(config_.uart_port, (char*)data, data_size);
+  if (ret != ESP_OK) {
+    goto error;
+  }
   return ESP_OK;
+
+error:
+  ESP_LOGE(TAG, "Write error %d", ret);
+  return ret;
 }
 esp_err_t E32_Driver::Write(const uint8_t* data, size_t data_size) {
   return Write(params_.address, params_.comm_channel, data, data_size);
@@ -261,23 +286,24 @@ esp_err_t E32_Driver::Write(const std::string& data) {
   return Write((uint8_t*)data.c_str(), data.size());
 }
 
-int E32_Driver::Read(uint8_t *data, int max_len, TickType_t ticks_to_wait) {
+int E32_Driver::Read(uint8_t* data, int max_len, TickType_t ticks_to_wait) {
-  return uart_read_bytes(config_.uart_port, data, max_len, ticks_to_wait);
+  return uart_read_bytes(config_.uart_port, (uint8_t*)data, max_len,
+                         ticks_to_wait);
 }
 
 int E32_Driver::Read(std::string& data, TickType_t ticks_to_wait) {
   data.clear();
-  uint8_t* buf = (uint8_t*) malloc(128);
+  uint8_t*   buf          = (uint8_t*)malloc(128);
-  TickType_t start_tick = xTaskGetTickCount();
+  TickType_t start_tick   = xTaskGetTickCount();
   TickType_t current_tick = start_tick;
-  int read, total_read = 0;
+  int        read, total_read = 0;
   while (current_tick <= start_tick + ticks_to_wait) {
     read = Read(buf, 128, ticks_to_wait - (current_tick - start_tick));
     if (read < 0) {
       free(buf);
       return read;
     }
-    data.append((const char*) buf, read);
+    data.append((const char*)buf, read);
     total_read += read;
     current_tick = xTaskGetTickCount();
   }

main/e32_driver.hh

@@ -68,7 +68,7 @@ class E32_Driver {
   esp_err_t Write(const std::string& data);
 
   int Read(uint8_t* data, int max_len,
-              TickType_t ticks_to_wait = portMAX_DELAY);
+           TickType_t ticks_to_wait = portMAX_DELAY);
   int Read(std::string& data, TickType_t ticks_to_wait = portMAX_DELAY);
 
  private:

main/ugv_comms.cc

@@ -4,7 +4,10 @@
 #include <esp_log.h>
 
 #include "messages.pb.h"
+
+#ifdef COMMS_SX127X
 #include "sx127x_registers.h"
+#endif
 
 namespace ugv {
 namespace comms {
@@ -24,6 +27,9 @@ CommsClass::CommsClass()
 }
 
 void CommsClass::Init() {
+  esp_err_t ret;
+
+#ifdef COMMS_SX127X
   sx127x_config_t lora_config  = sx127x_config_default();
   lora_config.sck_io_num       = (gpio_num_t)LORA_SCK;
   lora_config.miso_io_num      = (gpio_num_t)LORA_MISO;
@@ -38,20 +44,31 @@ void CommsClass::Init() {
   lora_config.sync_word        = 0x34;
   lora_config.crc              = SX127X_CRC_ENABLED;
 
-  esp_err_t ret;
-
   ret = sx127x_init(&lora_config, &lora);
   if (ret != ESP_OK) {
     const char *err_name = esp_err_to_name(ret);
     ESP_LOGE(TAG, "LoRa init failed: %s", err_name);
     return;
   }
-  ESP_LOGI(TAG, "LoRa initialized");
   ret = sx127x_start(lora);
   if (ret != ESP_OK) {
-    ESP_LOGI(TAG, "LoRa start failed: %d", ret);
+    ESP_LOGE(TAG, "LoRa start failed: %d", ret);
+    return;
+  }
+  ESP_LOGI(TAG, "LoRa initialized");
+#else
+  e32::Config lora_config;
+  lora_config.uart_port   = UART_NUM_2;
+  lora_config.uart_rx_pin = 10;
+  lora_config.uart_rx_pin = 11;
+  ret                     = lora.Init(lora_config);
+  if (ret != ESP_OK) {
+    const char *error_name = esp_err_to_name(ret);
+    ESP_LOGE(TAG, "E32 LoRa Init failed: %s (%d)", error_name, ret);
     return;
   }
+  packet_len                  = -1;
+#endif
 
   packet_num = 0;
 
@@ -67,17 +84,28 @@ void CommsClass::Unlock() { xSemaphoreGive(mutex); }
 
 int32_t CommsClass::ReadRssi() {
   int32_t rssi;
+#ifdef COMMS_SX127X
   sx127x_read_rssi(lora, &rssi);
+#else
+  rssi                        = 0;
+#endif
   return rssi;
 }
 
 uint8_t CommsClass::ReadLnaGain() {
   uint8_t lna_gain;
+#ifdef COMMS_SX127X
   sx127x_read_lna_gain(lora, &lna_gain);
+#else
+  lna_gain                    = 0;
+#endif
   return lna_gain;
 }
 
-void CommsClass::CommsTask(void *arg) { ((CommsClass *)arg)->RunTask(); }
+void CommsClass::CommsTask(void *arg) {
+  ((CommsClass *)arg)->RunTask();
+  vTaskDelete(NULL);
+}
 
 void CommsClass::RunTask() {
   using messages::Location;
@@ -89,8 +117,16 @@ void CommsClass::RunTask() {
   TickType_t current_tick = xTaskGetTickCount();
   TickType_t next_send    = current_tick + send_period;
 
-  esp_err_t          ret;
+  esp_err_t ret;
+
+#ifdef COMMS_SX127X
   sx127x_rx_packet_t rx_packet;
+#else
+  static const int rx_buf_len = MAX_PACKET_LEN * 2;
+
+  uint8_t *rx_buf = new uint8_t[rx_buf_len];
+  int      rx_len;
+#endif
 
   UGV_Message ugv_message;
   UGV_Status *status   = ugv_message.mutable_status();
@@ -103,26 +139,31 @@ void CommsClass::RunTask() {
 
   while (true) {
     TickType_t delay_ticks = next_send - current_tick;
-    ret                    = sx127x_recv_packet(lora, &rx_packet, delay_ticks);
+
+#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);
+#endif
 
     if (current_tick < next_send) {
       continue;
     }
 
     packet_num++;
-    auto str = ugv_message.SerializeAsString();
+    ret = SendPacket(ugv_message);
-    ret      = sx127x_send_packet(lora, str.c_str(), str.size(),
-                             0);  // 0 means error if queue full
-    // ret = sx127x_send_packet_pb(st->lora, uas_ugv_UGV_Message_fields,
-    //                             &ugv_message,
-    //                             0);  // 0 means error if queue full
     if (ret != ESP_OK) {
       ESP_LOGE(TAG, "error sending packet: %d", ret);
     } else {
@@ -132,11 +173,13 @@ void CommsClass::RunTask() {
     current_tick = xTaskGetTickCount();
     next_send    = current_tick + send_period;
   }
+#ifndef COMMS_SX127X
+  delete[] rx_buf;
+#endif
 }
 
+#ifdef COMMS_SX127X
 void CommsClass::HandlePacket(sx127x_rx_packet_t *rx_packet) {
-  using messages::GroundMessage;
-
   ESP_LOGI(TAG, "lora received packet (len %d, rssi: %d, snr: %f): %.*s\n",
            rx_packet->data_len, rx_packet->rssi, rx_packet->snr * 0.25f,
            rx_packet->data_len, rx_packet->data);
@@ -147,10 +190,40 @@ void CommsClass::HandlePacket(sx127x_rx_packet_t *rx_packet) {
   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;
+
   GroundMessage ground_message;
 
-  bool pb_ret =
+  bool pb_ret = ground_message.ParseFromArray(data, data_size);
-      ground_message.ParseFromArray(rx_packet->data, rx_packet->data_len);
   if (!pb_ret) {
     ESP_LOGE(TAG, "rx invalid protobuf");
     return;
@@ -174,8 +247,30 @@ void CommsClass::HandleCommand(const messages::GroundCommand &command) {
   messages::UGV_Message ugv_message;
   ugv_message.set_command_ack(command.id());
 
-  std::string str = ugv_message.SerializeAsString();
+  SendPacket(ugv_message);
-  sx127x_send_packet(lora, str.c_str(), str.size(), 0);
+}
+
+esp_err_t CommsClass::SendPacket(const char *data, size_t size) {
+#ifdef COMMS_SX127X
+  return sx127x_send_packet(lora, data, size, 0);
+#else
+  if (size >= MAX_PACKET_LEN) {
+    ESP_LOGE(TAG, "SendPacket size too long: %d", size);
+    return ESP_ERR_INVALID_SIZE;
+  }
+  uint8_t sz = size;
+  lora.Write(&sz, sizeof(sz));
+  return lora.Write((uint8_t *)data, size);
+#endif
+}
+
+esp_err_t CommsClass::SendPacket(const std::string &str) {
+  return SendPacket(str.c_str(), str.size());
+}
+
+esp_err_t CommsClass::SendPacket(const google::protobuf::MessageLite &message) {
+  std::string str = message.SerializeAsString();
+  return SendPacket(str);
 }
 
 }  // namespace comms

main/ugv_comms.hh

@@ -5,7 +5,12 @@
 #include <freertos/task.h>
 
 #include "messages.pb.h"
+
+#ifdef COMMS_SX127X
 #include "sx127x_driver.h"
+#else
+#include "e32_driver.hh"
+#endif
 
 namespace ugv {
 namespace comms {
@@ -14,6 +19,8 @@ namespace messages = uas::ugv::messages;
 
 class CommsClass {
  public:
+  static constexpr int MAX_PACKET_LEN = 255;
+
   CommsClass();
 
   void Init();
@@ -33,14 +40,32 @@ class CommsClass {
   int8_t              last_packet_snr;
 
  private:
-  sx127x_hndl  lora;
+#ifdef COMMS_SX127X
+  sx127x_hndl lora;
+#else
+  e32::E32_Driver lora;
+  int             packet_len;
+  std::string     packet_buf;
+#endif
   TaskHandle_t task_handle;
   uint16_t     packet_num;
 
   void RunTask();
+
+#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);
   void HandleCommand(const messages::GroundCommand& command);
 
+  esp_err_t SendPacket(const char* data, size_t size);
+  esp_err_t SendPacket(const std::string& str);
+  esp_err_t SendPacket(const google::protobuf::MessageLite& message);
+
   static void CommsTask(void* arg);
 };