more refactoring of ugv comms

main/ugv_comms.c

@@ -12,7 +12,13 @@ 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);
+
 void ugv_comms_init() {
+  ugv_comms_state_t *st = &ugv_comms_state;
+
   sx127x_config_t lora_config  = SX127X_CONFIG_DEFAULT;
   lora_config.sck_io_num       = LORA_SCK;
   lora_config.miso_io_num      = LORA_MISO;
@@ -29,14 +35,22 @@ void ugv_comms_init() {
 
   esp_err_t ret;
 
-  ret = sx127x_init(&lora_config, &lora);
+  st->mutex            = xSemaphoreCreateMutex();
+  uas_ugv_Location loc = uas_ugv_Location_init_default;
+  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(lora);
+  ret = sx127x_start(st->lora);
   if (ret != ESP_OK) {
     ESP_LOGI(TAG, "LoRa start failed: %d", ret);
     return;
@@ -44,14 +58,18 @@ void ugv_comms_init() {
 
   packet_num = 0;
 
-  xTaskCreate(ugv_comms_task, "ugv_comms", 2 * 1024, NULL, 2,
-              &ugv_comms_task_hndl);
+  xTaskCreate(ugv_comms_task, "ugv_comms", 2 * 1024, st, 2, &st->task_handle);
 }
 
-void ugv_comms_task(void *params) {
+static void ugv_comms_task(void *param) {
+  ugv_comms_state_t *st = (ugv_comms_state_t *)param;
+
   TickType_t send_period  = pdMS_TO_TICKS(2000);
-  TickType_t current_time = xTaskGetTickCount();
-  TickType_t next_send    = current_time + send_period;
+  TickType_t current_tick = xTaskGetTickCount();
+  TickType_t next_send    = current_tick + send_period;
+
+  esp_err_t          ret;
+  sx127x_rx_packet_t rx_packet;
 
   uas_ugv_UGV_Message ugv_message = uas_ugv_UGV_Message_init_default;
   ugv_message.which_ugv_message   = uas_ugv_UGV_Message_status_tag;
@@ -60,24 +78,46 @@ void ugv_comms_task(void *params) {
   ugv_message.ugv_message.status.location.longitude   = -116.20;
   ugv_message.ugv_message.status.location.altitude    = 2730;
   ugv_message.ugv_message.status.state                = uas_ugv_UGV_State_IDLE;
+
   while (true) {
-    TickType_t delay_ticks = next_send - current_time;
-    vTaskDelay(delay_ticks);
-    current_time = xTaskGetTickCount();
-    if (current_time < next_send) {
+    TickType_t delay_ticks = next_send - current_tick;
+    ret = sx127x_recv_packet(st->lora, &rx_packet, delay_ticks);
+
+    current_tick = xTaskGetTickCount();
+    if (ret == ESP_OK) {
+      ugv_comms_handle_packet(st, &rx_packet);
+
+      sx127x_packet_rx_free(&rx_packet);
+    }
+
+    if (current_tick < next_send) {
       continue;
     }
+
     packet_num++;
-    esp_err_t ret =
-        sx127x_send_packet_pb(lora, uas_ugv_UGV_Message_fields, &ugv_message,
-                              0);  // 0 means error if queue full
+    ret = sx127x_send_packet_pb(st->lora, uas_ugv_UGV_Message_fields,
+                                &ugv_message,
+                                0);  // 0 means error if queue full
     if (ret != ESP_OK) {
       ESP_LOGE(TAG, "error sending packet: %d", ret);
     } else {
       ESP_LOGI(TAG, "lora wrote UGV_Message packet");
     }
 
-    current_time = xTaskGetTickCount();
-    next_send    = current_time + send_period;
+    current_tick = xTaskGetTickCount();
+    next_send    = current_tick + send_period;
   }
 }
+
+static void ugv_comms_handle_packet(ugv_comms_state_t * st,
+                                    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(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);
+}

main/ugv_comms.h

@@ -1,11 +1,24 @@
 #pragma once
 
 #include <freertos/FreeRTOS.h>
+#include <freertos/semphr.h>
 #include <freertos/task.h>
 
+#include "messages.pb.h"
 #include "sx127x_driver.h"
 
-sx127x_hndl lora;
-TaskHandle_t ugv_comms_task_hndl;
+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;
+
+ugv_comms_state_t ugv_comms_state;
 
 void ugv_comms_init();

main/ugv_main.c

@@ -14,12 +14,6 @@ static const char *TAG = "ugv_main";
 
 u8g2_t u8g2;
 
-sx127x_hndl lora;
-
-void comms_task(void *params);
-
-sx127x_rx_packet_t rx_packet;
-
 void setup_oled(void) {
   u8g2_esp32_hal_t u8g2_hal_config = {
       .scl   = 15,
@@ -98,44 +92,54 @@ void setup(void) {
   setup_gps();
   setup_oled();
   ugv_comms_init();
-
-  memset(&rx_packet, 0, sizeof(rx_packet));
 }
 
-int32_t lora_rssi;
-uint8_t lora_lna_gain;
+#define BUF_SZ 32
 
 void loop(void) {
+  static int32_t    lora_rssi;
+  static uint8_t    lora_lna_gain;
+  static TickType_t last_packet_tick;
+  static int32_t    last_packet_rssi;
+  static int8_t     last_packet_snr;
+
+  static char buf[BUF_SZ];
+
   u8g2_FirstPage(&u8g2);
-  sx127x_recv_packet(lora, &rx_packet, 0);
-  sx127x_read_rssi(lora, &lora_rssi);
-  sx127x_read_lna_gain(lora, &lora_lna_gain);
+  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);
   do {
     u8g2_DrawRFrame(&u8g2, 0, 0, OLED_W, OLED_H, 4);
 
     multi_heap_info_t heap_info;
     heap_caps_get_info(&heap_info, MALLOC_CAP_DEFAULT);
-    u8g2_SetFont(&u8g2, u8g2_font_4x6_tr);
+    u8g2_SetFont(&u8g2, u8g2_font_4x6_mr);
     u8g2_DrawStr(&u8g2, 4, 8, "=====UAS UGV=====");
-    char buf[32];
-    memset(buf, 0, 32);
-    snprintf(buf, 32, "heap allc/free %d/%d", heap_info.total_allocated_bytes,
-             heap_info.total_free_bytes);
+    snprintf(buf, BUF_SZ, "heap allc/free %d/%d",
+             heap_info.total_allocated_bytes, heap_info.total_free_bytes);
     u8g2_DrawStr(&u8g2, 4, 2 * 8, buf);
 
-    if (rx_packet.data_len) {
-      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);
-      u8g2_SetFont(&u8g2, u8g2_font_4x6_tr);
-      snprintf(buf, 40, "lora pkt(rssi: %d, snr: %f)", rx_packet.rssi,
-               rx_packet.snr * 0.25f);
-      u8g2_DrawStr(&u8g2, 4, 3 * 8, buf);
-      snprintf(buf, 40, "%.*s", rx_packet.data_len, rx_packet.data);
+    snprintf(buf, BUF_SZ, "rssi: %d lna gain: %d", lora_rssi, lora_lna_gain);
+    u8g2_DrawStr(&u8g2, 4, 3 * 8, buf);
+
+    if (last_packet_tick > 0) {
+      double time_since_last_packet =
+          1000.0f /
+          ((xTaskGetTickCount() - last_packet_tick) * portTICK_RATE_MS);
+      snprintf(buf, BUF_SZ, "last pkt rx %f s ago", time_since_last_packet);
       u8g2_DrawStr(&u8g2, 4, 4 * 8, buf);
+      snprintf(buf, BUF_SZ, "pkt rssi: %d, snr: %f", last_packet_rssi,
+               last_packet_snr * 0.25f);
+      u8g2_DrawStr(&u8g2, 4, 5 * 8, buf);
+    } else {
+      u8g2_DrawStr(&u8g2, 4, 4 * 8, "no pkt rx");
     }
-    snprintf(buf, 40, "rssi: %d lna gain: %d", lora_rssi, lora_lna_gain);
-    u8g2_DrawStr(&u8g2, 4, 5 * 8, buf);
   } while (u8g2_NextPage(&u8g2));
   vTaskDelay(pdMS_TO_TICKS(1000));
 }