mirror of
https://github.com/BotChain-Robots/firmware.git
synced 2026-07-08 09:37:21 +02:00
Re-enable rmt
This commit is contained in:
@@ -1,162 +1,163 @@
|
||||
#include <iostream>
|
||||
|
||||
#include "CommunicationRouter.h"
|
||||
#include "AngleControlMessageBuilder.h"
|
||||
#include "include/wireless/mDNSDiscoveryService.h"
|
||||
#include "CommunicationRouter.h"
|
||||
#include "MPIMessageBuilder.h"
|
||||
#include "include/wireless/WifiManager.h"
|
||||
#include "freertos/FreeRTOS.h"
|
||||
#include "Tables.h"
|
||||
#include "PtrQueue.h"
|
||||
#include "OrientationDetection.h"
|
||||
#include "PtrQueue.h"
|
||||
#include "Tables.h"
|
||||
#include "constants/module.h"
|
||||
#include "freertos/FreeRTOS.h"
|
||||
#include "freertos/idf_additions.h"
|
||||
#include "freertos/projdefs.h"
|
||||
#include "include/wireless/WifiManager.h"
|
||||
#include "include/wireless/mDNSDiscoveryService.h"
|
||||
|
||||
#define TAG "CommunicationRouter"
|
||||
|
||||
CommunicationRouter::~CommunicationRouter() {
|
||||
vTaskDelete(m_router_thread);
|
||||
}
|
||||
CommunicationRouter::~CommunicationRouter() { vTaskDelete(m_router_thread); }
|
||||
|
||||
// todo: we really need to change all char to uint8_t everywhere
|
||||
// todo: get rid of copying going on, need to pass around sharedptrs/uniqueptrs
|
||||
|
||||
[[noreturn]] void CommunicationRouter::router_thread(void *args) {
|
||||
const auto that = static_cast<CommunicationRouter *>(args);
|
||||
const auto that = static_cast<CommunicationRouter *>(args);
|
||||
|
||||
while (true) {
|
||||
const auto buffer = that->m_tcp_rx_queue->dequeue();
|
||||
ESP_LOGD(TAG, "Got message from TCP");
|
||||
that->route(buffer->data(), buffer->size());
|
||||
}
|
||||
while (true) {
|
||||
const auto buffer = that->m_tcp_rx_queue->dequeue();
|
||||
ESP_LOGD(TAG, "Got message from TCP");
|
||||
that->route(buffer->data(), buffer->size());
|
||||
}
|
||||
}
|
||||
|
||||
[[noreturn]] void CommunicationRouter::link_layer_thread(void *args) {
|
||||
const auto* params = static_cast<link_layer_thread_params *>(args);
|
||||
const auto that = params->router;
|
||||
const auto channel = params->channel;
|
||||
delete params;
|
||||
const auto that = static_cast<CommunicationRouter *>(args);
|
||||
|
||||
size_t bytes_received = 0;
|
||||
that->m_data_link_manager->start_receive_frames(channel);
|
||||
while (true) {
|
||||
char buffer[512];
|
||||
const auto err = that->m_data_link_manager->receive(reinterpret_cast<uint8_t *>(buffer), 512, &bytes_received, channel);
|
||||
that->m_data_link_manager->start_receive_frames(channel);
|
||||
while (true) {
|
||||
for (uint8_t channel = 0;
|
||||
channel <
|
||||
MODULE_TO_NUM_CHANNELS_MAP[that->m_config_manager.get_module_type()];
|
||||
channel++) {
|
||||
uint16_t frame_size = 0;
|
||||
FrameHeader frame_header{};
|
||||
if (ESP_OK != that->m_data_link_manager->async_receive_info(
|
||||
&frame_size, &frame_header, channel) ||
|
||||
0 == frame_size) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// todo: do we only want one thread ever doing this?
|
||||
if (std::chrono::system_clock::now() - that->m_last_leader_updated > std::chrono::seconds(15)) {
|
||||
that->m_last_leader_updated = std::chrono::system_clock::now();
|
||||
ESP_LOGI(TAG, "Updating leader");
|
||||
that->update_leader();
|
||||
}
|
||||
|
||||
if (ESP_OK != err || bytes_received < 1) {
|
||||
continue;
|
||||
}
|
||||
|
||||
ESP_LOGD(TAG, "Got message from RMT");
|
||||
that->route(reinterpret_cast<uint8_t *>(buffer), bytes_received);
|
||||
std::vector<uint8_t> data{};
|
||||
data.resize(frame_size);
|
||||
that->m_data_link_manager->async_receive(data.data(), frame_size,
|
||||
&frame_header, channel);
|
||||
that->route(data.data(), frame_size);
|
||||
}
|
||||
|
||||
vTaskDelay(pdMS_TO_TICKS(50));
|
||||
}
|
||||
}
|
||||
|
||||
int CommunicationRouter::send_msg(char* buffer, const size_t length) const {
|
||||
ESP_LOGD(TAG, "Got message from application");
|
||||
route(reinterpret_cast<uint8_t *>(buffer), length);
|
||||
return 0;
|
||||
int CommunicationRouter::send_msg(char *buffer, const size_t length) const {
|
||||
ESP_LOGD(TAG, "Got message from application");
|
||||
route(reinterpret_cast<uint8_t *>(buffer), length);
|
||||
return 0;
|
||||
}
|
||||
|
||||
void CommunicationRouter::update_leader() {
|
||||
RIPRow_public table[RIP_MAX_ROUTES];
|
||||
size_t table_size = RIP_MAX_ROUTES;
|
||||
this->m_data_link_manager->get_routing_table(table, &table_size);
|
||||
RIPRow_public table[RIP_MAX_ROUTES];
|
||||
size_t table_size = RIP_MAX_ROUTES;
|
||||
this->m_data_link_manager->get_routing_table(table, &table_size);
|
||||
|
||||
// Leader election (just get the highest id in rip)
|
||||
std::vector<int> connected_module_ids;
|
||||
uint8_t max = m_module_id;
|
||||
for (int i = 0; i < table_size; i++) {
|
||||
const auto id = table[i].info.board_id;
|
||||
connected_module_ids.emplace_back(id);
|
||||
if (id > max) { // todo: change this to be correct
|
||||
max = id;
|
||||
}
|
||||
// Leader election (just get the highest id in rip)
|
||||
std::vector<int> connected_module_ids;
|
||||
uint8_t max = m_module_id;
|
||||
for (int i = 0; i < table_size; i++) {
|
||||
const auto id = table[i].info.board_id;
|
||||
connected_module_ids.emplace_back(id);
|
||||
if (id > max) { // todo: change this to be correct
|
||||
max = id;
|
||||
}
|
||||
}
|
||||
|
||||
// Leader has changed, we may need to change PC connection state
|
||||
if (this->m_leader != max) {
|
||||
if (max == m_module_id) {
|
||||
m_pc_connection->connect();
|
||||
m_lossless_server->startup();
|
||||
m_lossy_server->startup();
|
||||
} else if (this->m_leader == m_module_id) {
|
||||
m_pc_connection->disconnect();
|
||||
m_lossless_server->shutdown();
|
||||
m_lossless_server->shutdown();
|
||||
}
|
||||
// Leader has changed, we may need to change PC connection state
|
||||
if (this->m_leader != max) {
|
||||
if (max == m_module_id) {
|
||||
m_pc_connection->connect();
|
||||
m_lossless_server->startup();
|
||||
m_lossy_server->startup();
|
||||
} else if (this->m_leader == m_module_id) {
|
||||
m_pc_connection->disconnect();
|
||||
m_lossless_server->shutdown();
|
||||
m_lossless_server->shutdown();
|
||||
}
|
||||
}
|
||||
|
||||
this->m_leader = max;
|
||||
this->m_leader = max;
|
||||
|
||||
if (this->m_leader == m_module_id) {
|
||||
this->m_discovery_service->set_connected_boards(connected_module_ids);
|
||||
}
|
||||
if (this->m_leader == m_module_id) {
|
||||
this->m_discovery_service->set_connected_boards(connected_module_ids);
|
||||
}
|
||||
}
|
||||
|
||||
void CommunicationRouter::route(uint8_t* buffer, const size_t length) const {
|
||||
flatbuffers::Verifier verifier(buffer, length);
|
||||
// This could be moved to just be called on wireline data to save cpu cycles.
|
||||
if (bool ok = Messaging::VerifyMPIMessageBuffer(verifier); !ok) {
|
||||
ESP_LOGW(TAG, "route: got an invalid MPI message, disregarding");
|
||||
return;
|
||||
}
|
||||
void CommunicationRouter::route(uint8_t *buffer, const size_t length) const {
|
||||
flatbuffers::Verifier verifier(buffer, length);
|
||||
// This could be moved to just be called on wireline data to save cpu cycles.
|
||||
if (bool ok = Messaging::VerifyMPIMessageBuffer(verifier); !ok) {
|
||||
ESP_LOGW(TAG, "route: got an invalid MPI message, disregarding");
|
||||
return;
|
||||
}
|
||||
|
||||
if (const auto& mpi_message = Flatbuffers::MPIMessageBuilder::parse_mpi_message(buffer); mpi_message->destination() == m_module_id) {
|
||||
ESP_LOGD(TAG, "Routing to this module [dest: %d, length: %d]", static_cast<int>(mpi_message->destination()), length);
|
||||
this->m_rx_callback(reinterpret_cast<char *>(buffer), 512);
|
||||
} else if (mpi_message->destination() == PC_ADDR && this->m_leader == m_module_id) {
|
||||
ESP_LOGD(TAG, "Routing to wifi [dest: %d, length: %d]", static_cast<int>(mpi_message->destination()), length);
|
||||
if (mpi_message->is_durable()) {
|
||||
this->m_lossless_server->send_msg(reinterpret_cast<char *>(buffer), 512);
|
||||
} else {
|
||||
this->m_lossy_server->send_msg(reinterpret_cast<char *>(buffer), 512);
|
||||
}
|
||||
} else if (mpi_message->destination() == PC_ADDR) {
|
||||
ESP_LOGD(TAG, "Routing to wireline for wifi [dest: %d, length: %d]", static_cast<int>(mpi_message->destination()), length);
|
||||
this->m_data_link_manager->send(this->m_leader, buffer, length, FrameType::MOTOR_TYPE, 0);
|
||||
}else {
|
||||
ESP_LOGD(TAG, "Routing to wireline [dest: %d, length: %d]", static_cast<int>(mpi_message->destination()), length);
|
||||
this->m_data_link_manager->send(mpi_message->destination(), buffer, length, FrameType::MOTOR_TYPE, 0);
|
||||
}
|
||||
}
|
||||
|
||||
std::pair<std::vector<uint8_t>, std::vector<Orientation>> CommunicationRouter::get_physically_connected_modules() const {
|
||||
std::vector<RIPRow_public> table;
|
||||
table.resize(RIP_MAX_ROUTES);
|
||||
size_t table_size = RIP_MAX_ROUTES * sizeof(RIPRow_public);
|
||||
m_data_link_manager->get_routing_table(table.data(), &table_size);
|
||||
|
||||
std::vector<uint8_t> connected_module_ids;
|
||||
std::vector<Orientation> connected_module_orientations;
|
||||
connected_module_ids.resize(MAX_WIRED_CONNECTIONS);
|
||||
connected_module_orientations.resize(MAX_WIRED_CONNECTIONS);
|
||||
|
||||
for (int i = 0; i < MAX_WIRED_CONNECTIONS; i++) {
|
||||
connected_module_ids[i] = 0; // this is not the PC ID here, marking as nc.
|
||||
}
|
||||
|
||||
for (int i = 0; i < table_size; i++) {
|
||||
if (table[i].info.hops == 1 && table[i].channel < MAX_WIRED_CONNECTIONS) {
|
||||
connected_module_ids[table[i].channel] = table[i].info.board_id;
|
||||
}
|
||||
}
|
||||
|
||||
if (const auto id = connected_module_ids[0]; 0 == id) {
|
||||
connected_module_orientations[0] = Orientation_Deg0;
|
||||
if (const auto &mpi_message =
|
||||
Flatbuffers::MPIMessageBuilder::parse_mpi_message(buffer);
|
||||
mpi_message->destination() == m_module_id) {
|
||||
this->m_rx_callback(reinterpret_cast<char *>(buffer), 512);
|
||||
} else if (mpi_message->destination() == PC_ADDR &&
|
||||
this->m_leader == m_module_id) {
|
||||
if (mpi_message->is_durable()) {
|
||||
this->m_lossless_server->send_msg(reinterpret_cast<char *>(buffer), 512);
|
||||
} else {
|
||||
connected_module_orientations[0] = OrientationDetection::get_orientation(0);
|
||||
this->m_lossy_server->send_msg(reinterpret_cast<char *>(buffer), 512);
|
||||
}
|
||||
} else if (mpi_message->destination() == PC_ADDR) {
|
||||
this->m_data_link_manager->send(this->m_leader, buffer, length,
|
||||
FrameType::MOTOR_TYPE, 0);
|
||||
} else {
|
||||
this->m_data_link_manager->send(mpi_message->destination(), buffer, length,
|
||||
FrameType::MOTOR_TYPE, 0);
|
||||
}
|
||||
}
|
||||
|
||||
return { connected_module_ids, connected_module_orientations };
|
||||
std::pair<std::vector<uint8_t>, std::vector<Orientation>>
|
||||
CommunicationRouter::get_physically_connected_modules() const {
|
||||
std::vector<RIPRow_public> table;
|
||||
table.resize(RIP_MAX_ROUTES);
|
||||
size_t table_size = RIP_MAX_ROUTES * sizeof(RIPRow_public);
|
||||
m_data_link_manager->get_routing_table(table.data(), &table_size);
|
||||
|
||||
std::vector<uint8_t> connected_module_ids;
|
||||
std::vector<Orientation> connected_module_orientations;
|
||||
connected_module_ids.resize(MAX_WIRED_CONNECTIONS);
|
||||
connected_module_orientations.resize(MAX_WIRED_CONNECTIONS);
|
||||
|
||||
for (int i = 0; i < MAX_WIRED_CONNECTIONS; i++) {
|
||||
connected_module_ids[i] = 0; // this is not the PC ID here, marking as nc.
|
||||
}
|
||||
|
||||
for (int i = 0; i < table_size; i++) {
|
||||
if (table[i].info.hops == 1 && table[i].channel < MAX_WIRED_CONNECTIONS) {
|
||||
connected_module_ids[table[i].channel] = table[i].info.board_id;
|
||||
}
|
||||
}
|
||||
|
||||
if (const auto id = connected_module_ids[0]; 0 == id) {
|
||||
connected_module_orientations[0] = Orientation_Deg0;
|
||||
} else {
|
||||
connected_module_orientations[0] = OrientationDetection::get_orientation(0);
|
||||
}
|
||||
|
||||
return {connected_module_ids, connected_module_orientations};
|
||||
}
|
||||
|
||||
[[nodiscard]] uint8_t CommunicationRouter::get_leader() const {
|
||||
return this->m_leader;
|
||||
return this->m_leader;
|
||||
}
|
||||
|
||||
@@ -7,79 +7,77 @@
|
||||
#ifndef COMMUNICATIONROUTER_H
|
||||
#define COMMUNICATIONROUTER_H
|
||||
|
||||
#include <chrono>
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <chrono>
|
||||
|
||||
#include "CommunicationFactory.h"
|
||||
#include "IDiscoveryService.h"
|
||||
#include "ConfigManager.h"
|
||||
#include "OrientationDetection.h"
|
||||
#include "wireless/WifiManager.h"
|
||||
#include "wireless/TCPServer.h"
|
||||
#include "DataLinkManager.h"
|
||||
#include "constants/module.h"
|
||||
#include "IDiscoveryService.h"
|
||||
#include "OrientationDetection.h"
|
||||
#include "PtrQueue.h"
|
||||
#include "constants/module.h"
|
||||
#include "wireless/TCPServer.h"
|
||||
#include "wireless/WifiManager.h"
|
||||
|
||||
class CommunicationRouter {
|
||||
|
||||
class link_layer_thread_params {
|
||||
public:
|
||||
link_layer_thread_params(CommunicationRouter* router, const uint8_t channel) : router(router), channel(channel) {};
|
||||
CommunicationRouter *router;
|
||||
uint8_t channel;
|
||||
};
|
||||
|
||||
public:
|
||||
explicit CommunicationRouter(const std::function<void(char*, int)> &rx_callback)
|
||||
: m_tcp_rx_queue(std::make_shared<PtrQueue<std::vector<uint8_t>>>(10)),
|
||||
m_rx_callback(rx_callback),
|
||||
m_config_manager(ConfigManager::get_instance()),
|
||||
m_pc_connection(CommunicationFactory::create_connection_manager(m_config_manager.get_communication_method())),
|
||||
m_lossless_server(CommunicationFactory::create_lossless_server(m_config_manager.get_communication_method(), m_tcp_rx_queue)),
|
||||
m_lossy_server(CommunicationFactory::create_lossy_server(m_config_manager.get_communication_method(), m_tcp_rx_queue)),
|
||||
m_data_link_manager(std::make_unique<DataLinkManager>(m_config_manager.get_module_id(), MODULE_TO_NUM_CHANNELS_MAP[m_config_manager.get_module_type()])),
|
||||
m_module_id(m_config_manager.get_module_id()),
|
||||
m_last_leader_updated(std::chrono::system_clock::now()),
|
||||
m_discovery_service(CommunicationFactory::create_discovery_service(m_config_manager.get_communication_method())){
|
||||
OrientationDetection::init();
|
||||
update_leader();
|
||||
explicit CommunicationRouter(
|
||||
const std::function<void(char *, int)> &rx_callback)
|
||||
: m_tcp_rx_queue(std::make_shared<PtrQueue<std::vector<uint8_t>>>(10)),
|
||||
m_rx_callback(rx_callback),
|
||||
m_config_manager(ConfigManager::get_instance()),
|
||||
m_pc_connection(CommunicationFactory::create_connection_manager(
|
||||
m_config_manager.get_communication_method())),
|
||||
m_lossless_server(CommunicationFactory::create_lossless_server(
|
||||
m_config_manager.get_communication_method(), m_tcp_rx_queue)),
|
||||
m_lossy_server(CommunicationFactory::create_lossy_server(
|
||||
m_config_manager.get_communication_method(), m_tcp_rx_queue)),
|
||||
m_data_link_manager(std::make_unique<DataLinkManager>(
|
||||
m_config_manager.get_module_id(),
|
||||
MODULE_TO_NUM_CHANNELS_MAP[m_config_manager.get_module_type()])),
|
||||
m_module_id(m_config_manager.get_module_id()),
|
||||
m_last_leader_updated(std::chrono::system_clock::now()),
|
||||
m_discovery_service(CommunicationFactory::create_discovery_service(
|
||||
m_config_manager.get_communication_method())) {
|
||||
OrientationDetection::init();
|
||||
update_leader();
|
||||
|
||||
xTaskCreate(router_thread, "router", 4096, this, 3, &this->m_router_thread);
|
||||
xTaskCreate(router_thread, "router", 4096, this, 3, &this->m_router_thread);
|
||||
xTaskCreate(link_layer_thread, "router_rmt", 4096, this, 3,
|
||||
&this->m_link_layer_thread);
|
||||
}
|
||||
|
||||
// const auto num_channels = MODULE_TO_NUM_CHANNELS_MAP[m_config_manager.get_module_type()];
|
||||
// this->m_link_layer_threads.resize(num_channels);
|
||||
// for (int i = 0; i < num_channels; i++) {
|
||||
// auto *params = new link_layer_thread_params(this, i);
|
||||
// xTaskCreate(link_layer_thread, "router_rmt", 4096, params, 3, &this->m_link_layer_threads[i]);
|
||||
// }
|
||||
}
|
||||
~CommunicationRouter();
|
||||
|
||||
~CommunicationRouter();
|
||||
[[noreturn]] static void router_thread(void *args);
|
||||
[[noreturn]] static void link_layer_thread(void *args);
|
||||
int send_msg(char *buffer, size_t length) const;
|
||||
void update_leader();
|
||||
void route(uint8_t *buffer, size_t length) const;
|
||||
[[nodiscard]] std::pair<std::vector<uint8_t>, std::vector<Orientation>>
|
||||
get_physically_connected_modules() const;
|
||||
[[nodiscard]] uint8_t get_leader() const;
|
||||
|
||||
[[noreturn]] static void router_thread(void *args);
|
||||
[[noreturn]] static void link_layer_thread(void *args);
|
||||
int send_msg(char* buffer, size_t length) const;
|
||||
void update_leader();
|
||||
void route(uint8_t *buffer, size_t length) const;
|
||||
[[nodiscard]] std::pair<std::vector<uint8_t>, std::vector<Orientation>> get_physically_connected_modules() const;
|
||||
[[nodiscard]] uint8_t get_leader() const;
|
||||
// todo: does this really need to be here (so i can access from thread)?
|
||||
std::shared_ptr<PtrQueue<std::vector<uint8_t>>>
|
||||
m_tcp_rx_queue; // todo: this should probably be thread safe
|
||||
std::function<void(char *, int)> m_rx_callback;
|
||||
|
||||
// todo: does this really need to be here (so i can access from thread)?
|
||||
std::shared_ptr<PtrQueue<std::vector<uint8_t>>> m_tcp_rx_queue; // todo: this should probably be thread safe
|
||||
std::function<void(char*, int)> m_rx_callback;
|
||||
private:
|
||||
TaskHandle_t m_router_thread = nullptr;
|
||||
ConfigManager &m_config_manager;
|
||||
std::unique_ptr<IConnectionManager> m_pc_connection;
|
||||
std::vector<TaskHandle_t> m_link_layer_threads;
|
||||
std::unique_ptr<IRPCServer> m_lossless_server;
|
||||
std::unique_ptr<IRPCServer> m_lossy_server;
|
||||
std::unique_ptr<DataLinkManager> m_data_link_manager;
|
||||
uint8_t m_leader = 0;
|
||||
uint8_t m_module_id;
|
||||
std::chrono::time_point<std::chrono::system_clock> m_last_leader_updated;
|
||||
std::unique_ptr<IDiscoveryService> m_discovery_service;
|
||||
TaskHandle_t m_router_thread = nullptr;
|
||||
TaskHandle_t m_link_layer_thread = nullptr;
|
||||
ConfigManager &m_config_manager;
|
||||
std::unique_ptr<IConnectionManager> m_pc_connection;
|
||||
std::unique_ptr<IRPCServer> m_lossless_server;
|
||||
std::unique_ptr<IRPCServer> m_lossy_server;
|
||||
std::unique_ptr<DataLinkManager> m_data_link_manager;
|
||||
uint8_t m_leader = 0;
|
||||
uint8_t m_module_id;
|
||||
std::chrono::time_point<std::chrono::system_clock> m_last_leader_updated;
|
||||
std::unique_ptr<IDiscoveryService> m_discovery_service;
|
||||
};
|
||||
|
||||
#endif //COMMUNICATIONROUTER_H
|
||||
#endif // COMMUNICATIONROUTER_H
|
||||
|
||||
@@ -4,15 +4,15 @@ file(GLOB_RECURSE ALL_SRCS
|
||||
"${CMAKE_CURRENT_SOURCE_DIR}/*.S"
|
||||
)
|
||||
|
||||
if(DEFINED RMT_TEST AND RMT_TEST EQUAL 1)
|
||||
if(DEFINED RMT_TEST AND RMT_TEST EQUAL 1)
|
||||
message(STATUS "Building for testing RMT/Link Layer Functionality with main_rmt_test.cpp")
|
||||
list(REMOVE_ITEM ALL_SRCS
|
||||
"${CMAKE_CURRENT_SOURCE_DIR}/main.cpp"
|
||||
)
|
||||
idf_build_set_property(COMPILE_DEFINITIONS RMT_TEST APPEND)
|
||||
idf_build_set_property(COMPILE_DEFINITIONS RMT_TEST APPEND)
|
||||
else()
|
||||
list(REMOVE_ITEM ALL_SRCS
|
||||
"${CMAKE_CURRENT_SOURCE_DIR}/main.cpp"
|
||||
"${CMAKE_CURRENT_SOURCE_DIR}/main_rmt_test.cpp"
|
||||
)
|
||||
endif()
|
||||
|
||||
@@ -34,4 +34,3 @@ if(DEFINED RECEIVE_ONLY AND RECEIVE_ONLY)
|
||||
message(STATUS "Building for receive only")
|
||||
idf_build_set_property(COMPILE_DEFINITIONS RECEIVE_ONLY=${RECEIVE_ONLY} APPEND)
|
||||
endif()
|
||||
|
||||
|
||||
Reference in New Issue
Block a user