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

More work on getting E32 lora to work

Browse Source
This commit is contained in:
Alex Mikhalev 2019-01-23 18:37:45 -08:00
parent 361320c08d
commit 3afef4f2d5
5 changed files with 177 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
components/sx127x_driver/sx127x_driver.h
View File

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

38
main/e32_driver.cc
View File

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

2
main/e32_driver.hh
View File

@ -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:

133
main/ugv_comms.cc
View File

@ -4,7 +4,10 @@
#include <esp_log.h>
#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

27
main/ugv_comms.hh
View File

@ -5,7 +5,12 @@
#include <freertos/task.h>
#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);
};