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

use base64 encoding for messages

Browse Source
This commit is contained in:
Alex Mikhalev 2019-05-09 10:23:27 -07:00
parent 771b98a667
commit 35d3f6b9c7
5 changed files with 90 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
main/CMakeLists.txt
View File

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

50
main/e32_driver.cc
View File

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

8
main/e32_driver.hh
View File

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

80
main/ugv_comms.cc
View File

@ -2,6 +2,7 @@
#include "ugv_config.h"
#include <esp_log.h>
#include <mbedtls/base64.h>
#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) {

3
main/ugv_comms.hh
View File

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