56 lines
1.4 KiB
C++
56 lines
1.4 KiB
C++
#include "port/udp.h"
|
|
|
|
#include <cstring>
|
|
#include <poll.h>
|
|
#include <arpa/inet.h>
|
|
#include <netinet/in.h>
|
|
#include <sys/socket.h>
|
|
|
|
#include "crsf.h"
|
|
|
|
|
|
drivers::UdpDriver::UdpDriver(uint16_t port) {
|
|
fd = socket(AF_INET, SOCK_DGRAM, 0);
|
|
if (fd < 0) {
|
|
throw std::runtime_error(std::string("UdpDriver open error: ") + std::strerror(errno));
|
|
}
|
|
|
|
sockaddr_in addr{};
|
|
addr.sin_family = AF_INET;
|
|
addr.sin_port = htons(port);
|
|
addr.sin_addr.s_addr = INADDR_ANY;
|
|
|
|
if (bind(fd, (sockaddr*)&addr, sizeof(addr)) < 0) if (fd < 0) {
|
|
close(fd);
|
|
throw std::runtime_error(std::string("UdpDriver bind error: ") + std::strerror(errno));
|
|
}
|
|
|
|
events = POLLIN;
|
|
}
|
|
bool drivers::UdpDriver::sendTo(std::span<const uint8_t> data, const std::string& addr, uint16_t port) {
|
|
if (fd < 0) return false;
|
|
|
|
sockaddr_in dst{};
|
|
dst.sin_family = AF_INET;
|
|
dst.sin_port = htons(port);
|
|
inet_aton(addr.c_str(), &dst.sin_addr);
|
|
|
|
auto w = sendto(fd, data.data(), data.size(), 0, (sockaddr*)&dst, sizeof(dst));
|
|
return w == data.size();
|
|
}
|
|
bool drivers::UdpDriver::recvPacket(std::vector<uint8_t>& out) {
|
|
out.resize(crsf::CRSF_MAX_FRAME_SIZE);
|
|
ssize_t received = recvfrom(fd, out.data(), out.size(), MSG_DONTWAIT, nullptr, nullptr);
|
|
|
|
if (received > 0) {
|
|
out.resize(received);
|
|
return true;
|
|
}
|
|
out.clear();
|
|
|
|
return false;
|
|
}
|
|
drivers::UdpDriver::~UdpDriver() {
|
|
close(fd);
|
|
}
|