Reimplement comms with c++

main/messages.proto

@@ -1,6 +1,6 @@
 syntax = "proto3";
 
-package uas.ugv;
+package uas.ugv.messages;
 
 option optimize_for = LITE_RUNTIME;
 

main/ugv_comms.cc

@@ -1,29 +1,30 @@
-#include "ugv_comms.h"
+#include "ugv_comms.hh"
 #include "ugv_config.h"
 
 #include <esp_log.h>
 
 #include "messages.pb.h"
+#include "sx127x_registers.h"
 
-ugv_comms_state_t ugv_comms_state;
+namespace ugv {
+namespace comms {
+
+CommsClass Comms;
 
 static const char *TAG = "ugv_comms";
 
-static void     ugv_comms_task(void *arg);
+CommsClass::CommsClass()
-static uint16_t packet_num;
+    : location(),
+      ugv_state(messages::IDLE),
+      last_packet_tick(0),
+      last_packet_rssi(INT32_MIN),
+      last_packet_snr(INT8_MIN),
+      packet_num(0) {
+  mutex = xSemaphoreCreateMutex();
+}
 
-static void ugv_comms_task(void *arg);
+void CommsClass::Init() {
-static void ugv_comms_handle_packet(ugv_comms_state_t * st,
+  sx127x_config_t lora_config  = sx127x_config_default();
-                                    sx127x_rx_packet_t *rx_packet);
-static void ugv_comms_handle_command(ugv_comms_state_t *      st,
-                                     const uas::ugv::GroundCommand *command);
-
-void ugv_comms_init() {
-  ugv_comms_state_t *st = &ugv_comms_state;
-
-  st->mutex = xSemaphoreCreateMutex();
-
-  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;
@@ -39,21 +40,14 @@ void ugv_comms_init() {
 
   esp_err_t ret;
 
-  uas::ugv::Location loc;
+  ret = sx127x_init(&lora_config, &lora);
-  memcpy(&st->location, &loc, sizeof(loc));
-  st->ugv_state        = uas::ugv::UGV_State::IDLE;
-  st->last_packet_tick = 0;
-  st->last_packet_rssi = INT32_MIN;
-  st->last_packet_snr  = INT8_MIN;
-
-  ret = sx127x_init(&lora_config, &st->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(st->lora);
+  ret = sx127x_start(lora);
   if (ret != ESP_OK) {
     ESP_LOGI(TAG, "LoRa start failed: %d", ret);
     return;
@@ -61,11 +55,35 @@ void ugv_comms_init() {
 
   packet_num = 0;
 
-  xTaskCreate(ugv_comms_task, "ugv_comms", 2 * 1024, st, 2, &st->task_handle);
+  xTaskCreate(CommsClass::CommsTask, "ugv_comms", 2 * 1024, this, 2,
+              &task_handle);
 }
 
-static void ugv_comms_task(void *param) {
+void CommsClass::Lock(TickType_t ticks_to_wait) {
-  ugv_comms_state_t *st = (ugv_comms_state_t *)param;
+  xSemaphoreTake(mutex, ticks_to_wait);
+}
+
+void CommsClass::Unlock() { xSemaphoreGive(mutex); }
+
+int32_t CommsClass::ReadRssi() {
+  int32_t rssi;
+  sx127x_read_rssi(lora, &rssi);
+  return rssi;
+}
+
+uint8_t CommsClass::ReadLnaGain() {
+  uint8_t lna_gain;
+  sx127x_read_lna_gain(lora, &lna_gain);
+  return lna_gain;
+}
+
+void CommsClass::CommsTask(void *arg) { ((CommsClass *)arg)->RunTask(); }
+
+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();
@@ -74,22 +92,22 @@ static void ugv_comms_task(void *param) {
   esp_err_t          ret;
   sx127x_rx_packet_t rx_packet;
 
-  uas::ugv::UGV_Message ugv_message;
+  UGV_Message ugv_message;
-  uas::ugv::UGV_Status *status   = ugv_message.mutable_status();
+  UGV_Status *status   = ugv_message.mutable_status();
-  uas::ugv::Location *  location = status->mutable_location();
+  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(uas::ugv::UGV_State::IDLE);
+  status->set_state(UGV_State::IDLE);
 
   while (true) {
     TickType_t delay_ticks = next_send - current_tick;
-    ret = sx127x_recv_packet(st->lora, &rx_packet, delay_ticks);
+    ret                    = sx127x_recv_packet(lora, &rx_packet, delay_ticks);
 
     current_tick = xTaskGetTickCount();
     if (ret == ESP_OK) {
-      ugv_comms_handle_packet(st, &rx_packet);
+      HandlePacket(&rx_packet);
 
       sx127x_packet_rx_free(&rx_packet);
     }
@@ -100,7 +118,7 @@ static void ugv_comms_task(void *param) {
 
     packet_num++;
     auto str = ugv_message.SerializeAsString();
-    ret      = sx127x_send_packet(st->lora, str.c_str(), str.size(),
+    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,
@@ -116,30 +134,31 @@ static void ugv_comms_task(void *param) {
   }
 }
 
-static void ugv_comms_handle_packet(ugv_comms_state_t * st,
+void CommsClass::HandlePacket(sx127x_rx_packet_t *rx_packet) {
-                                    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);
 
-  xSemaphoreTake(st->mutex, portMAX_DELAY);
+  xSemaphoreTake(mutex, portMAX_DELAY);
-  st->last_packet_tick = xTaskGetTickCount();
+  last_packet_tick = xTaskGetTickCount();
-  st->last_packet_rssi = rx_packet->rssi;
+  last_packet_rssi = rx_packet->rssi;
-  st->last_packet_snr  = rx_packet->snr;
+  last_packet_snr  = rx_packet->snr;
-  xSemaphoreGive(st->mutex);
+  xSemaphoreGive(mutex);
 
-  uas::ugv::GroundMessage ground_message;
+  GroundMessage ground_message;
-  bool                    pb_ret;
 
-  pb_ret = ground_message.ParseFromArray(rx_packet->data, rx_packet->data_len);
+  bool pb_ret =
+      ground_message.ParseFromArray(rx_packet->data, rx_packet->data_len);
   if (!pb_ret) {
     ESP_LOGE(TAG, "rx invalid protobuf");
     return;
   }
 
   switch (ground_message.ground_message_case()) {
-    case uas::ugv::GroundMessage::kCommand:
+    case GroundMessage::kCommand:
-      ugv_comms_handle_command(st, &ground_message.command());
+      HandleCommand(ground_message.command());
       break;
     default:
       ESP_LOGE(TAG, "invalid ground message: %d",
@@ -148,14 +167,16 @@ static void ugv_comms_handle_packet(ugv_comms_state_t * st,
   }
 }
 
-static void ugv_comms_handle_command(ugv_comms_state_t *            st,
+void CommsClass::HandleCommand(const messages::GroundCommand &command) {
-                                     const uas::ugv::GroundCommand *command) {
+  ESP_LOGI(TAG, "rx command id %d type %d", command.id(), command.type());
-  ESP_LOGI(TAG, "rx command id %d type %d", command->id(), command->type());
   // TODO: handle command
 
-  uas::ugv::UGV_Message ugv_message;
+  messages::UGV_Message ugv_message;
-  ugv_message.set_command_ack(command->id());
+  ugv_message.set_command_ack(command.id());
 
   std::string str = ugv_message.SerializeAsString();
-  sx127x_send_packet(st->lora, str.c_str(), str.size(), 0);
+  sx127x_send_packet(lora, str.c_str(), str.size(), 0);
-}
+}
+
+}  // namespace comms
+}  // namespace ugv

main/ugv_comms.h

@@ -1,32 +0,0 @@
-#pragma once
-
-#include <freertos/FreeRTOS.h>
-#include <freertos/semphr.h>
-#include <freertos/task.h>
-
-#include "messages.pb.h"
-#include "sx127x_driver.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-typedef struct ugv_comms_state_s {
-  sx127x_hndl  lora;
-  TaskHandle_t task_handle;
-
-  SemaphoreHandle_t   mutex;
-  uas::ugv::Location  location;
-  uas::ugv::UGV_State ugv_state;
-  TickType_t          last_packet_tick;
-  int32_t             last_packet_rssi;
-  int8_t              last_packet_snr;
-} ugv_comms_state_t;
-
-extern ugv_comms_state_t ugv_comms_state;
-
-void ugv_comms_init();
-
-#ifdef __cplusplus
-}
-#endif

main/ugv_comms.hh

@@ -0,0 +1,50 @@
+#pragma once
+
+#include <freertos/FreeRTOS.h>
+#include <freertos/semphr.h>
+#include <freertos/task.h>
+
+#include "messages.pb.h"
+#include "sx127x_driver.h"
+
+namespace ugv {
+namespace comms {
+
+namespace messages = uas::ugv::messages;
+
+class CommsClass {
+ public:
+  CommsClass();
+
+  void Init();
+
+  void Lock(TickType_t ticks_to_wait = pdMS_TO_TICKS(1000));
+  void Unlock();
+
+  int32_t ReadRssi();
+  uint8_t ReadLnaGain();
+
+ public:
+  SemaphoreHandle_t   mutex;
+  messages::Location  location;
+  messages::UGV_State ugv_state;
+  TickType_t          last_packet_tick;
+  int32_t             last_packet_rssi;
+  int8_t              last_packet_snr;
+
+ private:
+  sx127x_hndl  lora;
+  TaskHandle_t task_handle;
+  uint16_t     packet_num;
+
+  void RunTask();
+  void HandlePacket(sx127x_rx_packet_t* packet);
+  void HandleCommand(const messages::GroundCommand& command);
+
+  static void CommsTask(void* arg);
+};
+
+extern CommsClass Comms;
+
+}  // namespace comms
+}  // namespace ugv

main/ugv_main.cc

@@ -3,15 +3,14 @@
 #include <u8g2.h>
 
 #include "U8g2lib.hh"
-#include "sx127x_driver.h"
+#include "ugv_comms.hh"
-#include "sx127x_registers.h"
-#include "ugv_comms.h"
 #include "ugv_config.h"
 #include "ugv_io.hh"
 
 namespace ugv {
 
 using ugv::io::IO;
+using ugv::comms::Comms;
 
 static const char *TAG = "ugv_main";
 
@@ -28,7 +27,7 @@ void setup(void) {
   ESP_LOGI(TAG, "setup");
 
   setup_oled();
-  ugv_comms_init();
+  Comms.Init();
   IO.Init();
 }
 
@@ -48,14 +47,14 @@ void loop(void) {
   // ESP_LOGI(TAG, "inputs %s", inputs.ToString());
 
   oled->firstPage();
-  sx127x_read_rssi(ugv_comms_state.lora, &lora_rssi);
+  lora_rssi = Comms.ReadRssi();
-  sx127x_read_lna_gain(ugv_comms_state.lora, &lora_lna_gain);
+  lora_lna_gain = Comms.ReadLnaGain();
 
-  xSemaphoreTake(ugv_comms_state.mutex, portMAX_DELAY);
+  Comms.Lock();
-  last_packet_tick = ugv_comms_state.last_packet_tick;
+  last_packet_tick = Comms.last_packet_tick;
-  last_packet_rssi = ugv_comms_state.last_packet_rssi;
+  last_packet_rssi = Comms.last_packet_rssi;
-  last_packet_snr  = ugv_comms_state.last_packet_snr;
+  last_packet_snr  = Comms.last_packet_snr;
-  xSemaphoreGive(ugv_comms_state.mutex);
+  Comms.Unlock();
   do {
     oled->drawRFrame(0, 0, OLED_W, OLED_H, 4);