botchain/firmware
1
0
Fork 0
mirror of https://github.com/BotChain-Robots/firmware.git synced 2026-07-08 09:37:21 +02:00
Code Issues Packages Projects Releases Wiki Activity

UDP implementation

Browse Source
This commit is contained in:
Johnathon Slightham
2026-01-06 14:02:56 -05:00
committed by Johnathon Slightham
parent 92ddc3faf9
commit 53d3c775ba
12 changed files with 388 additions and 86 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
components/constants/include/constants/udp.h Normal file
View File

@@ -0,0 +1,18 @@
//
// Created by Johnathon Slightham on 2025-12-21.
//
#ifndef UDP_H
#define UDP_H
#define SLEEP_AFTER_FAIL_MS 5000
#define NO_CLIENT_SLEEP_MS 400
#define RECV_MCAST "239.1.1.1"
#define RECV_PORT 3101
#define SEND_MCAST "239.1.1.2"
#define SEND_PORT 3100
#endif //UDP_H

58
components/dataLink/DataLinkRIP.cpp
View File

@@ -4,7 +4,7 @@
/**
* @brief Initializes the RIP table
*
*
*/
void DataLinkManager::init_rip(){
for (size_t i = 0; i < RIP_MAX_ROUTES; i++){
@@ -54,11 +54,11 @@ esp_err_t DataLinkManager::rip_add_entry(uint8_t board_id, uint8_t hops, uint8_t
(*entry)->ttl = RIP_TTL_START;
(*entry)->valid = 1;
// ESP_LOGI(DEBUG_LINK_TAG, "board_id %d now has hops %d from channel %d", (*entry)->info.board_id, (*entry)->info.hops, channel);
xSemaphoreGive((*entry)->row_sem);
if (uxQueueMessagesWaiting(manual_broadcasts) == 0){
bool dummy = true;
xQueueSend(manual_broadcasts, &dummy, 0); //new row - send broadcast
@@ -108,9 +108,9 @@ esp_err_t DataLinkManager::rip_update_entry(uint8_t new_hop, uint8_t channel, RI
(*entry)->channel = channel;
// ESP_LOGI(DEBUG_LINK_TAG, "updated board_id %d now has hops %d from channel %d", (*entry)->info.board_id, (*entry)->info.hops, channel);
}
(*entry)->ttl = RIP_TTL_START;
(*entry)->valid = 1;
(*entry)->valid = 1;
// ESP_LOGI(DEBUG_LINK_TAG, "refreshed board_id %d ttl", (*entry)->info.board_id);
@@ -128,10 +128,10 @@ esp_err_t DataLinkManager::rip_update_entry(uint8_t new_hop, uint8_t channel, RI
/**
* @brief Finds the board_id in the table if it exists and stores that row in `entry`
* TODO: use an unordered map instead of an array?
*
* @param board_id
* @param entry
* @return esp_err_t
*
* @param board_id
* @param entry
* @return esp_err_t
*/
esp_err_t DataLinkManager::rip_find_entry(uint8_t board_id, RIPRow** entry, bool reserve_row = false){
RIPRow* free_slot = nullptr;
@@ -144,7 +144,7 @@ esp_err_t DataLinkManager::rip_find_entry(uint8_t board_id, RIPRow** entry, bool
xSemaphoreGive(rip_table[i].row_sem);
// ESP_LOGI(DEBUG_LINK_TAG, "Found %d in table at row %d", board_id, i);
return ESP_OK;
}
}
if (rip_table[i].valid == RIP_INVALID_ROW && free_slot == nullptr){
free_slot = &rip_table[i];
}
@@ -176,10 +176,10 @@ esp_err_t DataLinkManager::rip_find_entry(uint8_t board_id, RIPRow** entry, bool
/**
* @brief Returns the associated RIP Table row by row number. Information returned is read only.
*
* @param entry
* @param row_num
* @return esp_err_t
*
* @param entry
* @param row_num
* @return esp_err_t
*/
esp_err_t DataLinkManager::rip_get_row(RIPRow** entry, uint8_t row_num){
if (entry == nullptr){
@@ -216,10 +216,10 @@ esp_err_t DataLinkManager::rip_get_row(RIPRow** entry, uint8_t row_num){
/**
* @brief Sends RIP frame
*
*
* @param broadcast True - broadcasts (sends rip table to all available channels); False - sends rip table via routing based on `dest_id`
* @param dest_id Destination board (requesting board) to send the rip table to (ignored if `broadcast is true`)
* @return esp_err_t
* @return esp_err_t
*/
esp_err_t DataLinkManager::send_rip_frame(bool broadcast, uint8_t dest_id){
//use the control frame for the demo (as the number of rows increase, we will need to use the generic frame)
@@ -236,17 +236,17 @@ esp_err_t DataLinkManager::send_rip_frame(bool broadcast, uint8_t dest_id){
for (size_t channel = 0; channel < num_channels; channel++){
for (size_t i = 0; i < RIP_MAX_ROUTES; i++){
res = rip_get_row(&entry, i);
if (res != ESP_OK){
continue;
}
if (entry == nullptr){
continue;
}
// ESP_LOGI(DEBUG_LINK_TAG, "Found entry for board %d with hops %d", entry->info.board_id, entry->info.hops);
if (entry->channel == channel){
//poisoned reverse
rip_message[message_idx++] = entry->info.board_id;
@@ -264,7 +264,7 @@ esp_err_t DataLinkManager::send_rip_frame(bool broadcast, uint8_t dest_id){
}
memset(send_data, 0, message_idx);
memcpy(send_data, rip_message, message_idx);
SchedulerMetadata metadata = {
.header = {
.preamble = START_OF_FRAME,
@@ -314,10 +314,10 @@ esp_err_t DataLinkManager::send_rip_frame(bool broadcast, uint8_t dest_id){
/**
* @brief Determines which channel to route the frame to, depending on the dest (board) id
*
* @param dest_id
* @param channel_to_send
* @return esp_err_t
*
* @param dest_id
* @param channel_to_send
* @return esp_err_t
*/
esp_err_t DataLinkManager::route_frame(uint8_t dest_id, uint8_t* channel_to_send){
RIPRow* entry = nullptr;
@@ -364,7 +364,7 @@ esp_err_t DataLinkManager::get_routing_table(RIPRow_public* table, size_t* table
table[i].info = rip_table[i].info;
table[i].channel = rip_table[i].channel;
curr_size++;
}
}
xSemaphoreGive(rip_table[i].row_sem);
}
@@ -427,7 +427,7 @@ esp_err_t DataLinkManager::get_routing_table(RIPRow_public* table, size_t* table
link_layer_obj->rip_table[i].ttl_flush = RIP_FLUSH_COUNT;
broadcast = true;
}
}
}
xSemaphoreGive(link_layer_obj->rip_table[i].row_sem);
}
@@ -452,5 +452,5 @@ void DataLinkManager::start_rip_tasks(){
ESP_LOGI(DEBUG_LINK_TAG, "Starting RIP Broadcast task");
xTaskCreate(DataLinkManager::rip_broadcast_timer_function, "RIPBroadcast", 4096, static_cast<void*>(this), 5, &rip_broadcast_task);
ESP_LOGI(DEBUG_LINK_TAG, "Starting RIP TTL task");
xTaskCreate(DataLinkManager::rip_ttl_decrement_task, "RIPTTL", 2048, static_cast<void*>(this), 5, &rip_ttl_task);
}
xTaskCreate(DataLinkManager::rip_ttl_decrement_task, "RIPTTL", 4096, static_cast<void*>(this), 5, &rip_ttl_task);
}

4
components/rpc/CommunicationFactory.cpp
View File

@@ -6,8 +6,10 @@
#include "CommunicationFactory.h"
#include "constants/tcp.h"
#include "constants/udp.h"
#include "wireless/mDNSDiscoveryService.h"
#include "wireless/TCPServer.h"
#include "wireless/UDPServer.h"
#include "wireless/WifiManager.h"
std::unique_ptr<IConnectionManager> CommunicationFactory::create_connection_manager(const CommunicationMethod type) {
@@ -31,7 +33,7 @@ std::unique_ptr<IDiscoveryService> CommunicationFactory::create_discovery_servic
std::unique_ptr<IRPCServer> CommunicationFactory::create_lossy_server(const CommunicationMethod type, const std::shared_ptr<PtrQueue<std::vector<uint8_t>>>& rx_queue) {
switch (type) {
case Wireless:
return std::make_unique<TCPServer>(TCP_PORT, rx_queue); // todo: replace with udp server
return std::make_unique<UDPServer>(RECV_PORT, SEND_PORT, rx_queue);
default:
return nullptr;
}

10
components/rpc/CommunicationRouter.cpp
View File

@@ -85,11 +85,11 @@ void CommunicationRouter::update_leader() {
if (max == m_module_id) {
m_pc_connection->connect();
m_lossless_server->startup();
// todo: BTS-22 add lossy server
m_lossy_server->startup();
} else if (this->m_leader == m_module_id) {
m_pc_connection->disconnect();
m_lossless_server->shutdown();
// todo: BTS-22 add lossy server
m_lossless_server->shutdown();
}
}
@@ -113,7 +113,11 @@ void CommunicationRouter::route(uint8_t* buffer, const size_t length) const {
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);
this->m_lossless_server->send_msg(reinterpret_cast<char *>(buffer), 512);
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);

18
components/rpc/include/CommunicationRouter.h
View File

@@ -37,6 +37,7 @@ public:
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()),
@@ -44,14 +45,14 @@ public:
OrientationDetection::init();
update_leader();
xTaskCreate(router_thread, "communication_router", 4096, this, 3, &this->m_router_thread);
xTaskCreate(router_thread, "router", 4096, this, 3, &this->m_router_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, "communication_router_rmt", 4096, params, 3, &this->m_link_layer_threads[i]);
}
// 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();
@@ -65,7 +66,7 @@ public:
[[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;
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;
@@ -73,6 +74,7 @@ private:
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;

41
components/rpc/include/wireless/UDPServer.h Normal file
View File

@@ -0,0 +1,41 @@
//
// Created by Johnathon Slightham on 2025-05-25.
//
#ifndef UDPSERVER_H
#define UDPSERVER_H
#include <memory>
#include <vector>
#include <unordered_set>
#include "freertos/FreeRTOS.h"
#include "IRPCServer.h"
#include "PtrQueue.h"
class UDPServer final : public IRPCServer {
public:
UDPServer(int rx_port, int tx_port, const std::shared_ptr<PtrQueue<std::vector<uint8_t>>>& rx_queue);
~UDPServer() override;
void startup() override;
void shutdown() override;
int send_msg(char* buffer, uint32_t length) const override;
private:
bool authenticate_client(int client_sock);
static bool is_network_connected();
[[noreturn]] static void socket_monitor_thread(void *args);
int m_tx_port;
int m_rx_port;
int m_tx_server_sock;
int m_rx_server_sock;
TaskHandle_t m_rx_task;
std::shared_ptr<PtrQueue<std::vector<uint8_t>>> m_rx_queue;
};
#endif //UDPSERVER_H

2
components/rpc/wireless/TCPServer.cpp
View File

@@ -84,7 +84,7 @@ void TCPServer::shutdown() {
constexpr int opt = 1;
setsockopt(that->m_server_sock, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt));
ESP_LOGI(TAG, "Socket created\n");
ESP_LOGI(TAG, "Socket created");
sockaddr_in server_addr = {
.sin_family = AF_INET,

214
components/rpc/wireless/UDPServer.cpp Normal file
View File

@@ -0,0 +1,214 @@
#include <memory>
#include <cstring>
#include "esp_log.h"
#include "sys/param.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_event.h"
#include "esp_netif.h"
#include "lwip/err.h"
#include "lwip/sockets.h"
#include "lwip/sys.h"
#include "lwip/netdb.h"
#include "wireless/UDPServer.h"
#include "bits/shared_ptr_base.h"
#include "constants/app_comms.h"
#include "constants/udp.h"
#include "esp_netif.h"
#define TAG "UDPServer"
#define MIN(X, Y) (((X) < (Y)) ? (X) : (Y))
// todo: - authenticate
UDPServer::UDPServer(const int rx_port, const int tx_port, const std::shared_ptr<PtrQueue<std::vector<uint8_t>>>& rx_queue) {
this->m_rx_port = rx_port;
this->m_tx_port = tx_port;
this->m_rx_task = nullptr;
this->m_rx_queue = rx_queue;
this->m_rx_server_sock = 0;
this->m_tx_server_sock = 0;
}
UDPServer::~UDPServer() {
this->shutdown();
}
void UDPServer::startup() {
ESP_LOGI(TAG, "Starting UDP server on port %d", this->m_rx_port);
if (nullptr != this->m_rx_task) {
ESP_LOGW(TAG, "Attempted to start UDP server when already started, ignoring start request");
return;
}
xTaskCreate(socket_monitor_thread, "udp_rx", 4096, this, 5, &this->m_rx_task);
}
void UDPServer::shutdown() {
ESP_LOGI(TAG, "Shutting down UDP server");
if (nullptr != this->m_rx_task) {
vTaskDelete(this->m_rx_task);
}
}
[[noreturn]] void UDPServer::socket_monitor_thread(void *args) {
const auto that = static_cast<UDPServer *>(args);
while(true) {
ESP_LOGI(TAG, "Attempting to start UDP Server on %d", that->m_rx_port);
if (!is_network_connected()) {
ESP_LOGW(TAG, "Network is disconnected");
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
continue;
}
sockaddr_in saddr = {0};
sockaddr_in from_addr = {0};
that->m_rx_server_sock = socket(AF_INET, SOCK_DGRAM, 0);
if (that->m_rx_server_sock == -1) {
ESP_LOGE(TAG, "Create UDP socket fail");
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
continue;
}
that->m_tx_server_sock = socket(AF_INET, SOCK_DGRAM, 0);
if (that->m_tx_server_sock < 0) {
ESP_LOGE(TAG, "Unable to create UDP tx socket: errno %d", errno);
close(that->m_rx_server_sock);
that->m_rx_server_sock = -1;
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
continue;
}
int reuse = 1;
if (setsockopt(that->m_rx_server_sock, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse)) < 0) {
ESP_LOGE(TAG, "Failed to set SO_REUSEADDR. Error %d", errno);
close(that->m_rx_server_sock);
close(that->m_tx_server_sock);
that->m_rx_server_sock = -1;
that->m_tx_server_sock = -1;
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
continue;
}
saddr.sin_family = AF_INET;
saddr.sin_port = htons(that->m_rx_port);
saddr.sin_addr.s_addr = htonl(INADDR_ANY);
int ret = bind(that->m_rx_server_sock, (struct sockaddr *)&saddr, sizeof(struct sockaddr_in));
if (ret < 0) {
ESP_LOGE(TAG, "Failed to bind socket. Error %d", errno);
close(that->m_rx_server_sock);
close(that->m_tx_server_sock);
that->m_rx_server_sock = -1;
that->m_tx_server_sock = -1;
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
continue;
}
struct ip_mreq imreq = {};
imreq.imr_multiaddr.s_addr = inet_addr(RECV_MCAST);
imreq.imr_interface.s_addr = htonl(INADDR_ANY);
if(setsockopt(that->m_rx_server_sock, IPPROTO_IP, IP_ADD_MEMBERSHIP, &imreq, sizeof(struct ip_mreq)) < 0) {
ESP_LOGE(TAG, "Failed to set IP_ADD_MEMBERSHIP. Error %d", errno);
close(that->m_rx_server_sock);
close(that->m_tx_server_sock);
that->m_rx_server_sock = -1;
that->m_tx_server_sock = -1;
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
continue;
}
uint32_t msg_size;
while(is_network_connected()) {
auto buffer = std::make_unique<std::vector<uint8_t>>();
buffer->resize(MAX_RX_BUFFER_SIZE + 4);
ESP_LOGI(TAG, "Before rx");
if (int len = recvfrom(that->m_rx_server_sock, buffer->data(), MAX_RX_BUFFER_SIZE, 0, nullptr, nullptr); len < 0) {
ESP_LOGE(TAG, "Error occurred during receiving: errno %d", errno);
} else if (len < 4 || len > MAX_RX_BUFFER_SIZE) {
ESP_LOGE(TAG, "Got illegal message size");
} else {
msg_size = *reinterpret_cast<uint32_t*>(buffer->data());
if (msg_size > len - 4) {
ESP_LOGW(TAG, "Message size incorrect");
continue;
}
buffer->erase(buffer->begin(), buffer->begin() + 4); // todo: copying
buffer->resize(msg_size);
that->m_rx_queue->enqueue(std::move(buffer));
}
}
ESP_LOGW(TAG, "Network disconnected");
close(that->m_tx_server_sock);
that->m_tx_server_sock = -1;
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
}
}
bool UDPServer::is_network_connected() {
esp_netif_ip_info_t ip_info;
esp_netif_t *netif = esp_netif_get_handle_from_ifkey("WIFI_STA_DEF");
if (netif != nullptr && esp_netif_get_ip_info(netif, &ip_info) == ESP_OK) {
return true;
}
if (0 != ip_info.ip.addr) {
return true;
}
netif = esp_netif_get_handle_from_ifkey("WIFI_AP_DEF");
if (netif != nullptr && esp_netif_get_ip_info(netif, &ip_info) == ESP_OK) {
return true;
}
if (0 != ip_info.ip.addr) {
return true;
}
return false;
}
bool UDPServer::authenticate_client(int sock) {
// todo: authentication (key?)
return 0;
}
int UDPServer::send_msg(char *buffer, const uint32_t length) const {
if (!is_network_connected() || m_tx_server_sock == -1) {
return -1;
}
sockaddr_in mcast_dest = {
.sin_family = AF_INET,
.sin_port = htons(m_tx_port),
.sin_addr = {
.s_addr = inet_addr(SEND_MCAST)
},
};
uint32_t size = length;
iovec iov[2];
iov[0].iov_base = &size;
iov[0].iov_len = 4;
iov[1].iov_base = buffer;
iov[1].iov_len = length;
msghdr msg = {};
msg.msg_iov = iov;
msg.msg_iovlen = 2;
msg.msg_name = &mcast_dest;
msg.msg_namelen = sizeof(mcast_dest);
sendmsg(this->m_tx_server_sock, &msg, 0);
return 0;
}

18
components/rpc/wireless/WifiManager.cpp
View File

@@ -108,6 +108,8 @@ int WifiManager::init_connection() {
esp_wifi_start();
esp_wifi_connect();
esp_netif_set_default_netif(esp_netif_get_handle_from_ifkey("WIFI_STA_DEF"));
return 0;
}
@@ -199,6 +201,8 @@ int WifiManager::init_softap() {
esp_wifi_start();
esp_netif_set_default_netif(esp_netif_get_handle_from_ifkey("WIFI_STA_DEF"));
return 0;
}
@@ -214,12 +218,12 @@ void WifiManager::wifi_event_handler(void *event_handler_arg, esp_event_base_t e
const auto that = static_cast<WifiManager *>(event_handler_arg);
if (WIFI_EVENT_STA_START == event_id) {
ESP_LOGI(TAG, "Station mode started\n");
ESP_LOGI(TAG, "Station mode started");
} else if (WIFI_EVENT_STA_CONNECTED == event_id) {
ESP_LOGI(TAG, "Connected to wifi in station mode\n");
ESP_LOGI(TAG, "Connected to wifi in station mode");
that->update_state(wifi_state::connected);
} else if (WIFI_EVENT_STA_DISCONNECTED == event_id) {
ESP_LOGI(TAG, "Station mode shutdown\n");
ESP_LOGI(TAG, "Station mode shutdown");
xSemaphoreTake(that->m_mutex, portMAX_DELAY);
if (that->m_state == wifi_state::connected) {
xSemaphoreGive(that->m_mutex);
@@ -228,12 +232,12 @@ void WifiManager::wifi_event_handler(void *event_handler_arg, esp_event_base_t e
xSemaphoreGive(that->m_mutex);
}
} else if (IP_EVENT_STA_GOT_IP == event_id) {
ESP_LOGI(TAG, "Got IP as station\n");
ESP_LOGI(TAG, "Got IP as station");
} else if (WIFI_EVENT_AP_STACONNECTED == event_id) {
ESP_LOGI(TAG, "User connected to AP\n");
ESP_LOGI(TAG, "User connected to AP");
} else if (WIFI_EVENT_AP_STADISCONNECTED == event_id) {
ESP_LOGI(TAG, "User disconnected from AP\n");
ESP_LOGI(TAG, "User disconnected from AP");
} else if (WIFI_EVENT_AP_START == event_id) {
ESP_LOGI(TAG, "AP started\n");
ESP_LOGI(TAG, "AP started");
}
}

14
main/LoopManager.cpp
View File

@@ -14,6 +14,7 @@
#define ACTUATOR_CMD_TAG 5
#define TOPOLOGY_CMD_TAG 6
#define METADATA_RX_TAG 7
#define METADATA_PERIOD_MS 1000
@@ -51,7 +52,18 @@
casted_orientations,
that->m_messaging_interface->get_connection_type(),
that->m_messaging_interface->get_leader());
that->m_messaging_interface->send(static_cast<char *>(data), size, PC_ADDR, TOPOLOGY_CMD_TAG, true);
that->m_messaging_interface->send(static_cast<char *>(data), size, PC_ADDR, TOPOLOGY_CMD_TAG, false);
vTaskDelay(METADATA_PERIOD_MS / portTICK_PERIOD_MS);
}
}
[[noreturn]] void LoopManager::metadata_rx_loop(char *args) {
const auto that = reinterpret_cast<LoopManager *>(args);
const auto buffer = std::make_unique<std::vector<char>>();
buffer->resize(512);
while (true) {
that->m_messaging_interface->recv(buffer->data(), 512, PC_ADDR, METADATA_RX_TAG);
}
}

23
main/control/ActuatorFactory.cpp
View File

@@ -9,15 +9,16 @@
#include "control/Servo1Actuator.h"
#include "flatbuffers_generated/RobotModule_generated.h"
std::unique_ptr<IActuator> ActuatorFactory::create_actuator(const ModuleType type) {
switch (type) {
case ModuleType_SERVO_1:
return std::make_unique<Servo1Actuator>();
case ModuleType_SERVO_2:
return std::make_unique<Servo1Actuator>();
case ModuleType_DC_MOTOR:
return std::make_unique<DCMotorActuator>();
default:
return nullptr;
}
std::unique_ptr<IActuator>
ActuatorFactory::create_actuator(const ModuleType type) {
switch (type) {
case ModuleType_SERVO_1:
return std::make_unique<Servo1Actuator>();
case ModuleType_SERVO_2:
return std::make_unique<Servo1Actuator>();
case ModuleType_DC_MOTOR:
return std::make_unique<DCMotorActuator>();
default:
return nullptr;
}
}

54
main/control/Servo1Actuator.cpp
View File

@@ -3,42 +3,46 @@
//
#include "control/Servo1Actuator.h"
#include "util/number_utils.h"
#include "driver/ledc.h"
#include "constants/module.h"
#include "AngleControlMessageBuilder.h"
#include "constants/module.h"
#include "driver/ledc.h"
#include "util/number_utils.h"
#define LOW_DUTY 200
#define HIGH_DUTY 1000
#define PWM_FREQ 50 // 4khz
Servo1Actuator::Servo1Actuator() {
ledc_timer_config_t ledc_timer = {
.speed_mode = LEDC_LOW_SPEED_MODE,
.duty_resolution = LEDC_TIMER_13_BIT,
.timer_num = LEDC_TIMER_0,
.freq_hz = PWM_FREQ,
.clk_cfg = LEDC_AUTO_CLK,
};
ESP_ERROR_CHECK(ledc_timer_config(&ledc_timer));
ledc_timer_config_t ledc_timer = {
.speed_mode = LEDC_LOW_SPEED_MODE,
.duty_resolution = LEDC_TIMER_13_BIT,
.timer_num = LEDC_TIMER_0,
.freq_hz = PWM_FREQ,
.clk_cfg = LEDC_AUTO_CLK,
};
ESP_ERROR_CHECK(ledc_timer_config(&ledc_timer));
ledc_channel_config_t ledc_channel = {
.gpio_num = SERVO_GPIO,
.speed_mode = LEDC_LOW_SPEED_MODE,
.channel = LEDC_CHANNEL_0,
.intr_type = LEDC_INTR_DISABLE,
.timer_sel = LEDC_TIMER_0,
.duty = (HIGH_DUTY + LOW_DUTY) / 2, // move motor to midpoint initially
.hpoint = 0,
};
ledc_channel_config_t ledc_channel = {
.gpio_num = SERVO_GPIO,
.speed_mode = LEDC_LOW_SPEED_MODE,
.channel = LEDC_CHANNEL_0,
.intr_type = LEDC_INTR_DISABLE,
.timer_sel = LEDC_TIMER_0,
.duty = (HIGH_DUTY + LOW_DUTY) / 2, // move motor to midpoint initially
.hpoint = 0,
};
ESP_ERROR_CHECK(ledc_channel_config(&ledc_channel));
ESP_ERROR_CHECK(ledc_channel_config(&ledc_channel));
}
void Servo1Actuator::actuate(uint8_t *cmd) {
const auto* angleControlCmd = Flatbuffers::AngleControlMessageBuilder::parse_angle_control_message(cmd);
const auto newDuty = util::mapRange<int32_t>(angleControlCmd->angle(), 0, 180, LOW_DUTY, HIGH_DUTY);
const auto *angleControlCmd =
Flatbuffers::AngleControlMessageBuilder::parse_angle_control_message(cmd);
const auto newDuty = util::mapRange<int32_t>(angleControlCmd->angle(), 0, 180,
LOW_DUTY, HIGH_DUTY);
ESP_ERROR_CHECK(ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0, newDuty));
ESP_ERROR_CHECK(ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0));
std::cout << "actuating to " << angleControlCmd->angle() << std::endl;
ESP_ERROR_CHECK(ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0, newDuty));
ESP_ERROR_CHECK(ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0));
}