60 lines
1.8 KiB
C++
60 lines
1.8 KiB
C++
#include <iostream>
|
|
#include <vector>
|
|
#include <atomic>
|
|
#include <port/udp.h>
|
|
|
|
#include "port/uart.h"
|
|
|
|
static constexpr uint16_t UDP_PORT = 1067;
|
|
|
|
int main(int argc, char* argv[]) {
|
|
std::string serial_port = "/dev/ttyUSB0";
|
|
if (argc > 1) {
|
|
serial_port = argv[1];
|
|
}
|
|
|
|
try {
|
|
// Создание UDP сервера
|
|
drivers::UdpDriver udp_server(UDP_PORT);
|
|
drivers::UartDriver uart(serial_port, 416666);
|
|
|
|
std::cout << "Ready to receive UDP packets and forward to serial port" << std::endl;
|
|
std::cout << "Press Ctrl+C to exit" << std::endl;
|
|
|
|
int packet_count = 0;
|
|
while (true) {
|
|
// Прием UDP пакета
|
|
std::vector<uint16_t> data = udp_server.recvPacket();
|
|
|
|
if (!data.empty()) {
|
|
packet_count++;
|
|
|
|
for (int i = 0; i < data.size() && i < SbusData::NUM_CH; ++i) {
|
|
auto item = static_cast<int16_t>(data[i]);
|
|
sb.ch[i] = static_cast<int16_t>((item - 1000.0) * 2);
|
|
if (sb.ch[i] < 50) {
|
|
sb.ch[i] = 50; // минимальное число
|
|
} else if (sb.ch[i] > 1900) {
|
|
sb.ch[i] = 1900; // максимальное число
|
|
}
|
|
}
|
|
sb.fillDataBuf();
|
|
serial.write(sb.buf_);
|
|
|
|
// Выводим статистику каждые 100 пакетов
|
|
if (packet_count % 100 == 0) {
|
|
std::cout << "Received " << packet_count << " packets total" << std::endl;
|
|
}
|
|
}
|
|
}
|
|
|
|
} catch (const std::exception& e) {
|
|
std::cerr << "Error: " << e.what() << std::endl;
|
|
return 1;
|
|
}
|
|
|
|
std::cout << "Exiting..." << std::endl;
|
|
return 0;
|
|
}
|
|
|