uas-ugv/main/ugv_comms.cc

#include "ugv_comms.hh"
#include "ugv_config.h"

#include <esp_log.h>

#include "messages.pb.h"

#ifdef COMMS_SX127X
#include "sx127x_registers.h"
#endif

namespace ugv {
namespace comms {

static const char *TAG = "ugv_comms";

CommsClass::CommsClass()
    : location(),
      ugv_state(messages::IDLE),
      last_packet_tick(0),
      last_packet_rssi(INT32_MIN),
      last_packet_snr(INT8_MIN),
      packet_num(0) {
  mutex = xSemaphoreCreateMutex();
}

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;
  lora_config.mosi_io_num      = (gpio_num_t)LORA_MOSI;
  lora_config.cs_io_num        = (gpio_num_t)LORA_CS;
  lora_config.rst_io_num       = (gpio_num_t)LORA_RST;
  lora_config.irq_io_num       = (gpio_num_t)LORA_IRQ;
  lora_config.frequency        = LORA_FREQ;
  lora_config.tx_power         = 17;
  lora_config.spreading_factor = 12;
  lora_config.signal_bandwidth = 10E3;
  lora_config.sync_word        = 0x34;
  lora_config.crc              = SX127X_CRC_ENABLED;

  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;
  }
  ret = sx127x_start(lora);
  if (ret != ESP_OK) {
    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 = 13;
  lora_config.uart_tx_pin = 15;
  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;

  xTaskCreate(CommsClass::CommsTask, "ugv_comms", 2 * 1024, this, 2,
              &task_handle);
}

void CommsClass::Lock(TickType_t ticks_to_wait) {
  xSemaphoreTake(mutex, ticks_to_wait);
}

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();
  vTaskDelete(NULL);
}

void CommsClass::RunTask() {
  using messages::Location;
  using messages::UGV_Message;
  using messages::UGV_State;
  using messages::UGV_Status;

  TickType_t send_period  = pdMS_TO_TICKS(2000);
  TickType_t current_tick = xTaskGetTickCount();
  TickType_t next_send    = current_tick + send_period;

  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();
  Location *  location = status->mutable_location();
  location->set_fix_quality(0);
  location->set_latitude(43.65);
  location->set_longitude(-116.20);
  location->set_altitude(2730);
  status->set_state(UGV_State::IDLE);

  while (true) {
    TickType_t delay_ticks = next_send - current_tick;

#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++;
    ret = SendPacket(ugv_message);
    if (ret != ESP_OK) {
      ESP_LOGE(TAG, "error sending packet: %d", ret);
    } else {
      ESP_LOGV(TAG, "lora wrote UGV_Message packet");
    }

    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) {
  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);

  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;

  GroundMessage ground_message;

  bool pb_ret = ground_message.ParseFromArray(data, data_size);
  if (!pb_ret) {
    ESP_LOGE(TAG, "rx invalid protobuf");
    return;
  }

  switch (ground_message.ground_message_case()) {
    case GroundMessage::kCommand:
      HandleCommand(ground_message.command());
      break;
    default:
      ESP_LOGE(TAG, "invalid ground message: %d",
               ground_message.ground_message_case());
      break;
  }
}

void CommsClass::HandleCommand(const messages::GroundCommand &command) {
  ESP_LOGI(TAG, "rx command id %d type %d", command.id(), command.type());
  // TODO: handle command

  messages::UGV_Message ugv_message;
  ugv_message.set_command_ack(command.id());

  SendPacket(ugv_message);
}

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));
  lora.Write((uint8_t *)data, size);
  return ESP_OK;
#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
}  // namespace ugv