diff --git a/main/ugv_comms.c b/main/ugv_comms.c index 6bad373..9730962 100644 --- a/main/ugv_comms.c +++ b/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); +} diff --git a/main/ugv_comms.h b/main/ugv_comms.h index c44db3f..940d434 100644 --- a/main/ugv_comms.h +++ b/main/ugv_comms.h @@ -1,11 +1,24 @@ #pragma once #include +#include #include +#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(); diff --git a/main/ugv_main.c b/main/ugv_main.c index 62cb229..f185324 100644 --- a/main/ugv_main.c +++ b/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)); }