diff --git a/main/ugv_comms.cc b/main/ugv_comms.cc index 2686d1e..f59ab8c 100644 --- a/main/ugv_comms.cc +++ b/main/ugv_comms.cc @@ -19,14 +19,19 @@ CommsClass::CommsClass() ugv_state(messages::IDLE), last_packet_tick(0), last_packet_rssi(INT32_MIN), - last_packet_snr(INT8_MIN), - packet_num(0) { + 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::IDLE; + #ifdef COMMS_SX127X sx127x_config_t lora_config = sx127x_config_default(); lora_config.sck_io_num = (gpio_num_t)LORA_SCK; @@ -68,8 +73,6 @@ void CommsClass::Init() { packet_len = -1; #endif - packet_num = 0; - xTaskCreate(CommsClass::CommsTask, "ugv_comms", 2 * 1024, this, 2, &task_handle); } @@ -127,13 +130,8 @@ void CommsClass::RunTask() { #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); + std::string ugv_message_data; + UGV_Status *status = ugv_message.mutable_status(); while (true) { TickType_t delay_ticks = next_send - current_tick; @@ -160,8 +158,12 @@ void CommsClass::RunTask() { continue; } - packet_num++; - ret = SendPacket(ugv_message); + 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 { diff --git a/main/ugv_comms.hh b/main/ugv_comms.hh index 431835e..436c160 100644 --- a/main/ugv_comms.hh +++ b/main/ugv_comms.hh @@ -48,7 +48,6 @@ class CommsClass { std::string packet_buf; #endif TaskHandle_t task_handle; - uint16_t packet_num; void RunTask();