amikhalev/uas-ugv
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

more refactoring of ugv comms

Browse Source
This commit is contained in:
Alex Mikhalev 2019-01-03 17:50:12 -07:00
parent 129dbc0c13
commit c5584ad801
3 changed files with 104 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

72
main/ugv_comms.c
View File

@ -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);
}

17
main/ugv_comms.h
View File

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

62
main/ugv_main.c
View File

@ -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));
}