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

Reimplement comms with c++

try-fix-comms-errors
Alex Mikhalev 6 years ago
parent
611bade298
commit
0a6c0ebd40
5 changed files with 137 additions and 99 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/messages.proto
  2. 129
      main/ugv_comms.cc
  3. 32
      main/ugv_comms.h
  4. 50
      main/ugv_comms.hh
  5. 23
      main/ugv_main.cc

2
main/messages.proto
Unescape View File

@ -1,6 +1,6 @@
syntax = "proto3"; syntax = "proto3";
package uas.ugv; package uas.ugv.messages;
option optimize_for = LITE_RUNTIME; option optimize_for = LITE_RUNTIME;

129
main/ugv_comms.cc
Unescape View File

@ -1,29 +1,30 @@
#include "ugv_comms.h" #include "ugv_comms.hh"
#include "ugv_config.h" #include "ugv_config.h"
#include <esp_log.h> #include <esp_log.h>
#include "messages.pb.h" #include "messages.pb.h"
#include "sx127x_registers.h"
ugv_comms_state_t ugv_comms_state; namespace ugv {
namespace comms {
static const char *TAG = "ugv_comms"; CommsClass Comms;
static void ugv_comms_task(void *arg);
static uint16_t packet_num;
static void ugv_comms_task(void *arg);
static void ugv_comms_handle_packet(ugv_comms_state_t * st,
sx127x_rx_packet_t *rx_packet);
static void ugv_comms_handle_command(ugv_comms_state_t * st,
const uas::ugv::GroundCommand *command);
void ugv_comms_init() { static const char *TAG = "ugv_comms";
ugv_comms_state_t *st = &ugv_comms_state;
st->mutex = xSemaphoreCreateMutex(); CommsClass::CommsClass()
: location(),
ugv_state(messages::IDLE),
last_packet_tick(0),
last_packet_rssi(INT32_MIN),
last_packet_snr(INT8_MIN),
packet_num(0) {
mutex = xSemaphoreCreateMutex();
}
sx127x_config_t lora_config = sx127x_config_default(); void CommsClass::Init() {
sx127x_config_t lora_config = sx127x_config_default();
lora_config.sck_io_num = (gpio_num_t)LORA_SCK; lora_config.sck_io_num = (gpio_num_t)LORA_SCK;
lora_config.miso_io_num = (gpio_num_t)LORA_MISO; lora_config.miso_io_num = (gpio_num_t)LORA_MISO;
lora_config.mosi_io_num = (gpio_num_t)LORA_MOSI; lora_config.mosi_io_num = (gpio_num_t)LORA_MOSI;
@ -39,21 +40,14 @@ void ugv_comms_init() {
esp_err_t ret; esp_err_t ret;
uas::ugv::Location loc; ret = sx127x_init(&lora_config, &lora);
memcpy(&st->location, &loc, sizeof(loc));
st->ugv_state = uas::ugv::UGV_State::IDLE;
st->last_packet_tick = 0;
st->last_packet_rssi = INT32_MIN;
st->last_packet_snr = INT8_MIN;
ret = sx127x_init(&lora_config, &st->lora);
if (ret != ESP_OK) { if (ret != ESP_OK) {
const char *err_name = esp_err_to_name(ret); const char *err_name = esp_err_to_name(ret);
ESP_LOGE(TAG, "LoRa init failed: %s", err_name); ESP_LOGE(TAG, "LoRa init failed: %s", err_name);
return; return;
} }
ESP_LOGI(TAG, "LoRa initialized"); ESP_LOGI(TAG, "LoRa initialized");
ret = sx127x_start(st->lora); ret = sx127x_start(lora);
if (ret != ESP_OK) { if (ret != ESP_OK) {
ESP_LOGI(TAG, "LoRa start failed: %d", ret); ESP_LOGI(TAG, "LoRa start failed: %d", ret);
return; return;
@ -61,11 +55,35 @@ void ugv_comms_init() {
packet_num = 0; packet_num = 0;
xTaskCreate(ugv_comms_task, "ugv_comms", 2 * 1024, st, 2, &st->task_handle); xTaskCreate(CommsClass::CommsTask, "ugv_comms", 2 * 1024, this, 2,
&task_handle);
}
void CommsClass::Lock(TickType_t ticks_to_wait) {
xSemaphoreTake(mutex, ticks_to_wait);
} }
static void ugv_comms_task(void *param) { void CommsClass::Unlock() { xSemaphoreGive(mutex); }
ugv_comms_state_t *st = (ugv_comms_state_t *)param;
int32_t CommsClass::ReadRssi() {
int32_t rssi;
sx127x_read_rssi(lora, &rssi);
return rssi;
}
uint8_t CommsClass::ReadLnaGain() {
uint8_t lna_gain;
sx127x_read_lna_gain(lora, &lna_gain);
return lna_gain;
}
void CommsClass::CommsTask(void *arg) { ((CommsClass *)arg)->RunTask(); }
void CommsClass::RunTask() {
using messages::Location;
using messages::UGV_Message;
using messages::UGV_State;
using messages::UGV_Status;
TickType_t send_period = pdMS_TO_TICKS(2000); TickType_t send_period = pdMS_TO_TICKS(2000);
TickType_t current_tick = xTaskGetTickCount(); TickType_t current_tick = xTaskGetTickCount();
@ -74,22 +92,22 @@ static void ugv_comms_task(void *param) {
esp_err_t ret; esp_err_t ret;
sx127x_rx_packet_t rx_packet; sx127x_rx_packet_t rx_packet;
uas::ugv::UGV_Message ugv_message; UGV_Message ugv_message;
uas::ugv::UGV_Status *status = ugv_message.mutable_status(); UGV_Status *status = ugv_message.mutable_status();
uas::ugv::Location * location = status->mutable_location(); Location * location = status->mutable_location();
location->set_fix_quality(0); location->set_fix_quality(0);
location->set_latitude(43.65); location->set_latitude(43.65);
location->set_longitude(-116.20); location->set_longitude(-116.20);
location->set_altitude(2730); location->set_altitude(2730);
status->set_state(uas::ugv::UGV_State::IDLE); status->set_state(UGV_State::IDLE);
while (true) { while (true) {
TickType_t delay_ticks = next_send - current_tick; TickType_t delay_ticks = next_send - current_tick;
ret = sx127x_recv_packet(st->lora, &rx_packet, delay_ticks); ret = sx127x_recv_packet(lora, &rx_packet, delay_ticks);
current_tick = xTaskGetTickCount(); current_tick = xTaskGetTickCount();
if (ret == ESP_OK) { if (ret == ESP_OK) {
ugv_comms_handle_packet(st, &rx_packet); HandlePacket(&rx_packet);
sx127x_packet_rx_free(&rx_packet); sx127x_packet_rx_free(&rx_packet);
} }
@ -100,7 +118,7 @@ static void ugv_comms_task(void *param) {
packet_num++; packet_num++;
auto str = ugv_message.SerializeAsString(); auto str = ugv_message.SerializeAsString();
ret = sx127x_send_packet(st->lora, str.c_str(), str.size(), ret = sx127x_send_packet(lora, str.c_str(), str.size(),
0); // 0 means error if queue full 0); // 0 means error if queue full
// ret = sx127x_send_packet_pb(st->lora, uas_ugv_UGV_Message_fields, // ret = sx127x_send_packet_pb(st->lora, uas_ugv_UGV_Message_fields,
// &ugv_message, // &ugv_message,
@ -116,30 +134,31 @@ static void ugv_comms_task(void *param) {
} }
} }
static void ugv_comms_handle_packet(ugv_comms_state_t * st, void CommsClass::HandlePacket(sx127x_rx_packet_t *rx_packet) {
sx127x_rx_packet_t *rx_packet) { using messages::GroundMessage;
ESP_LOGI(TAG, "lora received packet (len %d, rssi: %d, snr: %f): %.*s\n", 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->rssi, rx_packet->snr * 0.25f,
rx_packet->data_len, rx_packet->data); rx_packet->data_len, rx_packet->data);
xSemaphoreTake(st->mutex, portMAX_DELAY); xSemaphoreTake(mutex, portMAX_DELAY);
st->last_packet_tick = xTaskGetTickCount(); last_packet_tick = xTaskGetTickCount();
st->last_packet_rssi = rx_packet->rssi; last_packet_rssi = rx_packet->rssi;
st->last_packet_snr = rx_packet->snr; last_packet_snr = rx_packet->snr;
xSemaphoreGive(st->mutex); xSemaphoreGive(mutex);
uas::ugv::GroundMessage ground_message; GroundMessage ground_message;
bool pb_ret;
pb_ret = ground_message.ParseFromArray(rx_packet->data, rx_packet->data_len); bool pb_ret =
ground_message.ParseFromArray(rx_packet->data, rx_packet->data_len);
if (!pb_ret) { if (!pb_ret) {
ESP_LOGE(TAG, "rx invalid protobuf"); ESP_LOGE(TAG, "rx invalid protobuf");
return; return;
} }
switch (ground_message.ground_message_case()) { switch (ground_message.ground_message_case()) {
case uas::ugv::GroundMessage::kCommand: case GroundMessage::kCommand:
ugv_comms_handle_command(st, &ground_message.command()); HandleCommand(ground_message.command());
break; break;
default: default:
ESP_LOGE(TAG, "invalid ground message: %d", ESP_LOGE(TAG, "invalid ground message: %d",
@ -148,14 +167,16 @@ static void ugv_comms_handle_packet(ugv_comms_state_t * st,
} }
} }
static void ugv_comms_handle_command(ugv_comms_state_t * st, void CommsClass::HandleCommand(const messages::GroundCommand &command) {
const uas::ugv::GroundCommand *command) { ESP_LOGI(TAG, "rx command id %d type %d", command.id(), command.type());
ESP_LOGI(TAG, "rx command id %d type %d", command->id(), command->type());
// TODO: handle command // TODO: handle command
uas::ugv::UGV_Message ugv_message; messages::UGV_Message ugv_message;
ugv_message.set_command_ack(command->id()); ugv_message.set_command_ack(command.id());
std::string str = ugv_message.SerializeAsString(); std::string str = ugv_message.SerializeAsString();
sx127x_send_packet(st->lora, str.c_str(), str.size(), 0); sx127x_send_packet(lora, str.c_str(), str.size(), 0);
} }
} // namespace comms
} // namespace ugv

32
main/ugv_comms.h
Unescape View File

@ -1,32 +0,0 @@
#pragma once
#include <freertos/FreeRTOS.h>
#include <freertos/semphr.h>
#include <freertos/task.h>
#include "messages.pb.h"
#include "sx127x_driver.h"
#ifdef __cplusplus
extern "C" {
#endif
typedef struct ugv_comms_state_s {
sx127x_hndl lora;
TaskHandle_t task_handle;
SemaphoreHandle_t mutex;
uas::ugv::Location location;
uas::ugv::UGV_State ugv_state;
TickType_t last_packet_tick;
int32_t last_packet_rssi;
int8_t last_packet_snr;
} ugv_comms_state_t;
extern ugv_comms_state_t ugv_comms_state;
void ugv_comms_init();
#ifdef __cplusplus
}
#endif

50
main/ugv_comms.hh
Unescape View File

@ -0,0 +1,50 @@
#pragma once
#include <freertos/FreeRTOS.h>
#include <freertos/semphr.h>
#include <freertos/task.h>
#include "messages.pb.h"
#include "sx127x_driver.h"
namespace ugv {
namespace comms {
namespace messages = uas::ugv::messages;
class CommsClass {
public:
CommsClass();
void Init();
void Lock(TickType_t ticks_to_wait = pdMS_TO_TICKS(1000));
void Unlock();
int32_t ReadRssi();
uint8_t ReadLnaGain();
public:
SemaphoreHandle_t mutex;
messages::Location location;
messages::UGV_State ugv_state;
TickType_t last_packet_tick;
int32_t last_packet_rssi;
int8_t last_packet_snr;
private:
sx127x_hndl lora;
TaskHandle_t task_handle;
uint16_t packet_num;
void RunTask();
void HandlePacket(sx127x_rx_packet_t* packet);
void HandleCommand(const messages::GroundCommand& command);
static void CommsTask(void* arg);
};
extern CommsClass Comms;
} // namespace comms
} // namespace ugv

23
main/ugv_main.cc
Unescape View File

@ -3,15 +3,14 @@
#include <u8g2.h> #include <u8g2.h>
#include "U8g2lib.hh" #include "U8g2lib.hh"
#include "sx127x_driver.h" #include "ugv_comms.hh"
#include "sx127x_registers.h"
#include "ugv_comms.h"
#include "ugv_config.h" #include "ugv_config.h"
#include "ugv_io.hh" #include "ugv_io.hh"
namespace ugv { namespace ugv {
using ugv::io::IO; using ugv::io::IO;
using ugv::comms::Comms;
static const char *TAG = "ugv_main"; static const char *TAG = "ugv_main";
@ -28,7 +27,7 @@ void setup(void) {
ESP_LOGI(TAG, "setup"); ESP_LOGI(TAG, "setup");
setup_oled(); setup_oled();
ugv_comms_init(); Comms.Init();
IO.Init(); IO.Init();
} }
@ -48,14 +47,14 @@ void loop(void) {
// ESP_LOGI(TAG, "inputs %s", inputs.ToString()); // ESP_LOGI(TAG, "inputs %s", inputs.ToString());
oled->firstPage(); oled->firstPage();
sx127x_read_rssi(ugv_comms_state.lora, &lora_rssi); lora_rssi = Comms.ReadRssi();
sx127x_read_lna_gain(ugv_comms_state.lora, &lora_lna_gain); lora_lna_gain = Comms.ReadLnaGain();
xSemaphoreTake(ugv_comms_state.mutex, portMAX_DELAY); Comms.Lock();
last_packet_tick = ugv_comms_state.last_packet_tick; last_packet_tick = Comms.last_packet_tick;
last_packet_rssi = ugv_comms_state.last_packet_rssi; last_packet_rssi = Comms.last_packet_rssi;
last_packet_snr = ugv_comms_state.last_packet_snr; last_packet_snr = Comms.last_packet_snr;
xSemaphoreGive(ugv_comms_state.mutex); Comms.Unlock();
do { do {
oled->drawRFrame(0, 0, OLED_W, OLED_H, 4); oled->drawRFrame(0, 0, OLED_W, OLED_H, 4);

Write Preview
Loading…
Cancel
Save