diff --git a/main/messages.proto b/main/messages.proto index 3afe58c..dfdb57a 100644 --- a/main/messages.proto +++ b/main/messages.proto @@ -1,6 +1,6 @@ syntax = "proto3"; -package uas.ugv; +package uas.ugv.messages; option optimize_for = LITE_RUNTIME; diff --git a/main/ugv_comms.cc b/main/ugv_comms.cc index 06b17cb..6697120 100644 --- a/main/ugv_comms.cc +++ b/main/ugv_comms.cc @@ -1,29 +1,30 @@ -#include "ugv_comms.h" +#include "ugv_comms.hh" #include "ugv_config.h" #include #include "messages.pb.h" +#include "sx127x_registers.h" -ugv_comms_state_t ugv_comms_state; +namespace ugv { +namespace comms { -static const char *TAG = "ugv_comms"; - -static void ugv_comms_task(void *arg); -static uint16_t packet_num; - -static void ugv_comms_task(void *arg); -static void ugv_comms_handle_packet(ugv_comms_state_t * st, - sx127x_rx_packet_t *rx_packet); -static void ugv_comms_handle_command(ugv_comms_state_t * st, - const uas::ugv::GroundCommand *command); +CommsClass Comms; -void ugv_comms_init() { - ugv_comms_state_t *st = &ugv_comms_state; +static const char *TAG = "ugv_comms"; - st->mutex = xSemaphoreCreateMutex(); +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(); +} - sx127x_config_t lora_config = sx127x_config_default(); +void CommsClass::Init() { + 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; - 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); + 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(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); +} + +void CommsClass::Lock(TickType_t ticks_to_wait) { + xSemaphoreTake(mutex, ticks_to_wait); } -static void ugv_comms_task(void *param) { - ugv_comms_state_t *st = (ugv_comms_state_t *)param; +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; - uas::ugv::UGV_Status *status = ugv_message.mutable_status(); - uas::ugv::Location * location = status->mutable_location(); + 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(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, - sx127x_rx_packet_t *rx_packet) { +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); - xSemaphoreTake(st->mutex, portMAX_DELAY); - st->last_packet_tick = xTaskGetTickCount(); - st->last_packet_rssi = rx_packet->rssi; - st->last_packet_snr = rx_packet->snr; - xSemaphoreGive(st->mutex); + xSemaphoreTake(mutex, portMAX_DELAY); + last_packet_tick = xTaskGetTickCount(); + last_packet_rssi = rx_packet->rssi; + last_packet_snr = rx_packet->snr; + xSemaphoreGive(mutex); - uas::ugv::GroundMessage ground_message; - bool pb_ret; + GroundMessage ground_message; - 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: - ugv_comms_handle_command(st, &ground_message.command()); + case GroundMessage::kCommand: + 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, - const uas::ugv::GroundCommand *command) { - ESP_LOGI(TAG, "rx command id %d type %d", command->id(), command->type()); +void CommsClass::HandleCommand(const messages::GroundCommand &command) { + ESP_LOGI(TAG, "rx command id %d type %d", command.id(), command.type()); // TODO: handle command - uas::ugv::UGV_Message ugv_message; - ugv_message.set_command_ack(command->id()); + messages::UGV_Message ugv_message; + ugv_message.set_command_ack(command.id()); std::string str = ugv_message.SerializeAsString(); - sx127x_send_packet(st->lora, str.c_str(), str.size(), 0); -} \ No newline at end of file + sx127x_send_packet(lora, str.c_str(), str.size(), 0); +} + +} // namespace comms +} // namespace ugv diff --git a/main/ugv_comms.h b/main/ugv_comms.h deleted file mode 100644 index 1c932c5..0000000 --- a/main/ugv_comms.h +++ /dev/null @@ -1,32 +0,0 @@ -#pragma once - -#include -#include -#include - -#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 diff --git a/main/ugv_comms.hh b/main/ugv_comms.hh new file mode 100644 index 0000000..ed9ee13 --- /dev/null +++ b/main/ugv_comms.hh @@ -0,0 +1,50 @@ +#pragma once + +#include +#include +#include + +#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 diff --git a/main/ugv_main.cc b/main/ugv_main.cc index d21cc02..75a770a 100644 --- a/main/ugv_main.cc +++ b/main/ugv_main.cc @@ -3,15 +3,14 @@ #include #include "U8g2lib.hh" -#include "sx127x_driver.h" -#include "sx127x_registers.h" -#include "ugv_comms.h" +#include "ugv_comms.hh" #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); - sx127x_read_lna_gain(ugv_comms_state.lora, &lora_lna_gain); - - xSemaphoreTake(ugv_comms_state.mutex, portMAX_DELAY); - last_packet_tick = ugv_comms_state.last_packet_tick; - last_packet_rssi = ugv_comms_state.last_packet_rssi; - last_packet_snr = ugv_comms_state.last_packet_snr; - xSemaphoreGive(ugv_comms_state.mutex); + lora_rssi = Comms.ReadRssi(); + lora_lna_gain = Comms.ReadLnaGain(); + + Comms.Lock(); + last_packet_tick = Comms.last_packet_tick; + last_packet_rssi = Comms.last_packet_rssi; + last_packet_snr = Comms.last_packet_snr; + Comms.Unlock(); do { oled->drawRFrame(0, 0, OLED_W, OLED_H, 4);