some ugv comms improvements

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),
+      last_packet_snr(INT8_MIN) {
-      packet_num(0) {
   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();
+  std::string ugv_message_data;
-  Location *  location = status->mutable_location();
+  UGV_Status *status = ugv_message.mutable_status();
-  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;
@@ -160,8 +158,12 @@ void CommsClass::RunTask() {
       continue;
     }
 
-    packet_num++;
+    Lock();
-    ret = SendPacket(ugv_message);
+    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 {

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