From a0559f245d8999292979ee451b9e55ae294d61ac Mon Sep 17 00:00:00 2001 From: =?utf8?q?Nils=20Forss=C3=A9n?= Date: Mon, 2 Jun 2025 22:50:51 +0200 Subject: [PATCH] mavlink and UDP server working --- .vscode/c_cpp_properties.json | 17 -- .vscode/settings.json | 73 --------- processes/udp_server/CMakeLists.txt | 1 - .../udp_server/include/mavlink_handler.h | 15 -- processes/udp_server/include/udp_server.h | 18 +-- processes/udp_server/main.cpp | 19 +-- processes/udp_server/udp_server.cpp | 150 +++++++++++++----- 7 files changed, 124 insertions(+), 169 deletions(-) delete mode 100644 .vscode/c_cpp_properties.json delete mode 100644 .vscode/settings.json delete mode 100644 processes/udp_server/include/mavlink_handler.h diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json deleted file mode 100644 index 44f86bc..0000000 --- a/.vscode/c_cpp_properties.json +++ /dev/null @@ -1,17 +0,0 @@ -{ - "configurations": [ - { - "name": "Linux", - "includePath": [ - "${workspaceFolder}/**", - "${workspaceFolder}/processes/**", - ], - "defines": [], - "compilerPath": "/usr/bin/gcc", - "cStandard": "c17", - "cppStandard": "gnu++17", - "intelliSenseMode": "linux-gcc-x64" - } - ], - "version": 4 -} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json deleted file mode 100644 index 16961f3..0000000 --- a/.vscode/settings.json +++ /dev/null @@ -1,73 +0,0 @@ -{ - "files.associations": { - "array": "cpp", - "atomic": "cpp", - "bit": "cpp", - "*.tcc": "cpp", - "bitset": "cpp", - "cctype": "cpp", - "charconv": "cpp", - "chrono": "cpp", - "clocale": "cpp", - "cmath": "cpp", - "codecvt": "cpp", - "compare": "cpp", - "concepts": "cpp", - "condition_variable": "cpp", - "cstdarg": "cpp", - "cstddef": "cpp", - "cstdint": "cpp", - "cstdio": "cpp", - "cstdlib": "cpp", - "cstring": "cpp", - "ctime": "cpp", - "cwchar": "cpp", - "cwctype": "cpp", - "deque": "cpp", - "map": "cpp", - "string": "cpp", - "unordered_map": "cpp", - "vector": "cpp", - "exception": "cpp", - "algorithm": "cpp", - "functional": "cpp", - "iterator": "cpp", - "memory": "cpp", - "memory_resource": "cpp", - "numeric": "cpp", - "optional": "cpp", - "random": "cpp", - "ratio": "cpp", - "regex": "cpp", - "source_location": "cpp", - "string_view": "cpp", - "system_error": "cpp", - "tuple": "cpp", - "type_traits": "cpp", - "utility": "cpp", - "fstream": "cpp", - "future": "cpp", - "initializer_list": "cpp", - "iomanip": "cpp", - "iosfwd": "cpp", - "iostream": "cpp", - "istream": "cpp", - "limits": "cpp", - "mutex": "cpp", - "new": "cpp", - "numbers": "cpp", - "ostream": "cpp", - "semaphore": "cpp", - "span": "cpp", - "sstream": "cpp", - "stdexcept": "cpp", - "stop_token": "cpp", - "streambuf": "cpp", - "thread": "cpp", - "typeinfo": "cpp", - "variant": "cpp", - "stdint.h": "c", - "spi.h": "c" - }, - "C_Cpp.errorSquiggles": "enabled" -} \ No newline at end of file diff --git a/processes/udp_server/CMakeLists.txt b/processes/udp_server/CMakeLists.txt index 6060c8b..1e5ac00 100755 --- a/processes/udp_server/CMakeLists.txt +++ b/processes/udp_server/CMakeLists.txt @@ -1,7 +1,6 @@ add_executable(udp_server main.cpp udp_server.cpp - mavlink_handler.cpp ) target_include_directories(udp_server PRIVATE include) diff --git a/processes/udp_server/include/mavlink_handler.h b/processes/udp_server/include/mavlink_handler.h deleted file mode 100644 index 7f73f3c..0000000 --- a/processes/udp_server/include/mavlink_handler.h +++ /dev/null @@ -1,15 +0,0 @@ -#ifndef MAVLINK_HANDLER_H -#define MAVLINK_HANDLER_H - -// Standard -#include - -// Libs -#include -#include - -void init_mavlink_handler(std::shared_ptr logg); - -void handle_heartbeat(const mavlink_message_t* message); - -#endif \ No newline at end of file diff --git a/processes/udp_server/include/udp_server.h b/processes/udp_server/include/udp_server.h index fffb0fb..6902b61 100644 --- a/processes/udp_server/include/udp_server.h +++ b/processes/udp_server/include/udp_server.h @@ -3,39 +3,29 @@ // Standard #include -#include // Socket -#include #include #include -// Pipes -#include - // Libs #include -#include // Local -#include "util.h" #include "pipe_def.h" -#include "mavlink_handler.h" +#include "util.h" #define UDP_PORT 14552 #define UDP_RECV_BUF_SIZE 2048 // enough for MTU 1500 bytes -typedef int socket_fd_t; - typedef struct { - socket_fd_t socket_fd; + int socket_fd; struct sockaddr_in addr; socklen_t addr_len; bool addr_set; } gcs_t; -err_t udp_server_init(std::shared_ptr logg); - -err_t udp_server_run(pipe_fd_t recvpipe_fd, pipe_fd_t sendpipe_fd); +err_t udp_server_init(std::shared_ptr logg, const pipe_fd_t recvpipe_fd, const pipe_fd_t sendpipe_fd); +err_t udp_server_run(); #endif \ No newline at end of file diff --git a/processes/udp_server/main.cpp b/processes/udp_server/main.cpp index 8f39fb4..6c67a9d 100755 --- a/processes/udp_server/main.cpp +++ b/processes/udp_server/main.cpp @@ -18,32 +18,25 @@ static std::shared_ptr logger; -void receive_thread(gcs_t* src); -void handle_heartbeat(const mavlink_message_t* message); -void send_some(gcs_t* src); -void send_heartbeat(gcs_t* src); - // Main int main(int argc, char* argv[]) { logger = spdlog::stdout_color_mt("udp_server"); logger->set_level(UDP_LOGLEVEL); - udp_server_init(logger); - - logger->debug("Opening pipes, waiting for writing process to start"); + logger->debug("Opening pipes, waiting for writing process to start."); pipe_fd_t sendpipe_fd = open(SEND_PIPE, O_RDONLY); pipe_fd_t recvpipe_fd = open(RECV_PIPE, O_WRONLY); - - logger->debug("Pipes opened"); + + udp_server_init(logger, recvpipe_fd, sendpipe_fd); // Server mainloop, only exits on error - udp_server_run(recvpipe_fd, sendpipe_fd); + udp_server_run(); - logger->debug("Closing sendpipe"); + logger->debug("Closing sendpipe."); close(sendpipe_fd); - logger->debug("Closing recvpipe"); + logger->debug("Closing recvpipe."); close(recvpipe_fd); return -1; diff --git a/processes/udp_server/udp_server.cpp b/processes/udp_server/udp_server.cpp index 4e1ade5..2053b19 100644 --- a/processes/udp_server/udp_server.cpp +++ b/processes/udp_server/udp_server.cpp @@ -1,18 +1,86 @@ #include "udp_server.h" +// Standard +#include +#include + +// Socket +#include +#include +#include + +// Pipes +#include + +// Libs +#include +#include + +// Local +#include "system_def.h" +#include "pipe_def.h" +#include "util.h" + static std::shared_ptr logger; static bool recv_thread_running = true; static gcs_t gcs = {}; +static pipe_fd_t recvpipe; +static pipe_fd_t sendpipe; + // Declarations -static void recv_thread(pipe_fd_t sendpipe_fd); +static void handle_request_autopilot_capabilities(const mavlink_command_long_t& cmd_long); +static void handle_heartbeat(const mavlink_message_t& message); +static void recv_thread(); static void heartbeat_thread(); static void handle_mavlink(mavlink_message_t& message); // Definitions +static void handle_heartbeat(const mavlink_message_t& message) +{ + // Determine source of heartbeat + mavlink_heartbeat_t heartbeat; + mavlink_msg_heartbeat_decode(&message, &heartbeat); + + switch (heartbeat.type) { + case MAV_TYPE_GCS: + logger->debug("Heartbeat from GCS!"); + break; + default: + logger->warn("Heartbeat from unknown type: {}", heartbeat.type); + break; + } +} + +static void handle_request_autopilot_capabilities(const mavlink_command_long_t& cmd_long) +{ + // No need to handle the message, only send out a response + mavlink_message_t response_msg; + mavlink_autopilot_version_t version_struct = + { + + }; + + mavlink_msg_autopilot_version_encode( + MAV_SYS_ID, + MAV_COMP_ID_AUTOPILOT1, + &response_msg, + &version_struct + ); -static void recv_thread(pipe_fd_t recvpipe_fd) + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + auto len = mavlink_msg_to_send_buffer(buffer, &response_msg); + + auto ret = sendto(gcs.socket_fd, buffer, len, 0, (struct sockaddr*)(&(gcs.addr)), gcs.addr_len); + if (ret != len) { + logger->error("Autopilot version message error: {}.", strerror(errno)); + } else { + logger->info("Sent Autopilot version!"); + } +} + +static void recv_thread() { char buffer[UDP_RECV_BUF_SIZE]; // enough for MTU 1500 bytes @@ -26,7 +94,7 @@ static void recv_thread(pipe_fd_t recvpipe_fd) break; } else if (ret == 0) { // peer has done an orderly shutdown - logger->info("Peer has done an orderly shutdown"); + logger->info("Peer has done an orderly shutdown."); break; } @@ -40,32 +108,21 @@ static void recv_thread(pipe_fd_t recvpipe_fd) handle_mavlink(message); } } - logger->debug("Socket -> recvpipe: {} bytes", ret); - ret = write(recvpipe_fd, buffer, ret); - if (ret < 0) { - logger->error("Error while writing message. {}", strerror(errno)); - } } } static void heartbeat_thread() { - mavlink_message_t message; - - const uint8_t system_id = 42; - const uint8_t base_mode = 0; - const uint8_t custom_mode = 0; - mavlink_msg_heartbeat_pack_chan( - system_id, - MAV_COMP_ID_PERIPHERAL, + 1, // System ID + MAV_COMP_ID_AUTOPILOT1, MAVLINK_COMM_0, &message, MAV_TYPE_GENERIC, MAV_AUTOPILOT_GENERIC, - base_mode, - custom_mode, + 0, + 0, MAV_STATE_STANDBY); uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; @@ -75,7 +132,7 @@ static void heartbeat_thread() int ret = sendto(gcs.socket_fd, buffer, len, 0, (struct sockaddr*)(&(gcs.addr)), gcs.addr_len); if (ret != len) { - logger->error("Heartbeat sendto error: {}", strerror(errno)); + logger->error("Heartbeat sendto error: {}.", strerror(errno)); } else { logger->debug("Sent Heartbeat!"); } @@ -88,21 +145,41 @@ static void handle_mavlink(mavlink_message_t& message) { switch (message.msgid) { case MAVLINK_MSG_ID_HEARTBEAT: - handle_heartbeat(&message); + handle_heartbeat(message); + break; + case MAVLINK_MSG_ID_COMMAND_LONG: + mavlink_command_long_t cmd_long; + mavlink_msg_command_long_decode(&message, &cmd_long); + + switch (cmd_long.command) { + case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: + logger->debug("Got autopilot capabilities request."); + handle_request_autopilot_capabilities(cmd_long); + break; + default: + logger->warn("Unhandled command {} from {}/{}.", (int) cmd_long.command, message.sysid, message.compid); + break; + } break; default: - logger->debug("Received message {} from {}/{}", (int) message.msgid, message.sysid, message.compid); + logger->warn("Unhandled message {} from {}/{}.", (int) message.msgid, message.sysid, message.compid); + + logger->warn("Sending unhandled message to recvpipe: {} bytes.", message.len); + auto ret = write(recvpipe, message.payload64, message.len); + if (ret < 0) { + logger->error("Error while writing message to recvpipe. Errno {}.", strerror(errno)); + } break; } } -err_t udp_server_init(std::shared_ptr logg) +err_t udp_server_init(std::shared_ptr logg, const pipe_fd_t recvpipe_fd, const pipe_fd_t sendpipe_fd) { logger = logg; + sendpipe = sendpipe_fd; + recvpipe = recvpipe_fd; - init_mavlink_handler(logger); - - socket_fd_t socket_fd = socket(AF_INET, SOCK_DGRAM, 0); + auto socket_fd = socket(AF_INET, SOCK_DGRAM, 0); if (socket_fd < 0) { logger->error("socket error: {}", strerror(errno)); @@ -134,9 +211,9 @@ err_t udp_server_init(std::shared_ptr logg) return OK; } -err_t udp_server_run(pipe_fd_t recvpipe_fd, pipe_fd_t sendpipe_fd) +err_t udp_server_run() { - std::thread r_thread(recv_thread, recvpipe_fd); + std::thread r_thread(recv_thread); while (!(gcs.addr_set)) { logger->info("Waiting for GCS to connect..."); @@ -152,8 +229,10 @@ err_t udp_server_run(pipe_fd_t recvpipe_fd, pipe_fd_t sendpipe_fd) mavlink_message_t message; mavlink_status_t status; + message.msgid = 0; + while (true) { - ret = read(sendpipe_fd, &buffer, sizeof(buffer)); + ret = read(sendpipe, &buffer, sizeof(buffer)); if (ret == 0) { logger->error("No writing process on sendpipe. Exiting."); @@ -162,26 +241,25 @@ err_t udp_server_run(pipe_fd_t recvpipe_fd, pipe_fd_t sendpipe_fd) else if (ret < 0) logger->error("Error while reading message. {}", strerror(errno)); else { - /* + for (int i = 0; i < ret; ++i) { - if (mavlink_parse_char(MAVLINK_COMM_0, buffer[i], &message, &status) == 1) { - logger->debug("sendpipe -> socket: mavlink message id={},", (int) message.msgid); + if (mavlink_parse_char(MAVLINK_COMM_0, buffer[i], &message, &status)) { + logger->debug("sendpipe -> socket: mavlink message id={}, bytes: {}", (int) message.msgid, ret); } } - */ - logger->debug("sendpipe -> socket: {} bytes", ret); + ret = sendto(gcs.socket_fd, buffer, ret, 0, (struct sockaddr*)(&(gcs.addr)), gcs.addr_len); if (ret < 0) { - logger->error("Sending mavlink message to GCS sendto error {}", strerror(errno)); + logger->error("Sending mavlink message to GCS sendto error {}.", strerror(errno)); } } } - logger->debug("Closing recv_thread"); + logger->debug("Closing recv_thread."); recv_thread_running = false; - logger->debug("Closing socket"); + logger->debug("Closing socket."); close(gcs.socket_fd); return ERR; -- 2.30.2