From 35d3f6b9c781d4ee8c800d1c7e9517162390518b Mon Sep 17 00:00:00 2001 From: Alex Mikhalev Date: Thu, 9 May 2019 10:23:27 -0700 Subject: [PATCH] use base64 encoding for messages --- main/CMakeLists.txt | 2 +- main/e32_driver.cc | 50 +++++++++++++++++++++------- main/e32_driver.hh | 8 +++-- main/ugv_comms.cc | 80 ++++++++++++++++++++++++--------------------- main/ugv_comms.hh | 3 +- 5 files changed, 90 insertions(+), 53 deletions(-) diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt index e9e892c..d0c0480 100644 --- a/main/CMakeLists.txt +++ b/main/CMakeLists.txt @@ -15,7 +15,7 @@ set(COMPONENT_SRCS "e32_driver.cc" ) set(COMPONENT_PRIV_INCLUDEDIRS "." ${PB_OUT}) -set(COMPONENT_REQUIRES "u8g2" "sx127x_driver" "protobuf" "MPU-driver" "minmea") +set(COMPONENT_REQUIRES "u8g2" "sx127x_driver" "protobuf" "MPU-driver" "minmea" "mbedtls") proto_generate_cpp(messages.proto ${PB_OUT} PROTO_HDRS PROTO_SRCS) list(APPEND COMPONENT_SRCS ${PROTO_SRCS} ${PROTO_HDRS}) diff --git a/main/e32_driver.cc b/main/e32_driver.cc index a5c05ba..87dc59f 100644 --- a/main/e32_driver.cc +++ b/main/e32_driver.cc @@ -78,7 +78,7 @@ esp_err_t E32_Driver::Init(Config config) { // ReadParams(params_); // if (ret != ESP_OK) { - // goto error; + // goto error; // } return ESP_OK; @@ -284,6 +284,17 @@ int E32_Driver::Write(const std::string& data) { return Write((uint8_t*)data.c_str(), data.size()); } +int E32_Driver::WriteLn(const uint8_t* data, size_t data_size) { + int written = 0; + written += Write((const uint8_t*)data, data_size); + written += Write((const uint8_t*)"\n", 1); + return written; +} + +int E32_Driver::WriteLn(const std::string& data) { + return WriteLn((const uint8_t*)data.c_str(), data.size()); +} + esp_err_t E32_Driver::WaitWriteDone(TickType_t ticks_to_wait) { return uart_wait_tx_done(config_.uart_port, ticks_to_wait); } @@ -293,30 +304,47 @@ int E32_Driver::Read(uint8_t* data, int max_len, TickType_t ticks_to_wait) { ticks_to_wait); } -int E32_Driver::Read(std::string& data, TickType_t ticks_to_wait) { - data.clear(); - uint8_t* buf = (uint8_t*)malloc(128); +int E32_Driver::ReadLn(char* data, size_t data_size, TickType_t ticks_to_wait) { + // TODO: more proper way to read a line + uint8_t byte; TickType_t start_tick = xTaskGetTickCount(); TickType_t current_tick = start_tick; int read, total_read = 0; - while (current_tick <= start_tick + ticks_to_wait) { - read = Read(buf, 128, ticks_to_wait - (current_tick - start_tick)); + while (total_read < data_size) { + read = Read(&byte, 1, ticks_to_wait - (current_tick - start_tick)); if (read < 0) { - free(buf); return read; } - data.append((const char*)buf, read); + if (byte == '\n') break; + data[total_read] = (char)byte; total_read += read; current_tick = xTaskGetTickCount(); } - free(buf); return total_read; } -void E32_Driver::Flush() { - uart_flush(config_.uart_port); +int E32_Driver::ReadLn(std::string& data, TickType_t ticks_to_wait) { + // TODO: more proper way to read a line + data.clear(); + uint8_t byte; + TickType_t start_tick = xTaskGetTickCount(); + TickType_t current_tick = start_tick; + int read, total_read = 0; + while (true) { + read = Read(&byte, 1, ticks_to_wait - (current_tick - start_tick)); + if (read < 0) { + return read; + } + if (byte == '\n') break; + data += (char)byte; + total_read += read; + current_tick = xTaskGetTickCount(); + } + return total_read; } +void E32_Driver::Flush() { uart_flush(config_.uart_port); } + int E32_Driver::RawWrite(const uint8_t* data, size_t data_size) { int written = uart_write_bytes(config_.uart_port, (const char*)data, data_size); diff --git a/main/e32_driver.hh b/main/e32_driver.hh index 19b7f65..66d5385 100644 --- a/main/e32_driver.hh +++ b/main/e32_driver.hh @@ -62,16 +62,20 @@ class E32_Driver { esp_err_t WriteParams(const Params& params); int Write(Address address, Channel channel, const uint8_t* data, - size_t data_size); + size_t data_size); int Write(const uint8_t* data, size_t data_size); int Write(Address address, Channel channel, const std::string& data); int Write(const std::string& data); + int WriteLn(const uint8_t* data, size_t data_size); + int WriteLn(const std::string& data); esp_err_t WaitWriteDone(TickType_t ticks_to_wait = portMAX_DELAY); int Read(uint8_t* data, int max_len, TickType_t ticks_to_wait = portMAX_DELAY); - int Read(std::string& data, TickType_t ticks_to_wait = portMAX_DELAY); + int ReadLn(char* data, size_t data_size, + TickType_t ticks_to_wait = portMAX_DELAY); + int ReadLn(std::string& data, TickType_t ticks_to_wait = portMAX_DELAY); void Flush(); diff --git a/main/ugv_comms.cc b/main/ugv_comms.cc index 0d37abc..05099ae 100644 --- a/main/ugv_comms.cc +++ b/main/ugv_comms.cc @@ -2,6 +2,7 @@ #include "ugv_config.h" #include +#include #include "messages.pb.h" @@ -70,7 +71,6 @@ void CommsClass::Init() { ESP_LOGE(TAG, "E32 LoRa Init failed: %s (%d)", error_name, ret); return; } - packet_len = -1; #endif xTaskCreate(CommsClass::CommsTask, "ugv_comms", 8 * 1024, this, 2, @@ -89,7 +89,7 @@ int32_t CommsClass::ReadRssi() { #ifdef COMMS_SX127X sx127x_read_rssi(lora, &rssi); #else - rssi = 0; + rssi = 0; #endif return rssi; } @@ -99,7 +99,7 @@ uint8_t CommsClass::ReadLnaGain() { #ifdef COMMS_SX127X sx127x_read_lna_gain(lora, &lna_gain); #else - lna_gain = 0; + lna_gain = 0; #endif return lna_gain; } @@ -124,10 +124,8 @@ void CommsClass::RunTask() { #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; + std::string rx_buf; + int rx_len; #endif UGV_Message ugv_message; @@ -145,27 +143,14 @@ void CommsClass::RunTask() { sx127x_packet_rx_free(&rx_packet); } #else - if (packet_len <= 0) { // need to receive packet size first - rx_len = lora.Read(rx_buf, 1, delay_ticks); - if (rx_len > 0) { - packet_len = rx_buf[0]; - if (packet_len > MAX_PACKET_LEN) { // should not be possible - ESP_LOGE(TAG, "invalid packet len received: %d", packet_len); - packet_len = -1; - } else { - ESP_LOGV(TAG, "rx packet len: %d", packet_len); - } - } - } else { // receiving packet data now - rx_len = lora.Read(rx_buf, packet_len, PACKET_RX_TIMEOUT); - if (rx_len < packet_len) { - ESP_LOGE(TAG, "timeout for packet rx"); - lora.Flush(); - } else { - ESP_LOGV(TAG, "rx data: %.*s", packet_len, rx_buf); - HandlePacket(rx_buf, packet_len); - } - packet_len = -1; // wait for new packet len + // receiving packet data now + rx_len = lora.ReadLn(rx_buf, PACKET_RX_TIMEOUT); + if (rx_len <= 0) { + ESP_LOGE(TAG, "timeout for packet rx"); + lora.Flush(); + } else { + ESP_LOGV(TAG, "rx data: %.*s", rx_buf.size(), rx_buf.c_str()); + HandlePacket((uint8_t *)rx_buf.c_str(), rx_buf.size()); } // TODO: checksum? #endif @@ -192,9 +177,6 @@ void CommsClass::RunTask() { current_tick = xTaskGetTickCount(); next_send = current_tick + send_period; } -#ifndef COMMS_SX127X - delete[] rx_buf; -#endif } #ifdef COMMS_SX127X @@ -219,9 +201,23 @@ void CommsClass::HandlePacket(const uint8_t *data, size_t data_size) { last_packet_tick = xTaskGetTickCount(); xSemaphoreGive(mutex); + int ret; + size_t decoded_size = (data_size * 4 + 2) / 3; + uint8_t *decoded = new uint8_t[decoded_size]; + size_t decoded_len; + + ret = mbedtls_base64_decode(decoded, decoded_size, &decoded_len, data, + data_size); + if (ret != 0) { + ESP_LOGE(TAG, "error with base64 decode: %d", ret); + delete[] decoded; + return; + } + GroundMessage ground_message; - bool pb_ret = ground_message.ParseFromArray(data, data_size); + bool pb_ret = ground_message.ParseFromArray(decoded, decoded_len); + delete[] decoded; if (!pb_ret) { ESP_LOGE(TAG, "rx invalid protobuf"); return; @@ -261,7 +257,7 @@ void CommsClass::HandleCommand(const messages::GroundCommand &command) { SendPacket(ugv_message); } -esp_err_t CommsClass::SendPacket(const char *data, size_t size) { +esp_err_t CommsClass::SendPacket(const uint8_t *data, size_t size) { #ifdef COMMS_SX127X return sx127x_send_packet(lora, data, size, 0); #else @@ -269,15 +265,25 @@ esp_err_t CommsClass::SendPacket(const char *data, size_t size) { ESP_LOGE(TAG, "SendPacket size too long: %d", size); return ESP_ERR_INVALID_SIZE; } - uint8_t sz = size; - lora.Write(&sz, sizeof(sz)); - lora.Write((uint8_t *)data, size); + size_t encoded_size = ((size + 2) / 3) * 4; + uint8_t *encoded = new uint8_t[encoded_size]; + size_t encoded_len; + int ret = + mbedtls_base64_encode(encoded, encoded_size, &encoded_len, data, size); + if (ret != 0) { + delete[] encoded; + ESP_LOGE(TAG, "base64 encode error: %d", ret); + return ESP_FAIL; + } + // TODO: checksum + lora.WriteLn(encoded, encoded_len); + delete[] encoded; return ESP_OK; #endif } esp_err_t CommsClass::SendPacket(const std::string &str) { - return SendPacket(str.c_str(), str.size()); + return SendPacket((const uint8_t *)str.c_str(), str.size()); } esp_err_t CommsClass::SendPacket(const google::protobuf::MessageLite &message) { diff --git a/main/ugv_comms.hh b/main/ugv_comms.hh index e8c2212..873cdce 100644 --- a/main/ugv_comms.hh +++ b/main/ugv_comms.hh @@ -46,7 +46,6 @@ class CommsClass { sx127x_hndl lora; #else e32::E32_Driver lora; - int packet_len; #endif TaskHandle_t task_handle; @@ -59,7 +58,7 @@ class CommsClass { 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 uint8_t* data, size_t size); esp_err_t SendPacket(const std::string& str); esp_err_t SendPacket(const google::protobuf::MessageLite& message);