добавил программу для воздуха, но она пока не работает
This commit is contained in:
@@ -9,17 +9,16 @@
|
||||
## Linux
|
||||
|
||||
```shell
|
||||
sudo apt install libhidapi-dev
|
||||
sudo apt install libsdl2-dev
|
||||
```
|
||||
|
||||
## MacOS
|
||||
|
||||
```shell
|
||||
brew install hidapi
|
||||
brew install sdl2
|
||||
```
|
||||
|
||||
## Шиндовс
|
||||
|
||||
Под шиндовс, как всегда, надо страдать. Рекомендуется установить `libusb` и `hidapi` через `vcpkg`.
|
||||
Под шиндовс, как всегда, надо страдать. Рекомендуется установить `sdl2` через `vcpkg`.
|
||||
|
||||
#
|
||||
|
||||
295
air/main.cpp
Normal file
295
air/main.cpp
Normal file
@@ -0,0 +1,295 @@
|
||||
#include <asm/termbits.h>
|
||||
#include <sys/ioctl.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>
|
||||
|
||||
|
||||
|
||||
|
||||
class UDPServer {
|
||||
private:
|
||||
int sockfd;
|
||||
sockaddr_in server_addr, client_addr;
|
||||
socklen_t client_len;
|
||||
|
||||
public:
|
||||
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(16);
|
||||
|
||||
ssize_t received = recvfrom(sockfd,
|
||||
data.data(),
|
||||
data.size() * sizeof(uint16_t),
|
||||
MSG_DONTWAIT, // Неблокирующий режим
|
||||
(sockaddr*)&client_addr,
|
||||
&client_len);
|
||||
|
||||
if (received > 0) {
|
||||
if (received == 16 * sizeof(uint16_t)) {
|
||||
// Выводим информацию о отправителе и данные
|
||||
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;
|
||||
} else {
|
||||
std::cerr << "Invalid packet size: " << received << " bytes" << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
return {};
|
||||
}
|
||||
};
|
||||
|
||||
struct SbusData {
|
||||
bool lost_frame = false;
|
||||
bool failsafe = false;
|
||||
bool ch17 = false, ch18 = false;
|
||||
static constexpr size_t NUM_CH = 16;
|
||||
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;
|
||||
/* Data */
|
||||
uint8_t buf_[BUF_LEN_];
|
||||
|
||||
void fillDataBuf() {
|
||||
/* Assemble packet */
|
||||
buf_[0] = HEADER_;
|
||||
buf_[1] = static_cast<uint8_t>((ch[0] & 0x07FF));
|
||||
buf_[2] = static_cast<uint8_t>((ch[0] & 0x07FF) >> 8 |
|
||||
(ch[1] & 0x07FF) << 3);
|
||||
buf_[3] = static_cast<uint8_t>((ch[1] & 0x07FF) >> 5 |
|
||||
(ch[2] & 0x07FF) << 6);
|
||||
buf_[4] = static_cast<uint8_t>((ch[2] & 0x07FF) >> 2);
|
||||
buf_[5] = static_cast<uint8_t>((ch[2] & 0x07FF) >> 10 |
|
||||
(ch[3] & 0x07FF) << 1);
|
||||
buf_[6] = static_cast<uint8_t>((ch[3] & 0x07FF) >> 7 |
|
||||
(ch[4] & 0x07FF) << 4);
|
||||
buf_[7] = static_cast<uint8_t>((ch[4] & 0x07FF) >> 4 |
|
||||
(ch[5] & 0x07FF) << 7);
|
||||
buf_[8] = static_cast<uint8_t>((ch[5] & 0x07FF) >> 1);
|
||||
buf_[9] = static_cast<uint8_t>((ch[5] & 0x07FF) >> 9 |
|
||||
(ch[6] & 0x07FF) << 2);
|
||||
buf_[10] = static_cast<uint8_t>((ch[6] & 0x07FF) >> 6 |
|
||||
(ch[7] & 0x07FF) << 5);
|
||||
buf_[11] = static_cast<uint8_t>((ch[7] & 0x07FF) >> 3);
|
||||
buf_[12] = static_cast<uint8_t>((ch[8] & 0x07FF));
|
||||
buf_[13] = static_cast<uint8_t>((ch[8] & 0x07FF) >> 8 |
|
||||
(ch[9] & 0x07FF) << 3);
|
||||
buf_[14] = static_cast<uint8_t>((ch[9] & 0x07FF) >> 5 |
|
||||
(ch[10] & 0x07FF) << 6);
|
||||
buf_[15] = static_cast<uint8_t>((ch[10] & 0x07FF) >> 2);
|
||||
buf_[16] = static_cast<uint8_t>((ch[10] & 0x07FF) >> 10 |
|
||||
(ch[11] & 0x07FF) << 1);
|
||||
buf_[17] = static_cast<uint8_t>((ch[11] & 0x07FF) >> 7 |
|
||||
(ch[12] & 0x07FF) << 4);
|
||||
buf_[18] = static_cast<uint8_t>((ch[12] & 0x07FF) >> 4 |
|
||||
(ch[13] & 0x07FF) << 7);
|
||||
buf_[19] = static_cast<uint8_t>((ch[13] & 0x07FF) >> 1);
|
||||
buf_[20] = static_cast<uint8_t>((ch[13] & 0x07FF) >> 9 |
|
||||
(ch[14] & 0x07FF) << 2);
|
||||
buf_[21] = static_cast<uint8_t>((ch[14] & 0x07FF) >> 6 |
|
||||
(ch[15] & 0x07FF) << 5);
|
||||
buf_[22] = static_cast<uint8_t>((ch[15] & 0x07FF) >> 3);
|
||||
buf_[23] = 0x00 | (ch17 * CH17_MASK_) | (ch18 * CH18_MASK_) |
|
||||
(failsafe * FAILSAFE_MASK_) |
|
||||
(lost_frame * LOST_FRAME_MASK_);
|
||||
buf_[24] = FOOTER_;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
class SerialPort {
|
||||
private:
|
||||
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::cerr << "Failed to open serial port " << port_path << ": " << std::strerror(errno) << std::endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
struct termios2 tty2;
|
||||
if (ioctl(fd, TCGETS2, &tty2) != 0) {
|
||||
std::cerr << "Failed to get termios2 attributes: " << std::strerror(errno) << std::endl;
|
||||
close(fd);
|
||||
return false;
|
||||
}
|
||||
|
||||
// Отключаем все флаги управления потоком и прочие опции
|
||||
tty2.c_cflag &= ~CSIZE;
|
||||
tty2.c_cflag |= CS8; // 8 бит данных
|
||||
tty2.c_cflag |= PARENB; // включаем четность
|
||||
tty2.c_cflag &= ~PARODD; // even parity
|
||||
tty2.c_cflag |= CSTOPB; // 2 стоп-бита
|
||||
tty2.c_cflag |= (CLOCAL | CREAD);
|
||||
|
||||
tty2.c_iflag &= ~(IXON | IXOFF | IXANY | INLCR | ICRNL);
|
||||
tty2.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
|
||||
tty2.c_oflag &= ~OPOST;
|
||||
|
||||
tty2.c_cc[VMIN] = 0;
|
||||
tty2.c_cc[VTIME] = 5;
|
||||
|
||||
// Устанавливаем нестандартную скорость
|
||||
tty2.c_cflag &= ~CBAUD;
|
||||
tty2.c_cflag |= BOTHER;
|
||||
tty2.c_ispeed = 100000;
|
||||
tty2.c_ospeed = 100000;
|
||||
|
||||
if (ioctl(fd, TCSETS2, &tty2) != 0) {
|
||||
std::cerr << "Failed to set termios2 attributes: " << std::strerror(errno) << std::endl;
|
||||
close(fd);
|
||||
return false;
|
||||
}
|
||||
|
||||
// tcflush(fd, TCIOFLUSH);
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
// Метод для записи данных в порт
|
||||
bool write(std::span<const uint8_t> data) {
|
||||
if (fd < 0) return false;
|
||||
|
||||
ssize_t written = ::write(fd, data.data(), data.size());
|
||||
if (written < 0) {
|
||||
std::cerr << "Failed to write to serial port" << std::endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
// Принудительная отправка данных
|
||||
// tcdrain(fd);
|
||||
return true;
|
||||
}
|
||||
};
|
||||
|
||||
int main(int argc, char* argv[]) {
|
||||
// Парсим аргументы командной строки
|
||||
std::string serial_port = "/dev/ttyUSB0";
|
||||
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{};
|
||||
|
||||
int packet_count = 0;
|
||||
while (true) {
|
||||
// Прием UDP пакета
|
||||
std::vector<uint16_t> data = udp_server.receive();
|
||||
|
||||
if (!data.empty()) {
|
||||
packet_count++;
|
||||
|
||||
for (int i = 0; i < data.size() && i < sb.NUM_CH; ++i) {
|
||||
sb.ch[i] = static_cast<int16_t>(data[i]);
|
||||
}
|
||||
sb.fillDataBuf();
|
||||
serial.write(sb.buf_);
|
||||
|
||||
// Выводим статистику каждые 100 пакетов
|
||||
if (packet_count % 100 == 0) {
|
||||
std::cout << "Received " << packet_count << " packets total" << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
// Небольшая пауза чтобы не грузить CPU
|
||||
usleep(1000); // 1 мс
|
||||
}
|
||||
|
||||
} catch (const std::exception& e) {
|
||||
std::cerr << "Error: " << e.what() << std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
std::cout << "Exiting..." << std::endl;
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -69,7 +69,7 @@ bool JoystickReader::readData(std::vector<uint16_t>& data) {
|
||||
|
||||
for (auto& i: data) {
|
||||
if (i < 900) i = 900;
|
||||
if (i > 1100) i = 1100;
|
||||
if (i > 2100) i = 2100;
|
||||
}
|
||||
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(updateIntervalMs));
|
||||
|
||||
@@ -9,7 +9,7 @@
|
||||
int main(int argc, char* argv[]) {
|
||||
|
||||
// Парсим аргументы командной строки для частоты
|
||||
int frequency = 50;
|
||||
int frequency = 5;
|
||||
const char* sendAddress;
|
||||
if (argc == 1) {
|
||||
sendAddress = "127.0.0.1";
|
||||
|
||||
Reference in New Issue
Block a user