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

Reimplement comms with c++

Browse Source
This commit is contained in:
Alex Mikhalev 2019-01-17 22:34:34 -08:00
parent 611bade298
commit 0a6c0ebd40
5 changed files with 136 additions and 98 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
main/messages.proto
View File

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

129
main/ugv_comms.cc
View File

@ -1,29 +1,30 @@
#include "ugv_comms.h"
#include "ugv_comms.hh"
#include "ugv_config.h"
#include <esp_log.h>
#include "messages.pb.h"
#include "sx127x_registers.h"
ugv_comms_state_t ugv_comms_state;
namespace ugv {
namespace comms {
CommsClass Comms;
static const char *TAG = "ugv_comms";
static void ugv_comms_task(void *arg);
static uint16_t packet_num;
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();
}
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() {
ugv_comms_state_t *st = &ugv_comms_state;
st->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.miso_io_num = (gpio_num_t)LORA_MISO;
lora_config.mosi_io_num = (gpio_num_t)LORA_MOSI;
@ -39,21 +40,14 @@ void ugv_comms_init() {
esp_err_t ret;
uas::ugv::Location loc;
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);
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(st->lora);
ret = sx127x_start(lora);
if (ret != ESP_OK) {
ESP_LOGI(TAG, "LoRa start failed: %d", ret);
return;
@ -61,11 +55,35 @@ void ugv_comms_init() {
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);
}
static void ugv_comms_task(void *param) {
ugv_comms_state_t *st = (ugv_comms_state_t *)param;
void CommsClass::Lock(TickType_t ticks_to_wait) {
xSemaphoreTake(mutex, ticks_to_wait);
}
void CommsClass::Unlock() { xSemaphoreGive(mutex); }
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 current_tick = xTaskGetTickCount();
@ -74,22 +92,22 @@ static void ugv_comms_task(void *param) {
esp_err_t ret;
sx127x_rx_packet_t rx_packet;
uas::ugv::UGV_Message ugv_message;
uas::ugv::UGV_Status *status = ugv_message.mutable_status();
uas::ugv::Location * location = status->mutable_location();
UGV_Message ugv_message;
UGV_Status *status = ugv_message.mutable_status();
Location * location = status->mutable_location();
location->set_fix_quality(0);
location->set_latitude(43.65);
location->set_longitude(-116.20);
location->set_altitude(2730);
status->set_state(uas::ugv::UGV_State::IDLE);
status->set_state(UGV_State::IDLE);
while (true) {
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();
if (ret == ESP_OK) {
ugv_comms_handle_packet(st, &rx_packet);
HandlePacket(&rx_packet);
sx127x_packet_rx_free(&rx_packet);
}
@ -100,7 +118,7 @@ static void ugv_comms_task(void *param) {
packet_num++;
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
// ret = sx127x_send_packet_pb(st->lora, uas_ugv_UGV_Message_fields,
// &ugv_message,
@ -116,30 +134,31 @@ static void ugv_comms_task(void *param) {
}
}
static void ugv_comms_handle_packet(ugv_comms_state_t * st,
sx127x_rx_packet_t *rx_packet) {
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);
xSemaphoreTake(st->mutex, portMAX_DELAY);
st->last_packet_tick = xTaskGetTickCount();
st->last_packet_rssi = rx_packet->rssi;
st->last_packet_snr = rx_packet->snr;
xSemaphoreGive(st->mutex);
xSemaphoreTake(mutex, portMAX_DELAY);
last_packet_tick = xTaskGetTickCount();
last_packet_rssi = rx_packet->rssi;
last_packet_snr = rx_packet->snr;
xSemaphoreGive(mutex);
uas::ugv::GroundMessage ground_message;
bool pb_ret;
GroundMessage ground_message;
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) {
ESP_LOGE(TAG, "rx invalid protobuf");
return;
}
switch (ground_message.ground_message_case()) {
case uas::ugv::GroundMessage::kCommand:
ugv_comms_handle_command(st, &ground_message.command());
case GroundMessage::kCommand:
HandleCommand(ground_message.command());
break;
default:
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,
const uas::ugv::GroundCommand *command) {
ESP_LOGI(TAG, "rx command id %d type %d", command->id(), command->type());
void CommsClass::HandleCommand(const messages::GroundCommand &command) {
ESP_LOGI(TAG, "rx command id %d type %d", command.id(), command.type());
// TODO: handle command
uas::ugv::UGV_Message ugv_message;
ugv_message.set_command_ack(command->id());
messages::UGV_Message ugv_message;
ugv_message.set_command_ack(command.id());
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
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 Normal file
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

21
main/ugv_main.cc
View File

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