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::STATE_IDLE),
      last_packet_tick(0),
      last_packet_rssi(INT32_MIN),
      last_packet_snr(INT8_MIN) {
  mutex = xSemaphoreCreateMutex();
}

void CommsClass::Init() {
  esp_err_t ret;

  location.set_fix_quality(0);
  location.set_latitude(43.65);
  location.set_longitude(-116.20);
  location.set_altitude(2730);
  ugv_state = messages::UGV_State::STATE_IDLE;

#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   = LORA_UART;
  lora_config.uart_rx_pin = LORA_RX;
  lora_config.uart_tx_pin = LORA_TX;
  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

  xTaskCreate(CommsClass::CommsTask, "ugv_comms", 2 * 1024, this, 2,
              &task_handle);
  ESP_LOGD(TAG, "UGV comms started");
}

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;
  std::string ugv_message_data;
  UGV_Status *status = ugv_message.mutable_status();

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

#ifdef COMMS_SX127X
    ret = sx127x_recv_packet(lora, &rx_packet, delay_ticks);
    if (ret == ESP_OK) {
      HandlePacket(&rx_packet);

      sx127x_packet_rx_free(&rx_packet);
    }
#else
    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_LOGV(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_LOGV(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;
    }

    Lock();
    status->mutable_location()->CopyFrom(this->location);
    status->set_state(this->ugv_state);
    Unlock();
    ugv_message.SerializeToString(&ugv_message_data);
    ret = SendPacket(ugv_message_data);
    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_rssi = rx_packet->rssi;
  last_packet_snr  = rx_packet->snr;
  xSemaphoreGive(mutex);

  HandlePacket(rx_packet->data, rx_packet->data_len);
}
#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);
  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());

  xSemaphoreTake(mutex, pdMS_TO_TICKS(10));
  switch (command.type()) {
    case messages::CMD_DISABLE: ugv_state = messages::STATE_IDLE; break;
    case messages::CMD_DRIVE_TO_TARGET:
      ugv_state = messages::STATE_AQUIRING;
      break;
    case messages::CMD_TEST: ugv_state = messages::STATE_TEST; break;
    default:
      ESP_LOGW(TAG, "unhandled command type: %d", command.type());
      xSemaphoreGive(mutex);
      return;  // early return, no ack
  }
  xSemaphoreGive(mutex);

  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