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

more refactoring of ugv comms

ugv_io
Alex Mikhalev 6 years ago
parent
129dbc0c13
commit
c5584ad801
3 changed files with 104 additions and 47 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 70
      main/ugv_comms.c
  2. 15
      main/ugv_comms.h
  3. 60
      main/ugv_main.c

70
main/ugv_comms.c
Unescape View File

@ -12,7 +12,13 @@ static const char *TAG = "ugv_comms"; @@ -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() { @@ -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() { @@ -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,16 +78,25 @@ void ugv_comms_task(void *params) { @@ -60,16 +78,25 @@ 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,
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);
@ -77,7 +104,20 @@ void ugv_comms_task(void *params) { @@ -77,7 +104,20 @@ void ugv_comms_task(void *params) {
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);
}

15
main/ugv_comms.h
Unescape View File

@ -1,11 +1,24 @@ @@ -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"
typedef struct ugv_comms_state_s {
sx127x_hndl lora;
TaskHandle_t ugv_comms_task_hndl;
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();

60
main/ugv_main.c
Unescape View File

@ -14,12 +14,6 @@ static const char *TAG = "ugv_main"; @@ -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) { @@ -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);
snprintf(buf, BUF_SZ, "rssi: %d lna gain: %d", lora_rssi, lora_lna_gain);
u8g2_DrawStr(&u8g2, 4, 3 * 8, buf);
snprintf(buf, 40, "%.*s", rx_packet.data_len, rx_packet.data);
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, 40, "rssi: %d lna gain: %d", lora_rssi, lora_lna_gain);
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");
}
} while (u8g2_NextPage(&u8g2));
vTaskDelay(pdMS_TO_TICKS(1000));
}

Write Preview
Loading…
Cancel
Save