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

use base64 encoding for messages

master
Alex Mikhalev 6 years ago
parent
771b98a667
commit
35d3f6b9c7
5 changed files with 90 additions and 53 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      main/CMakeLists.txt
  2. 50
      main/e32_driver.cc
  3. 8
      main/e32_driver.hh
  4. 80
      main/ugv_comms.cc
  5. 3
      main/ugv_comms.hh

2
main/CMakeLists.txt
Unescape View File

@ -15,7 +15,7 @@ set(COMPONENT_SRCS
"e32_driver.cc" "e32_driver.cc"
) )
set(COMPONENT_PRIV_INCLUDEDIRS "." ${PB_OUT}) 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) proto_generate_cpp(messages.proto ${PB_OUT} PROTO_HDRS PROTO_SRCS)
list(APPEND COMPONENT_SRCS ${PROTO_SRCS} ${PROTO_HDRS}) list(APPEND COMPONENT_SRCS ${PROTO_SRCS} ${PROTO_HDRS})

50
main/e32_driver.cc
Unescape View File

@ -78,7 +78,7 @@ esp_err_t E32_Driver::Init(Config config) {
// ReadParams(params_); // ReadParams(params_);
// if (ret != ESP_OK) { // if (ret != ESP_OK) {
// goto error; // goto error;
// } // }
return ESP_OK; return ESP_OK;
@ -284,6 +284,17 @@ int E32_Driver::Write(const std::string& data) {
return Write((uint8_t*)data.c_str(), data.size()); 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) { esp_err_t E32_Driver::WaitWriteDone(TickType_t ticks_to_wait) {
return uart_wait_tx_done(config_.uart_port, 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); ticks_to_wait);
} }
int E32_Driver::Read(std::string& data, TickType_t ticks_to_wait) { int E32_Driver::ReadLn(char* data, size_t data_size, TickType_t ticks_to_wait) {
data.clear(); // TODO: more proper way to read a line
uint8_t* buf = (uint8_t*)malloc(128); uint8_t byte;
TickType_t start_tick = xTaskGetTickCount(); TickType_t start_tick = xTaskGetTickCount();
TickType_t current_tick = start_tick; TickType_t current_tick = start_tick;
int read, total_read = 0; int read, total_read = 0;
while (current_tick <= start_tick + ticks_to_wait) { while (total_read < data_size) {
read = Read(buf, 128, ticks_to_wait - (current_tick - start_tick)); read = Read(&byte, 1, ticks_to_wait - (current_tick - start_tick));
if (read < 0) { if (read < 0) {
free(buf);
return read; return read;
} }
data.append((const char*)buf, read); if (byte == '\n') break;
data[total_read] = (char)byte;
total_read += read; total_read += read;
current_tick = xTaskGetTickCount(); current_tick = xTaskGetTickCount();
} }
free(buf);
return total_read; return total_read;
} }
void E32_Driver::Flush() { int E32_Driver::ReadLn(std::string& data, TickType_t ticks_to_wait) {
uart_flush(config_.uart_port); // 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 E32_Driver::RawWrite(const uint8_t* data, size_t data_size) {
int written = int written =
uart_write_bytes(config_.uart_port, (const char*)data, data_size); uart_write_bytes(config_.uart_port, (const char*)data, data_size);

8
main/e32_driver.hh
Unescape View File

@ -62,16 +62,20 @@ class E32_Driver {
esp_err_t WriteParams(const Params& params); esp_err_t WriteParams(const Params& params);
int Write(Address address, Channel channel, const uint8_t* data, 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(const uint8_t* data, size_t data_size);
int Write(Address address, Channel channel, const std::string& data); int Write(Address address, Channel channel, const std::string& data);
int Write(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); esp_err_t WaitWriteDone(TickType_t ticks_to_wait = portMAX_DELAY);
int Read(uint8_t* data, int max_len, 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); 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(); void Flush();

80
main/ugv_comms.cc
Unescape View File

@ -2,6 +2,7 @@
#include "ugv_config.h" #include "ugv_config.h"
#include <esp_log.h> #include <esp_log.h>
#include <mbedtls/base64.h>
#include "messages.pb.h" #include "messages.pb.h"
@ -70,7 +71,6 @@ void CommsClass::Init() {
ESP_LOGE(TAG, "E32 LoRa Init failed: %s (%d)", error_name, ret); ESP_LOGE(TAG, "E32 LoRa Init failed: %s (%d)", error_name, ret);
return; return;
} }
packet_len = -1;
#endif #endif
xTaskCreate(CommsClass::CommsTask, "ugv_comms", 8 * 1024, this, 2, xTaskCreate(CommsClass::CommsTask, "ugv_comms", 8 * 1024, this, 2,
@ -89,7 +89,7 @@ int32_t CommsClass::ReadRssi() {
#ifdef COMMS_SX127X #ifdef COMMS_SX127X
sx127x_read_rssi(lora, &rssi); sx127x_read_rssi(lora, &rssi);
#else #else
rssi = 0; rssi = 0;
#endif #endif
return rssi; return rssi;
} }
@ -99,7 +99,7 @@ uint8_t CommsClass::ReadLnaGain() {
#ifdef COMMS_SX127X #ifdef COMMS_SX127X
sx127x_read_lna_gain(lora, &lna_gain); sx127x_read_lna_gain(lora, &lna_gain);
#else #else
lna_gain = 0; lna_gain = 0;
#endif #endif
return lna_gain; return lna_gain;
} }
@ -124,10 +124,8 @@ void CommsClass::RunTask() {
#ifdef COMMS_SX127X #ifdef COMMS_SX127X
sx127x_rx_packet_t rx_packet; sx127x_rx_packet_t rx_packet;
#else #else
static const int rx_buf_len = MAX_PACKET_LEN * 2; std::string rx_buf;
int rx_len;
uint8_t *rx_buf = new uint8_t[rx_buf_len];
int rx_len;
#endif #endif
UGV_Message ugv_message; UGV_Message ugv_message;
@ -145,27 +143,14 @@ void CommsClass::RunTask() {
sx127x_packet_rx_free(&rx_packet); sx127x_packet_rx_free(&rx_packet);
} }
#else #else
if (packet_len <= 0) { // need to receive packet size first // receiving packet data now
rx_len = lora.Read(rx_buf, 1, delay_ticks); rx_len = lora.ReadLn(rx_buf, PACKET_RX_TIMEOUT);
if (rx_len > 0) { if (rx_len <= 0) {
packet_len = rx_buf[0]; ESP_LOGE(TAG, "timeout for packet rx");
if (packet_len > MAX_PACKET_LEN) { // should not be possible lora.Flush();
ESP_LOGE(TAG, "invalid packet len received: %d", packet_len); } else {
packet_len = -1; ESP_LOGV(TAG, "rx data: %.*s", rx_buf.size(), rx_buf.c_str());
} else { HandlePacket((uint8_t *)rx_buf.c_str(), rx_buf.size());
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
} }
// TODO: checksum? // TODO: checksum?
#endif #endif
@ -192,9 +177,6 @@ void CommsClass::RunTask() {
current_tick = xTaskGetTickCount(); current_tick = xTaskGetTickCount();
next_send = current_tick + send_period; next_send = current_tick + send_period;
} }
#ifndef COMMS_SX127X
delete[] rx_buf;
#endif
} }
#ifdef COMMS_SX127X #ifdef COMMS_SX127X
@ -219,9 +201,23 @@ void CommsClass::HandlePacket(const uint8_t *data, size_t data_size) {
last_packet_tick = xTaskGetTickCount(); last_packet_tick = xTaskGetTickCount();
xSemaphoreGive(mutex); 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; 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) { if (!pb_ret) {
ESP_LOGE(TAG, "rx invalid protobuf"); ESP_LOGE(TAG, "rx invalid protobuf");
return; return;
@ -261,7 +257,7 @@ void CommsClass::HandleCommand(const messages::GroundCommand &command) {
SendPacket(ugv_message); 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 #ifdef COMMS_SX127X
return sx127x_send_packet(lora, data, size, 0); return sx127x_send_packet(lora, data, size, 0);
#else #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); ESP_LOGE(TAG, "SendPacket size too long: %d", size);
return ESP_ERR_INVALID_SIZE; return ESP_ERR_INVALID_SIZE;
} }
uint8_t sz = size; size_t encoded_size = ((size + 2) / 3) * 4;
lora.Write(&sz, sizeof(sz)); uint8_t *encoded = new uint8_t[encoded_size];
lora.Write((uint8_t *)data, 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; return ESP_OK;
#endif #endif
} }
esp_err_t CommsClass::SendPacket(const std::string &str) { 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) { esp_err_t CommsClass::SendPacket(const google::protobuf::MessageLite &message) {

3
main/ugv_comms.hh
Unescape View File

@ -46,7 +46,6 @@ class CommsClass {
sx127x_hndl lora; sx127x_hndl lora;
#else #else
e32::E32_Driver lora; e32::E32_Driver lora;
int packet_len;
#endif #endif
TaskHandle_t task_handle; TaskHandle_t task_handle;
@ -59,7 +58,7 @@ class CommsClass {
void HandlePacket(const uint8_t* data, size_t size); void HandlePacket(const uint8_t* data, size_t size);
void HandleCommand(const messages::GroundCommand& command); 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 std::string& str);
esp_err_t SendPacket(const google::protobuf::MessageLite& message); esp_err_t SendPacket(const google::protobuf::MessageLite& message);

Write Preview
Loading…
Cancel
Save