You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
83 lines
2.5 KiB
83 lines
2.5 KiB
#include "ugv_comms.h" |
|
#include "ugv_config.h" |
|
|
|
#include <esp_log.h> |
|
#include <pb_decode.h> |
|
#include <pb_encode.h> |
|
|
|
#include "messages.pb.h" |
|
|
|
static const char *TAG = "ugv_comms"; |
|
|
|
static void ugv_comms_task(void *arg); |
|
static uint16_t packet_num; |
|
|
|
void ugv_comms_init() { |
|
sx127x_config_t lora_config = SX127X_CONFIG_DEFAULT; |
|
lora_config.sck_io_num = LORA_SCK; |
|
lora_config.miso_io_num = LORA_MISO; |
|
lora_config.mosi_io_num = LORA_MOSI; |
|
lora_config.cs_io_num = LORA_CS; |
|
lora_config.rst_io_num = LORA_RST; |
|
lora_config.irq_io_num = LORA_IRQ; |
|
lora_config.frequency = LORA_FREQ; |
|
lora_config.tx_power = 17; |
|
lora_config.spreading_factor = 12; |
|
lora_config.signal_bandwidth = 10E3; |
|
lora_config.sync_word = 0x34; |
|
lora_config.crc = SX127X_CRC_ENABLED; |
|
|
|
esp_err_t ret; |
|
|
|
ret = sx127x_init(&lora_config, &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); |
|
if (ret != ESP_OK) { |
|
ESP_LOGI(TAG, "LoRa start failed: %d", ret); |
|
return; |
|
} |
|
|
|
packet_num = 0; |
|
|
|
xTaskCreate(ugv_comms_task, "ugv_comms", 2 * 1024, NULL, 2, |
|
&ugv_comms_task_hndl); |
|
} |
|
|
|
void ugv_comms_task(void *params) { |
|
TickType_t send_period = pdMS_TO_TICKS(2000); |
|
TickType_t current_time = xTaskGetTickCount(); |
|
TickType_t next_send = current_time + send_period; |
|
|
|
uas_ugv_UGV_Message ugv_message = uas_ugv_UGV_Message_init_default; |
|
ugv_message.which_ugv_message = uas_ugv_UGV_Message_status_tag; |
|
ugv_message.ugv_message.status.location.fix_quality = 0; |
|
ugv_message.ugv_message.status.location.latitude = 43.65; |
|
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) { |
|
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 |
|
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; |
|
} |
|
}
|
|
|