365 lines
13 KiB
C++
365 lines
13 KiB
C++
#include <sys/ioctl.h>
|
||
#include <sys/poll.h>
|
||
#include <asm/termbits.h>
|
||
#include <sys/socket.h>
|
||
#include <netinet/in.h>
|
||
#include <arpa/inet.h>
|
||
#include <unistd.h>
|
||
#include <fcntl.h>
|
||
|
||
#include <iostream>
|
||
#include <vector>
|
||
#include <cstring>
|
||
#include <atomic>
|
||
#include <span>
|
||
#include <time.h>
|
||
|
||
/**
|
||
* Вспомогательная функция для получения текущего времени в миллисекундах
|
||
*/
|
||
inline int64_t milliseconds() {
|
||
timespec ts{};
|
||
clock_gettime(CLOCK_MONOTONIC, &ts);
|
||
return static_cast<int64_t>(ts.tv_sec) * 1000 + static_cast<int64_t>(ts.tv_nsec) / 1000000;
|
||
}
|
||
|
||
constexpr int64_t SBUS_SEND_FRAME_INTERVAL = 50;
|
||
constexpr int64_t SBUS_RXLOSS_INTERVAL = 500;
|
||
|
||
class UDPServer {
|
||
public:
|
||
int sockfd;
|
||
sockaddr_in server_addr, client_addr;
|
||
socklen_t client_len;
|
||
|
||
UDPServer(uint16_t port) : client_len(sizeof(client_addr)) {
|
||
// Создание UDP сокета
|
||
sockfd = socket(AF_INET, SOCK_DGRAM, 0);
|
||
if (sockfd < 0) {
|
||
throw std::runtime_error("Failed to create socket");
|
||
}
|
||
|
||
// Настройка адреса сервера
|
||
memset(&server_addr, 0, sizeof(server_addr));
|
||
server_addr.sin_family = AF_INET;
|
||
server_addr.sin_addr.s_addr = INADDR_ANY;
|
||
server_addr.sin_port = htons(port);
|
||
|
||
// Привязка сокета
|
||
if (bind(sockfd, (sockaddr*)&server_addr, sizeof(server_addr)) < 0) {
|
||
close(sockfd);
|
||
throw std::runtime_error("Failed to bind socket");
|
||
}
|
||
|
||
std::cout << "UDP server listening on port " << port << std::endl;
|
||
}
|
||
|
||
~UDPServer() {
|
||
if (sockfd >= 0) {
|
||
close(sockfd);
|
||
}
|
||
}
|
||
|
||
std::vector<uint16_t> receive() {
|
||
std::vector<uint16_t> data(64);
|
||
|
||
ssize_t received = recvfrom(sockfd,
|
||
data.data(),
|
||
data.size() * sizeof(uint16_t),
|
||
MSG_DONTWAIT, // Неблокирующий режим
|
||
(sockaddr*)&client_addr,
|
||
&client_len);
|
||
|
||
if (received > 0) {
|
||
// Выводим информацию о отправителе и данные
|
||
data.resize(received / 2);
|
||
char client_ip[INET_ADDRSTRLEN];
|
||
inet_ntop(AF_INET, &client_addr.sin_addr, client_ip, INET_ADDRSTRLEN);
|
||
// std::cout << "Received data from " << client_ip << ":" << ntohs(client_addr.sin_port) << std::endl;
|
||
|
||
// std::cout << "Data: ";
|
||
// for (int i = 0; i < 4 && i < data.size(); ++i) {
|
||
// std::cout << data[i] << " ";
|
||
// }
|
||
// std::cout << "..." << std::endl;
|
||
|
||
return data;
|
||
}
|
||
|
||
return {};
|
||
}
|
||
};
|
||
|
||
struct SbusData {
|
||
bool lost_frame = false;
|
||
bool failsafe = false;
|
||
bool ch17 = false, ch18 = false;
|
||
static constexpr size_t NUM_CH = 18;
|
||
static constexpr int16_t SBUS_CH_MIN = 173 + 20;
|
||
static constexpr int16_t SBUS_CH_MAX = 1812 - 20;
|
||
int16_t ch[NUM_CH];
|
||
|
||
/* Message len */
|
||
static constexpr int8_t BUF_LEN_ = 25;
|
||
/* SBUS message defs */
|
||
static constexpr int8_t NUM_SBUS_CH_ = 16;
|
||
static constexpr uint8_t HEADER_ = 0x0F;
|
||
static constexpr uint8_t FOOTER_ = 0x00;
|
||
static constexpr uint8_t FOOTER2_ = 0x04;
|
||
static constexpr uint8_t CH17_MASK_ = 0x01;
|
||
static constexpr uint8_t CH18_MASK_ = 0x02;
|
||
static constexpr uint8_t LOST_FRAME_MASK_ = 0x04;
|
||
static constexpr uint8_t FAILSAFE_MASK_ = 0x08;
|
||
uint8_t binaryBuffer[25];
|
||
|
||
typedef struct sbusChannels_s {
|
||
// 176 bits of data (11 bits per channel * 16 channels) = 22 bytes.
|
||
unsigned int chan0 : 11;
|
||
unsigned int chan1 : 11;
|
||
unsigned int chan2 : 11;
|
||
unsigned int chan3 : 11;
|
||
unsigned int chan4 : 11;
|
||
unsigned int chan5 : 11;
|
||
unsigned int chan6 : 11;
|
||
unsigned int chan7 : 11;
|
||
unsigned int chan8 : 11;
|
||
unsigned int chan9 : 11;
|
||
unsigned int chan10 : 11;
|
||
unsigned int chan11 : 11;
|
||
unsigned int chan12 : 11;
|
||
unsigned int chan13 : 11;
|
||
unsigned int chan14 : 11;
|
||
unsigned int chan15 : 11;
|
||
} __attribute__((__packed__)) sbusChannels_t;
|
||
static_assert(sizeof(sbusChannels_s) == 22);
|
||
struct sbusFrame_s {
|
||
uint8_t syncByte = HEADER_;
|
||
sbusChannels_t channels{};
|
||
uint8_t flags{};
|
||
/**
|
||
* The endByte is 0x00 on FrSky and some futaba RX's, on Some SBUS2 RX's the value indicates the telemetry byte that is sent after every 4th sbus frame.
|
||
*
|
||
* See https://github.com/cleanflight/cleanflight/issues/590#issuecomment-101027349
|
||
* and
|
||
* https://github.com/cleanflight/cleanflight/issues/590#issuecomment-101706023
|
||
*/
|
||
uint8_t endByte = FOOTER_;
|
||
} __attribute__ ((__packed__));
|
||
static_assert(sizeof(sbusFrame_s) == sizeof(binaryBuffer));
|
||
|
||
void fillDataBuf() {
|
||
auto* dest = reinterpret_cast<sbusFrame_s*>(binaryBuffer);
|
||
dest->syncByte = HEADER_;
|
||
dest->channels.chan0 = std::min(std::max(ch[0], SBUS_CH_MIN), SBUS_CH_MAX);
|
||
dest->channels.chan1 = std::min(std::max(ch[1], SBUS_CH_MIN), SBUS_CH_MAX);
|
||
dest->channels.chan2 = std::min(std::max(ch[2], SBUS_CH_MIN), SBUS_CH_MAX);
|
||
dest->channels.chan3 = std::min(std::max(ch[3], SBUS_CH_MIN), SBUS_CH_MAX);
|
||
dest->channels.chan4 = std::min(std::max(ch[4], SBUS_CH_MIN), SBUS_CH_MAX);
|
||
dest->channels.chan5 = std::min(std::max(ch[5], SBUS_CH_MIN), SBUS_CH_MAX);
|
||
dest->channels.chan6 = std::min(std::max(ch[6], SBUS_CH_MIN), SBUS_CH_MAX);
|
||
dest->channels.chan7 = std::min(std::max(ch[7], SBUS_CH_MIN), SBUS_CH_MAX);
|
||
dest->channels.chan8 = std::min(std::max(ch[8], SBUS_CH_MIN), SBUS_CH_MAX);
|
||
dest->channels.chan9 = std::min(std::max(ch[9], SBUS_CH_MIN), SBUS_CH_MAX);
|
||
dest->channels.chan10 = std::min(std::max(ch[10], SBUS_CH_MIN), SBUS_CH_MAX);
|
||
dest->channels.chan11 = std::min(std::max(ch[11], SBUS_CH_MIN), SBUS_CH_MAX);
|
||
dest->channels.chan12 = std::min(std::max(ch[12], SBUS_CH_MIN), SBUS_CH_MAX);
|
||
dest->channels.chan13 = std::min(std::max(ch[13], SBUS_CH_MIN), SBUS_CH_MAX);
|
||
dest->channels.chan14 = std::min(std::max(ch[14], SBUS_CH_MIN), SBUS_CH_MAX);
|
||
dest->channels.chan15 = std::min(std::max(ch[15], SBUS_CH_MIN), SBUS_CH_MAX);
|
||
|
||
dest->flags = 0;
|
||
if (ch[16] >= (SBUS_CH_MAX + SBUS_CH_MAX) / 2) { dest->flags |= CH17_MASK_; }
|
||
if (ch[17] >= (SBUS_CH_MAX + SBUS_CH_MAX) / 2) { dest->flags |= CH18_MASK_; }
|
||
|
||
dest->endByte = FOOTER_;
|
||
}
|
||
|
||
};
|
||
|
||
class SerialPort {
|
||
int fd;
|
||
|
||
public:
|
||
SerialPort() : fd(-1) {}
|
||
|
||
bool open(const std::string& port_path) {
|
||
// Открытие последовательного порта
|
||
fd = ::open(port_path.c_str(), O_RDWR | O_NOCTTY | O_SYNC);
|
||
if (fd < 0) {
|
||
std::cout << "Failed to open serial port " << port_path << ": " << std::strerror(errno) << std::endl;
|
||
return false;
|
||
}
|
||
struct termios2 tio{};
|
||
ioctl(fd, TCGETS2, &tio);
|
||
// 8bit
|
||
tio.c_cflag &= ~CSIZE;
|
||
tio.c_cflag |= CS8;
|
||
// even
|
||
tio.c_cflag &= ~(PARODD | CMSPAR);
|
||
tio.c_cflag |= PARENB;
|
||
// 2 stop bits
|
||
tio.c_cflag |= CSTOPB;
|
||
// baud rate
|
||
tio.c_ispeed = 100000;
|
||
tio.c_ospeed = 100000;
|
||
// other
|
||
tio.c_iflag |= (INPCK|IGNBRK|IGNCR|ISTRIP);
|
||
tio.c_cflag &= ~CBAUD;
|
||
tio.c_cflag |= (BOTHER|CREAD|CLOCAL);
|
||
|
||
if (ioctl(fd, TCSETS2, &tio) != 0) {
|
||
std::cout << "Failed to set termios2 attributes: " << std::strerror(errno) << std::endl;
|
||
close(fd);
|
||
return false;
|
||
}
|
||
|
||
std::cout << "Serial port " << port_path << " opened and configured: 100000 baud, 8E2" << std::endl;
|
||
return true;
|
||
}
|
||
|
||
~SerialPort() {
|
||
_close();
|
||
}
|
||
|
||
void _close() {
|
||
if (fd >= 0) {
|
||
::close(fd);
|
||
fd = -1;
|
||
}
|
||
}
|
||
|
||
// Метод для получения файлового дескриптора порта
|
||
int getDescriptor() const {
|
||
return fd;
|
||
}
|
||
|
||
int drain() {
|
||
int ret;
|
||
do {
|
||
ret = ioctl(fd, TCSBRK, 1);
|
||
} while (ret < 0 && errno == EINTR);
|
||
|
||
return ret;
|
||
}
|
||
|
||
// Метод для записи данных в порт
|
||
bool writeBuffer(std::span<const uint8_t> data) {
|
||
if (fd < 0) return false;
|
||
|
||
ssize_t written = write(fd, data.data(), data.size());
|
||
if (written < 0) {
|
||
std::cout << "Failed to write to serial port " << strerror(errno) << std::endl;
|
||
return false;
|
||
}
|
||
|
||
// Принудительная отправка данных
|
||
if (drain() < 0) {
|
||
std::cout << "Failed to flush data to serial port" << strerror(errno) << std::endl;
|
||
return false;
|
||
}
|
||
return true;
|
||
}
|
||
};
|
||
|
||
int main(int argc, char* argv[]) {
|
||
// Парсим аргументы командной строки
|
||
std::string serial_port = "/dev/ttyPS0";
|
||
if (argc > 1) {
|
||
serial_port = argv[1];
|
||
}
|
||
|
||
try {
|
||
// Создание UDP сервера
|
||
UDPServer udp_server(1066);
|
||
|
||
// Открытие последовательного порта
|
||
SerialPort serial;
|
||
if (!serial.open(serial_port)) {
|
||
return 1;
|
||
}
|
||
|
||
std::cout << "Ready to receive UDP packets and forward to serial port" << std::endl;
|
||
std::cout << "Press Ctrl+C to exit" << std::endl;
|
||
|
||
SbusData sb{};
|
||
int64_t lastSbusWrite = 0;
|
||
int64_t lastSbusRecv = 0;
|
||
|
||
int packetCount = 0;
|
||
int totalPacketCount = 0;
|
||
int serialErrors = 0;
|
||
int64_t lastStatisticsShow = milliseconds();
|
||
int64_t _lastLoopNow = lastStatisticsShow;
|
||
while (true) {
|
||
pollfd udpFd{.fd = udp_server.sockfd, .events = POLLIN, .revents = 0};
|
||
poll(&udpFd, 1, SBUS_SEND_FRAME_INTERVAL / 4);
|
||
auto now = milliseconds();
|
||
if (std::abs(now - _lastLoopNow) > SBUS_SEND_FRAME_INTERVAL) {
|
||
std::cout << "Warning: Loop freeze, reset timers. now time:" << now << std::endl;
|
||
lastSbusWrite = 0;
|
||
lastSbusRecv = 0;
|
||
lastStatisticsShow = 0;
|
||
}
|
||
_lastLoopNow = now;
|
||
|
||
if (udpFd.revents & POLLIN) {
|
||
// Прием UDP пакета
|
||
std::vector<uint16_t> data = udp_server.receive();
|
||
lastSbusRecv = now;
|
||
|
||
if (!data.empty()) {
|
||
packetCount++;
|
||
|
||
for (int i = 0; i < SbusData::NUM_CH; ++i) {
|
||
auto item = i < data.size() ? static_cast<double>(data[i]) : 1500.0;
|
||
item -= 1000.0;
|
||
item = std::min(item, 1000.0);
|
||
item = std::max(item, 0.0);
|
||
item *= (SbusData::SBUS_CH_MAX - SbusData::SBUS_CH_MIN) / 1000.0;
|
||
item += SbusData::SBUS_CH_MIN;
|
||
|
||
sb.ch[i] = static_cast<int16_t>(item);
|
||
if (sb.ch[i] < SbusData::SBUS_CH_MIN) {
|
||
sb.ch[i] = SbusData::SBUS_CH_MIN; // минимальное число
|
||
} else if (sb.ch[i] > SbusData::SBUS_CH_MAX) {
|
||
sb.ch[i] = SbusData::SBUS_CH_MAX; // максимальное число
|
||
}
|
||
}
|
||
sb.fillDataBuf();
|
||
}
|
||
}
|
||
|
||
// std::cout << "from loop: current time " << now << std::endl;
|
||
if (now - lastSbusWrite >= SBUS_SEND_FRAME_INTERVAL && now - lastSbusRecv <= SBUS_RXLOSS_INTERVAL) {
|
||
lastSbusWrite = now;
|
||
if (!serial.writeBuffer(sb.binaryBuffer)) {
|
||
serialErrors++;
|
||
} else {
|
||
serialErrors = 0;
|
||
}
|
||
|
||
// после 50 ошибок дальше не будем пытаться что-то писать, ибо это бесполезно
|
||
if (serialErrors >= 50) {
|
||
std::cout << "FATAL: 50 errors on serial port write operation, exit" << std::endl;
|
||
break;
|
||
}
|
||
}
|
||
|
||
if (now - lastStatisticsShow >= 1000) {
|
||
lastStatisticsShow = now;
|
||
totalPacketCount += packetCount;
|
||
std::cout << "Received " << totalPacketCount << " packets total, " << packetCount << " in last second" << std::endl;
|
||
packetCount = 0;
|
||
}
|
||
}
|
||
|
||
} catch (const std::exception& e) {
|
||
std::cout << "Error: " << e.what() << std::endl;
|
||
return 1;
|
||
}
|
||
|
||
std::cout << "Exiting..." << std::endl;
|
||
return 0;
|
||
}
|
||
|