#include "udp_server.h"
+// Standard
+#include <thread>
+#include <memory>
+
+// Socket
+#include <arpa/inet.h>
+#include <netinet/in.h>
+#include <sys/socket.h>
+
+// Pipes
+#include <unistd.h>
+
+// Libs
+#include <common/mavlink.h>
+#include <spdlog/spdlog.h>
+
+// Local
+#include "system_def.h"
+#include "pipe_def.h"
+#include "util.h"
+
static std::shared_ptr<spdlog::logger> 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
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;
}
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];
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!");
}
{
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<spdlog::logger> logg)
+err_t udp_server_init(std::shared_ptr<spdlog::logger> 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));
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...");
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.");
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;
projects / flygplan.git / commitdiff