From: Nils Forssén Date: Wed, 12 Jun 2024 23:17:01 +0000 (+0200) Subject: mavlink example X-Git-Url: https://gitweb.forssennils.se/?a=commitdiff_plain;h=6b186decd5a5829db2c0dd55d3bb76a47d05345a;p=flygplan.git mavlink example --- diff --git a/README.md b/README.md index 18ff068..218312d 100644 --- a/README.md +++ b/README.md @@ -3,7 +3,10 @@ Enable fan pin 20 through raspi-config ``` -sudo apt install -y cmake build-essential checkinstall zlib1g-dev libssl-dev python3 python3-pip ninja-build git +sudo apt install -y cmake build-essential checkinstall zlib1g-dev libssl-dev python3 python3-pip ninja-build git ufw +``` +based a bit on example [this is my link](https://github.com/mavlink/mavlink/blob/master/examples/c/udp_example.c) -``` \ No newline at end of file +ufw for firewall +sudo ufw allow 1234, 14550, 14551 \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 2a94894..391aa97 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,9 +1,171 @@ -#include -#include "test.h" +// Simple example receiving and sending MAVLink v2 over UDP +// based on POSIX APIs (e.g. Linux, BSD, macOS). + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + #include -int main(int, char**){ - printf("Hello, from clientcamera!\n"); + +void receive_some(int socket_fd, struct sockaddr_in* src_addr, socklen_t* src_addr_len, bool* src_addr_set); +void handle_heartbeat(const mavlink_message_t* message); + +void send_some(int socket_fd, const struct sockaddr_in* src_addr, socklen_t src_addr_len); +void send_heartbeat(int socket_fd, const struct sockaddr_in* src_addr, socklen_t src_addr_len); + + +int main(int argc, char* argv[]) +{ + // Open UDP socket + const int socket_fd = socket(PF_INET, SOCK_DGRAM, 0); + + if (socket_fd < 0) { + printf("socket error: %s\n", strerror(errno)); + return -1; + } + + // Bind to port + struct sockaddr_in addr = {}; + memset(&addr, 0, sizeof(addr)); + addr.sin_family = AF_INET; + inet_pton(AF_INET, "0.0.0.0", &(addr.sin_addr)); // listen on all network interfaces + addr.sin_port = htons(14550); // default port on the ground + + if (bind(socket_fd, (struct sockaddr*)(&addr), sizeof(addr)) != 0) { + printf("bind error: %s\n", strerror(errno)); + return -2; + } + + // We set a timeout at 100ms to prevent being stuck in recvfrom for too + // long and missing our chance to send some stuff. + struct timeval tv; + tv.tv_sec = 0; + tv.tv_usec = 100000; + if (setsockopt(socket_fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)) < 0) { + printf("setsockopt error: %s\n", strerror(errno)); + return -3; + } + + struct sockaddr_in src_addr = {}; + socklen_t src_addr_len = sizeof(src_addr); + bool src_addr_set = false; + + while (true) { + // For illustration purposes we don't bother with threads or async here + // and just interleave receiving and sending. + // This only works if receive_some returns every now and then. + receive_some(socket_fd, &src_addr, &src_addr_len, &src_addr_set); + + if (src_addr_set) { + send_some(socket_fd, &src_addr, src_addr_len); + } + } return 0; } + +void receive_some(int socket_fd, struct sockaddr_in* src_addr, socklen_t* src_addr_len, bool* src_addr_set) +{ + // We just receive one UDP datagram and then return again. + char buffer[2048]; // enough for MTU 1500 bytes + + const int ret = recvfrom( + socket_fd, buffer, sizeof(buffer), 0, (struct sockaddr*)(src_addr), src_addr_len); + + if (ret < 0) { + printf("recvfrom error: %s\n", strerror(errno)); + } else if (ret == 0) { + // peer has done an orderly shutdown + return; + } + + *src_addr_set = true; + + mavlink_message_t message; + mavlink_status_t status; + for (int i = 0; i < ret; ++i) { + if (mavlink_parse_char(MAVLINK_COMM_0, buffer[i], &message, &status) == 1) { + + // printf( + // "Received message %d from %d/%d\n", + // message.msgid, message.sysid, message.compid); + + switch (message.msgid) { + case MAVLINK_MSG_ID_HEARTBEAT: + handle_heartbeat(&message); + break; + } + } + } +} + +void handle_heartbeat(const mavlink_message_t* message) +{ + mavlink_heartbeat_t heartbeat; + mavlink_msg_heartbeat_decode(message, &heartbeat); + + printf("Got heartbeat from "); + switch (heartbeat.autopilot) { + case MAV_AUTOPILOT_GENERIC: + printf("generic"); + break; + case MAV_AUTOPILOT_ARDUPILOTMEGA: + printf("ArduPilot"); + break; + case MAV_AUTOPILOT_PX4: + printf("PX4"); + break; + default: + printf("other"); + break; + } + printf(" autopilot\n"); +} + +void send_some(int socket_fd, const struct sockaddr_in* src_addr, socklen_t src_addr_len) +{ + // Whenever a second has passed, we send a heartbeat. + static time_t last_time = 0; + time_t current_time = time(NULL); + if (current_time - last_time >= 1) { + send_heartbeat(socket_fd, src_addr, src_addr_len); + last_time = current_time; + } +} + +void send_heartbeat(int socket_fd, const struct sockaddr_in* src_addr, socklen_t src_addr_len) +{ + 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, + MAVLINK_COMM_0, + &message, + MAV_TYPE_GENERIC, + MAV_AUTOPILOT_GENERIC, + base_mode, + custom_mode, + MAV_STATE_STANDBY); + + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + const int len = mavlink_msg_to_send_buffer(buffer, &message); + + int ret = sendto(socket_fd, buffer, len, 0, (const struct sockaddr*)src_addr, src_addr_len); + if (ret != len) { + printf("sendto error: %s\n", strerror(errno)); + } else { + printf("Sent heartbeat\n"); + } +} \ No newline at end of file