From 3afef4f2d5303afcc6aa9c094aa786e0d6861c48 Mon Sep 17 00:00:00 2001 From: Alex Mikhalev Date: Wed, 23 Jan 2019 18:37:45 -0800 Subject: [PATCH] More work on getting E32 lora to work --- components/sx127x_driver/sx127x_driver.h | 8 +- main/e32_driver.cc | 38 ++++++- main/e32_driver.hh | 2 +- main/ugv_comms.cc | 133 +++++++++++++++++++---- main/ugv_comms.hh | 27 ++++- 5 files changed, 177 insertions(+), 31 deletions(-) diff --git a/components/sx127x_driver/sx127x_driver.h b/components/sx127x_driver/sx127x_driver.h index b3de351..fdafba8 100644 --- a/components/sx127x_driver/sx127x_driver.h +++ b/components/sx127x_driver/sx127x_driver.h @@ -75,15 +75,15 @@ typedef struct sx127x_config { // clang-format on typedef struct sx127x_packet_rx { - char * data; - size_t data_len; + uint8_t *data; + size_t data_len; // rssi value int32_t rssi; // snr value in steps of .25 - int8_t snr; + int8_t snr; } sx127x_rx_packet_t; -typedef struct sx127x* sx127x_hndl; +typedef struct sx127x *sx127x_hndl; sx127x_config_t sx127x_config_default(); diff --git a/main/e32_driver.cc b/main/e32_driver.cc index 90d3faa..5ea5b87 100644 --- a/main/e32_driver.cc +++ b/main/e32_driver.cc @@ -76,6 +76,11 @@ esp_err_t E32_Driver::Init(Config config) { } initialized_ = true; + ReadParams(params_); + if (ret != ESP_OK) { + goto error; + } + return ESP_OK; error: @@ -248,7 +253,27 @@ esp_err_t E32_Driver::WriteParams(const Params& params) { esp_err_t E32_Driver::Write(Address address, Channel channel, const uint8_t* data, size_t data_size) { + esp_err_t ret; + if (params_.tx_mode == TxFixed) { + uint8_t header[3]; + header[0] = address >> 8; + header[1] = address & 0xFF; + header[2] = channel; + + ret = uart_write_bytes(config_.uart_port, (char*)header, sizeof(header)); + if (ret != ESP_OK) { + goto error; + } + } + ret = uart_write_bytes(config_.uart_port, (char*)data, data_size); + if (ret != ESP_OK) { + goto error; + } return ESP_OK; + +error: + ESP_LOGE(TAG, "Write error %d", ret); + return ret; } esp_err_t E32_Driver::Write(const uint8_t* data, size_t data_size) { return Write(params_.address, params_.comm_channel, data, data_size); @@ -261,23 +286,24 @@ esp_err_t E32_Driver::Write(const std::string& data) { return Write((uint8_t*)data.c_str(), data.size()); } -int E32_Driver::Read(uint8_t *data, int max_len, TickType_t ticks_to_wait) { - return uart_read_bytes(config_.uart_port, data, max_len, ticks_to_wait); +int E32_Driver::Read(uint8_t* data, int max_len, TickType_t ticks_to_wait) { + return uart_read_bytes(config_.uart_port, (uint8_t*)data, max_len, + ticks_to_wait); } int E32_Driver::Read(std::string& data, TickType_t ticks_to_wait) { data.clear(); - uint8_t* buf = (uint8_t*) malloc(128); - TickType_t start_tick = xTaskGetTickCount(); + uint8_t* buf = (uint8_t*)malloc(128); + TickType_t start_tick = xTaskGetTickCount(); TickType_t current_tick = start_tick; - int read, total_read = 0; + int read, total_read = 0; while (current_tick <= start_tick + ticks_to_wait) { read = Read(buf, 128, ticks_to_wait - (current_tick - start_tick)); if (read < 0) { free(buf); return read; } - data.append((const char*) buf, read); + data.append((const char*)buf, read); total_read += read; current_tick = xTaskGetTickCount(); } diff --git a/main/e32_driver.hh b/main/e32_driver.hh index e5a7079..b7ca363 100644 --- a/main/e32_driver.hh +++ b/main/e32_driver.hh @@ -68,7 +68,7 @@ class E32_Driver { esp_err_t Write(const std::string& data); int Read(uint8_t* data, int max_len, - TickType_t ticks_to_wait = portMAX_DELAY); + TickType_t ticks_to_wait = portMAX_DELAY); int Read(std::string& data, TickType_t ticks_to_wait = portMAX_DELAY); private: diff --git a/main/ugv_comms.cc b/main/ugv_comms.cc index 6697120..2b41951 100644 --- a/main/ugv_comms.cc +++ b/main/ugv_comms.cc @@ -4,7 +4,10 @@ #include #include "messages.pb.h" + +#ifdef COMMS_SX127X #include "sx127x_registers.h" +#endif namespace ugv { namespace comms { @@ -24,6 +27,9 @@ CommsClass::CommsClass() } void CommsClass::Init() { + esp_err_t ret; + +#ifdef COMMS_SX127X sx127x_config_t lora_config = sx127x_config_default(); lora_config.sck_io_num = (gpio_num_t)LORA_SCK; lora_config.miso_io_num = (gpio_num_t)LORA_MISO; @@ -38,20 +44,31 @@ void CommsClass::Init() { 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); + ESP_LOGE(TAG, "LoRa start failed: %d", ret); + return; + } + ESP_LOGI(TAG, "LoRa initialized"); +#else + e32::Config lora_config; + lora_config.uart_port = UART_NUM_2; + lora_config.uart_rx_pin = 10; + lora_config.uart_rx_pin = 11; + ret = lora.Init(lora_config); + if (ret != ESP_OK) { + const char *error_name = esp_err_to_name(ret); + ESP_LOGE(TAG, "E32 LoRa Init failed: %s (%d)", error_name, ret); return; } + packet_len = -1; +#endif packet_num = 0; @@ -67,17 +84,28 @@ void CommsClass::Unlock() { xSemaphoreGive(mutex); } int32_t CommsClass::ReadRssi() { int32_t rssi; +#ifdef COMMS_SX127X sx127x_read_rssi(lora, &rssi); +#else + rssi = 0; +#endif return rssi; } uint8_t CommsClass::ReadLnaGain() { uint8_t lna_gain; +#ifdef COMMS_SX127X sx127x_read_lna_gain(lora, &lna_gain); +#else + lna_gain = 0; +#endif return lna_gain; } -void CommsClass::CommsTask(void *arg) { ((CommsClass *)arg)->RunTask(); } +void CommsClass::CommsTask(void *arg) { + ((CommsClass *)arg)->RunTask(); + vTaskDelete(NULL); +} void CommsClass::RunTask() { using messages::Location; @@ -89,8 +117,16 @@ void CommsClass::RunTask() { TickType_t current_tick = xTaskGetTickCount(); TickType_t next_send = current_tick + send_period; - esp_err_t ret; + esp_err_t ret; + +#ifdef COMMS_SX127X sx127x_rx_packet_t rx_packet; +#else + static const int rx_buf_len = MAX_PACKET_LEN * 2; + + uint8_t *rx_buf = new uint8_t[rx_buf_len]; + int rx_len; +#endif UGV_Message ugv_message; UGV_Status *status = ugv_message.mutable_status(); @@ -103,26 +139,31 @@ void CommsClass::RunTask() { while (true) { TickType_t delay_ticks = next_send - current_tick; - ret = sx127x_recv_packet(lora, &rx_packet, delay_ticks); + +#ifdef COMMS_SX127X + ret = sx127x_recv_packet(lora, &rx_packet, delay_ticks); +#else + rx_len = lora.Read(rx_buf, rx_buf_len, delay_ticks); +#endif current_tick = xTaskGetTickCount(); + +#ifdef COMMS_SX127X if (ret == ESP_OK) { HandlePacket(&rx_packet); sx127x_packet_rx_free(&rx_packet); } +#else + HandleRxData(rx_buf, rx_len); +#endif if (current_tick < next_send) { continue; } packet_num++; - auto str = ugv_message.SerializeAsString(); - ret = sx127x_send_packet(lora, str.c_str(), str.size(), - 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 + ret = SendPacket(ugv_message); if (ret != ESP_OK) { ESP_LOGE(TAG, "error sending packet: %d", ret); } else { @@ -132,11 +173,13 @@ void CommsClass::RunTask() { current_tick = xTaskGetTickCount(); next_send = current_tick + send_period; } +#ifndef COMMS_SX127X + delete[] rx_buf; +#endif } +#ifdef COMMS_SX127X void CommsClass::HandlePacket(sx127x_rx_packet_t *rx_packet) { - using messages::GroundMessage; - 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); @@ -147,10 +190,40 @@ void CommsClass::HandlePacket(sx127x_rx_packet_t *rx_packet) { last_packet_snr = rx_packet->snr; xSemaphoreGive(mutex); + HandlePacket(rx_packet->data, rx_packet->data_len); +} +#else +void CommsClass::HandleRxData(const uint8_t *data, int size) { + if (size <= 0) { + return; + } + int i = 0; + while (i < size) { + if (packet_len <= 0) { + packet_len = data[i++]; + packet_buf.clear(); + } else { + int remaining = packet_len - packet_buf.size(); + int available = size - i; + int read = (remaining > available) ? available : remaining; + packet_buf.append((const char *)&data[i], read); + i += read; + if (packet_buf.size() >= packet_len) { + HandlePacket((uint8_t *)packet_buf.c_str(), packet_buf.size()); + packet_buf.clear(); + packet_len = -1; + } + } + } +} +#endif + +void CommsClass::HandlePacket(const uint8_t *data, size_t data_size) { + using messages::GroundMessage; + GroundMessage ground_message; - bool pb_ret = - ground_message.ParseFromArray(rx_packet->data, rx_packet->data_len); + bool pb_ret = ground_message.ParseFromArray(data, data_size); if (!pb_ret) { ESP_LOGE(TAG, "rx invalid protobuf"); return; @@ -174,8 +247,30 @@ void CommsClass::HandleCommand(const messages::GroundCommand &command) { messages::UGV_Message ugv_message; ugv_message.set_command_ack(command.id()); - std::string str = ugv_message.SerializeAsString(); - sx127x_send_packet(lora, str.c_str(), str.size(), 0); + SendPacket(ugv_message); +} + +esp_err_t CommsClass::SendPacket(const char *data, size_t size) { +#ifdef COMMS_SX127X + return sx127x_send_packet(lora, data, size, 0); +#else + if (size >= MAX_PACKET_LEN) { + ESP_LOGE(TAG, "SendPacket size too long: %d", size); + return ESP_ERR_INVALID_SIZE; + } + uint8_t sz = size; + lora.Write(&sz, sizeof(sz)); + return lora.Write((uint8_t *)data, size); +#endif +} + +esp_err_t CommsClass::SendPacket(const std::string &str) { + return SendPacket(str.c_str(), str.size()); +} + +esp_err_t CommsClass::SendPacket(const google::protobuf::MessageLite &message) { + std::string str = message.SerializeAsString(); + return SendPacket(str); } } // namespace comms diff --git a/main/ugv_comms.hh b/main/ugv_comms.hh index ed9ee13..1d20568 100644 --- a/main/ugv_comms.hh +++ b/main/ugv_comms.hh @@ -5,7 +5,12 @@ #include #include "messages.pb.h" + +#ifdef COMMS_SX127X #include "sx127x_driver.h" +#else +#include "e32_driver.hh" +#endif namespace ugv { namespace comms { @@ -14,6 +19,8 @@ namespace messages = uas::ugv::messages; class CommsClass { public: + static constexpr int MAX_PACKET_LEN = 255; + CommsClass(); void Init(); @@ -33,14 +40,32 @@ class CommsClass { int8_t last_packet_snr; private: - sx127x_hndl lora; +#ifdef COMMS_SX127X + sx127x_hndl lora; +#else + e32::E32_Driver lora; + int packet_len; + std::string packet_buf; +#endif TaskHandle_t task_handle; uint16_t packet_num; void RunTask(); + +#ifdef COMMS_SX127X void HandlePacket(sx127x_rx_packet_t* packet); +#else + + void HandleRxData(const uint8_t* data, int size); +#endif + + void HandlePacket(const uint8_t* data, size_t size); void HandleCommand(const messages::GroundCommand& command); + esp_err_t SendPacket(const char* data, size_t size); + esp_err_t SendPacket(const std::string& str); + esp_err_t SendPacket(const google::protobuf::MessageLite& message); + static void CommsTask(void* arg); };