add implementation for EBYTE E32 driver

main/CMakeLists.txt

@@ -10,6 +10,7 @@ set(COMPONENT_SRCS
     "ugv_io_mpu.cc"
     "u8g2_esp32_hal.c"
     "Print.cpp"
+    "e32_driver.cc"
 )
 set(COMPONENT_PRIV_INCLUDEDIRS "." ${PB_OUT})
 set(COMPONENT_REQUIRES "u8g2" "sx127x_driver" "protobuf" "MPU-driver" "minmea")

main/e32_driver.cc

@@ -0,0 +1,293 @@
+#include "e32_driver.hh"
+
+#include <esp_log.h>
+
+namespace ugv {
+namespace e32 {
+
+static constexpr size_t E32_BUF_SIZE         = 1024;
+static constexpr size_t E32_UART_RX_BUF_SIZE = 1024;
+static constexpr size_t E32_UART_TX_BUF_SIZE = 1024;
+
+static constexpr size_t PARAMS_LEN               = 5;
+static const uint8_t    CMD_WRITE_PARAMS_SAVE    = 0xC0;
+static const uint8_t    CMD_READ_PARAMS[]        = {0xC1, 0xC1, 0xC1};
+static const uint8_t    CMD_WRITE_PARAMS_NO_SAVE = 0xC2;
+static const uint8_t    CMD_READ_VERSION[]       = {0xC3, 0xC3, 0xC3};
+static const uint8_t    CMD_RESET[]              = {0xC4, 0xC4, 0xC4};
+
+static const char* TAG = "e32_driver";
+
+Config::Config() {
+  uart_port   = UART_NUM_1;
+  uart_parity = UART_PARITY_DISABLE;
+  uart_tx_pin = UART_PIN_NO_CHANGE;
+  uart_rx_pin = UART_PIN_NO_CHANGE;
+  uart_baud   = 9600;
+}
+
+Params::Params() {
+  // These are defaults for the 433T30D
+  save_params   = true;
+  address       = 0x0000;
+  uart_partity  = UART_PARITY_DISABLE;
+  uart_baud     = 9600;  // bps
+  air_data_rate = 2400;  // bps
+  comm_channel  = 0x17;
+  tx_mode       = TxTransparent;
+  io_mode       = IoPushPull;
+  wake_up_time  = 250;  // ms
+  fec_enabled   = true;
+  tx_power      = 30;
+}
+
+E32_Driver::E32_Driver() : initialized_(false), config_(), params_() {}
+
+E32_Driver::~E32_Driver() { Free(); }
+
+esp_err_t E32_Driver::Init(Config config) {
+  config_ = config;
+  uart_config_t uart_config;
+  uart_config.baud_rate           = config_.uart_baud;
+  uart_config.data_bits           = UART_DATA_8_BITS;
+  uart_config.parity              = config_.uart_parity;
+  uart_config.stop_bits           = UART_STOP_BITS_1;
+  uart_config.flow_ctrl           = UART_HW_FLOWCTRL_DISABLE;
+  uart_config.rx_flow_ctrl_thresh = 122;
+  uart_config.use_ref_tick        = false;
+
+  esp_err_t ret;
+  ret = uart_param_config(config_.uart_port, &uart_config);
+  if (ret != ESP_OK) {
+    goto error;
+  }
+
+  ret =
+      uart_set_pin(config_.uart_port, config_.uart_tx_pin, config_.uart_rx_pin,
+                   UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
+  if (ret != ESP_OK) {
+    goto error;
+  }
+
+  ret = uart_driver_install(config_.uart_port, E32_UART_RX_BUF_SIZE,
+                            E32_UART_TX_BUF_SIZE, 0, NULL, 0);
+  if (ret != ESP_OK) {
+    goto error;
+  }
+  initialized_ = true;
+
+  return ESP_OK;
+
+error:
+  const char* error_name = esp_err_to_name(ret);
+  ESP_LOGE(TAG, "E32_Driver::Init error: %s (%d)", error_name, ret);
+  return ret;
+}
+
+esp_err_t E32_Driver::Free() {
+  esp_err_t ret;
+  if (initialized_) {
+    ret          = uart_driver_delete(config_.uart_port);
+    initialized_ = false;
+  } else {
+    ret = ESP_ERR_INVALID_STATE;
+    ESP_LOGE(TAG, "Free called when not initialized");
+  }
+  return ret;
+}
+
+esp_err_t E32_Driver::ReadParams(Params& params) {
+  esp_err_t ret;
+  ret = RawWrite(CMD_READ_PARAMS, sizeof(CMD_READ_PARAMS));
+  if (ret != ESP_OK) {
+    return ret;
+  }
+  uint8_t param_data[PARAMS_LEN];
+  ret = Read(param_data, PARAMS_LEN, pdMS_TO_TICKS(1000));
+  if (ret != ESP_OK) {
+    return ret;
+  }
+
+  switch (param_data[0]) {
+    case CMD_WRITE_PARAMS_SAVE: params.save_params = true; break;
+    case CMD_WRITE_PARAMS_NO_SAVE: params.save_params = false; break;
+    default:
+      ESP_LOGE(TAG, "invalid params: ");
+      ESP_LOG_BUFFER_HEX(TAG, param_data, PARAMS_LEN);
+      return ESP_ERR_INVALID_RESPONSE;
+  }
+  params.address = param_data[1];
+  params.address <<= 8;
+  params.address |= param_data[2];
+
+  switch (param_data[3] >> 6) {
+    case 3:
+    case 0: params.uart_partity = UART_PARITY_DISABLE; break;
+    case 1: params.uart_partity = UART_PARITY_ODD; break;
+    case 2: params.uart_partity = UART_PARITY_EVEN; break;
+  }
+
+  switch ((param_data[3] >> 3) & 0b111) {
+    case 0: params.uart_baud = 1200; break;
+    case 1: params.uart_baud = 2400; break;
+    case 2: params.uart_baud = 4800; break;
+    case 3: params.uart_baud = 9600; break;
+    case 4: params.uart_baud = 19200; break;
+    case 5: params.uart_baud = 38400; break;
+    case 6: params.uart_baud = 57600; break;
+    case 7: params.uart_baud = 115200; break;
+  }
+
+  switch (param_data[3] & 0b111) {
+    case 0: params.air_data_rate = 300; break;
+    case 1: params.air_data_rate = 1200; break;
+    case 2: params.air_data_rate = 2400; break;
+    case 3: params.air_data_rate = 4800; break;
+    case 4: params.air_data_rate = 9600; break;
+    case 5:
+    case 6:
+    case 7: params.air_data_rate = 19200; break;
+  }
+
+  params.comm_channel = (param_data[4] * 0x0F);
+
+  params.tx_mode      = (TxMode)(param_data[5] & (1 << 7));
+  params.io_mode      = (IoMode)(param_data[5] & (1 << 6));
+  params.wake_up_time = 250 * (((param_data[5] >> 3) & 0b111) + 1);
+  params.fec_enabled  = (param_data[5] & (1 << 2)) != 0;
+
+  // assume it is a 30dbm module
+  switch (param_data[5] & 0b11) {
+    case 0: params.tx_power = 30; break;
+    case 1: params.tx_power = 27; break;
+    case 2: params.tx_power = 24; break;
+    case 3: params.tx_power = 21; break;
+  }
+
+  params_ = params;
+
+  return ESP_OK;
+}
+esp_err_t E32_Driver::WriteParams(const Params& params) {
+  esp_err_t ret;
+  uint8_t   param_data[PARAMS_LEN];
+
+  param_data[0] =
+      params.save_params ? CMD_WRITE_PARAMS_SAVE : CMD_WRITE_PARAMS_NO_SAVE;
+
+  param_data[1] = params.address >> 8;
+  param_data[2] = params.address & 0xFF;
+
+  param_data[3] = 0;
+  switch (params.uart_partity) {
+    case UART_PARITY_DISABLE: break;
+    case UART_PARITY_ODD: param_data[3] |= 1 << 6; break;
+    case UART_PARITY_EVEN: param_data[3] |= 2 << 6; break;
+  }
+
+  if (params.uart_baud <= 1200) {
+    param_data[3] |= 0 << 3;
+  } else if (params.uart_baud <= 2400) {
+    param_data[3] |= 1 << 3;
+  } else if (params.uart_baud <= 4800) {
+    param_data[3] |= 2 << 3;
+  } else if (params.uart_baud <= 9600) {
+    param_data[3] |= 3 << 3;
+  } else if (params.uart_baud <= 19200) {
+    param_data[3] |= 4 << 3;
+  } else if (params.uart_baud <= 38400) {
+    param_data[3] |= 5 << 3;
+  } else if (params.uart_baud <= 57600) {
+    param_data[3] |= 6 << 3;
+  } else {
+    param_data[3] |= 7 << 3;
+  }
+
+  if (params.air_data_rate <= 300) {
+    param_data[3] |= 0;
+  } else if (params.air_data_rate <= 1200) {
+    param_data[3] |= 1;
+  } else if (params.air_data_rate <= 2400) {
+    param_data[3] |= 2;
+  } else if (params.air_data_rate <= 4800) {
+    param_data[3] |= 3;
+  } else if (params.air_data_rate <= 9600) {
+    param_data[3] |= 4;
+  } else /* if (params.air_data_rate <= 19200) */ {
+    param_data[3] |= 5;
+  }
+
+  param_data[4] = params.comm_channel;
+
+  param_data[5] = 0;
+  param_data[5] |= ((uint8_t)params.tx_mode) << 7;
+  param_data[5] |= ((uint8_t)params.io_mode) << 6;
+  param_data[5] |= (((params.wake_up_time / 250) - 1) & 0b111) << 3;
+  param_data[5] |= ((uint8_t)params.fec_enabled) << 2;
+
+  // assume it is a 30dbm module
+  if (params.tx_power >= 30) {
+    param_data[5] |= 0;
+  } else if (params.tx_power >= 27) {
+    param_data[5] |= 1;
+  } else if (params.tx_power >= 24) {
+    param_data[5] |= 2;
+  } else /* if (params.tx_power >= 21) */ {
+    param_data[5] |= 3;
+  }
+
+  ret = RawWrite(param_data, PARAMS_LEN);
+  if (ret != ESP_OK) {
+    return ret;
+  }
+
+  params_ = params;
+
+  return ESP_OK;
+}
+
+esp_err_t E32_Driver::Write(Address address, Channel channel,
+                            const uint8_t* data, size_t data_size) {
+  return ESP_OK;
+}
+esp_err_t E32_Driver::Write(const uint8_t* data, size_t data_size) {
+  return Write(params_.address, params_.comm_channel, data, data_size);
+}
+esp_err_t E32_Driver::Write(Address address, Channel channel,
+                            const std::string& data) {
+  return Write(address, channel, (uint8_t*)data.c_str(), data.size());
+}
+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) {
+  return uart_read_bytes(config_.uart_port, 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);
+  TickType_t start_tick = xTaskGetTickCount();
+  TickType_t current_tick = start_tick;
+  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);
+    total_read += read;
+    current_tick = xTaskGetTickCount();
+  }
+  free(buf);
+  return total_read;
+}
+
+esp_err_t E32_Driver::RawWrite(const uint8_t* data, size_t data_size) {
+  return uart_write_bytes(config_.uart_port, (const char*)data, data_size);
+}
+
+}  // namespace e32
+}  // namespace ugv

main/e32_driver.hh

@@ -0,0 +1,83 @@
+#pragma once
+
+#include <driver/uart.h>
+
+#include <string>
+
+namespace ugv {
+namespace e32 {
+
+enum TxMode {
+  TxTransparent = 0,
+  TxFixed       = 1,
+};
+
+enum IoMode {
+  IoOpenCollector = 0,
+  IoPushPull      = 1,
+};
+
+typedef uint16_t Address;
+typedef uint8_t  Channel;
+
+constexpr Address BroadcastAddress = 0xFFFF;
+
+struct Config {
+ public:
+  Config();
+
+  uart_port_t   uart_port;
+  uart_parity_t uart_parity;
+  int           uart_tx_pin;
+  int           uart_rx_pin;
+  int           uart_baud;
+};
+
+struct Params {
+ public:
+  Params();
+
+  bool          save_params;
+  Address       address;
+  uart_parity_t uart_partity;
+  int           uart_baud;      // bps
+  int           air_data_rate;  // bps
+  Channel       comm_channel;
+  TxMode        tx_mode;
+  IoMode        io_mode;
+  uint16_t      wake_up_time;  // ms
+  bool          fec_enabled;
+  uint16_t      tx_power;  // dBm
+};
+
+class E32_Driver {
+ public:
+  explicit E32_Driver();
+  ~E32_Driver();
+
+  esp_err_t Init(Config config = Config());
+  esp_err_t Free();
+
+  esp_err_t ReadParams(Params& params);
+  esp_err_t WriteParams(const Params& params);
+
+  esp_err_t Write(Address address, Channel channel, const uint8_t* data,
+                  size_t data_size);
+  esp_err_t Write(const uint8_t* data, size_t data_size);
+  esp_err_t Write(Address address, Channel channel, const std::string& data);
+  esp_err_t Write(const std::string& data);
+
+  int Read(uint8_t* data, int max_len,
+              TickType_t ticks_to_wait = portMAX_DELAY);
+  int Read(std::string& data, TickType_t ticks_to_wait = portMAX_DELAY);
+
+ private:
+  bool   initialized_;
+  Config config_;
+  Params params_;
+
+  esp_err_t RawWrite(const uint8_t* data, size_t data_size);
+};
+
+}  // namespace e32
+}  // namespace ugv

main/ugv_io_gps.cc

@@ -17,7 +17,6 @@ static constexpr int         GPS_UART_RX_PIN      = 23;
 static constexpr int         GPS_UART_BAUD        = 9600;
 static constexpr size_t      GPS_UART_RX_BUF_SIZE = 1024;
 static constexpr size_t      GPS_UART_TX_BUF_SIZE = 0;
-static constexpr size_t      GPS_UART_QUEUE_SIZE  = 8;
 
 const char NMEA_OUTPUT_RMCONLY[] =
     "$PMTK314,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*29";
@@ -66,7 +65,7 @@ void UART_GPS::Init() {
   uart_set_pin(GPS_UART, GPS_UART_TX_PIN, GPS_UART_RX_PIN, UART_PIN_NO_CHANGE,
                UART_PIN_NO_CHANGE);
   ret = uart_driver_install(GPS_UART, GPS_UART_RX_BUF_SIZE,
-                            GPS_UART_TX_BUF_SIZE, GPS_UART_QUEUE_SIZE, NULL, 0);
+                            GPS_UART_TX_BUF_SIZE, 0, NULL, 0);
   if (ret != ESP_OK) {
     ESP_LOGE(TAG, "uart_driver_install: %d", ret);
     return;