добавил программу для воздуха, все вроде работает
This commit is contained in:
71
air/main.cpp
71
air/main.cpp
@@ -1,5 +1,5 @@
|
||||
#include <asm/termbits.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <asm/termbits.h>
|
||||
#include <sys/socket.h>
|
||||
#include <netinet/in.h>
|
||||
#include <arpa/inet.h>
|
||||
@@ -51,7 +51,7 @@ public:
|
||||
}
|
||||
|
||||
std::vector<uint16_t> receive() {
|
||||
std::vector<uint16_t> data(16);
|
||||
std::vector<uint16_t> data(64);
|
||||
|
||||
ssize_t received = recvfrom(sockfd,
|
||||
data.data(),
|
||||
@@ -61,8 +61,8 @@ public:
|
||||
&client_len);
|
||||
|
||||
if (received > 0) {
|
||||
if (received == 16 * sizeof(uint16_t)) {
|
||||
// Выводим информацию о отправителе и данные
|
||||
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;
|
||||
@@ -74,9 +74,6 @@ public:
|
||||
// std::cout << "..." << std::endl;
|
||||
|
||||
return data;
|
||||
} else {
|
||||
std::cerr << "Invalid packet size: " << received << " bytes" << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
return {};
|
||||
@@ -165,43 +162,30 @@ public:
|
||||
std::cerr << "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);
|
||||
|
||||
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) {
|
||||
if (ioctl(fd, TCSETS2, &tio) != 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;
|
||||
}
|
||||
@@ -268,8 +252,14 @@ int main(int argc, char* argv[]) {
|
||||
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]);
|
||||
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_);
|
||||
@@ -279,9 +269,6 @@ int main(int argc, char* argv[]) {
|
||||
std::cout << "Received " << packet_count << " packets total" << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
// Небольшая пауза чтобы не грузить CPU
|
||||
usleep(1000); // 1 мс
|
||||
}
|
||||
|
||||
} catch (const std::exception& e) {
|
||||
|
||||
@@ -3,8 +3,8 @@
|
||||
#include <thread>
|
||||
#include <chrono>
|
||||
|
||||
JoystickReader::JoystickReader(int frequency)
|
||||
: joystick(nullptr), updateIntervalMs(1000 / frequency) {}
|
||||
JoystickReader::JoystickReader()
|
||||
: joystick(nullptr) {}
|
||||
|
||||
JoystickReader::~JoystickReader() {
|
||||
if (joystick) {
|
||||
@@ -50,29 +50,28 @@ bool JoystickReader::readData(std::vector<uint16_t>& data) {
|
||||
|
||||
SDL_JoystickUpdate();
|
||||
|
||||
data.resize(16, 1500); // Заполняем нейтральным значением
|
||||
data.resize(24); // Заполняем нейтральным значением
|
||||
for (auto& i: data) { i = 1500; }
|
||||
|
||||
// Читаем оси (обычно до 6 осей)
|
||||
int axes = std::min(SDL_JoystickNumAxes(joystick), 6);
|
||||
for (int i = 0; i < axes; ++i) {
|
||||
int axes = std::min(SDL_JoystickNumAxes(joystick), 8);
|
||||
for (int i = 0; i < std::min(8, axes); ++i) {
|
||||
Sint16 axisValue = SDL_JoystickGetAxis(joystick, i);
|
||||
// Преобразуем из [-32768, 32767] в [1000, 2000]
|
||||
data[i] = static_cast<uint16_t>((axisValue + 32768) * 1000 / 65536 + 1000);
|
||||
}
|
||||
axes = 8;
|
||||
|
||||
// Читаем кнопки
|
||||
int buttons = SDL_JoystickNumButtons(joystick);
|
||||
for (int i = 0; i < buttons && i < data.size() - axes; ++i) {
|
||||
Uint8 buttonState = SDL_JoystickGetButton(joystick, i);
|
||||
data[axes + i] = buttonState ? 2000 : 1000;
|
||||
auto buttonState = SDL_JoystickGetButton(joystick, i);
|
||||
data[axes + i] = static_cast<uint16_t>(1000.0 + (buttonState * (1000.0 / 255.0)));
|
||||
}
|
||||
|
||||
for (auto& i: data) {
|
||||
if (i < 900) i = 900;
|
||||
if (i > 2100) i = 2100;
|
||||
if (i < 950) i = 950;
|
||||
if (i > 2050) i = 2050;
|
||||
}
|
||||
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(updateIntervalMs));
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
@@ -13,7 +13,7 @@
|
||||
|
||||
class JoystickReader {
|
||||
public:
|
||||
JoystickReader(int frequency = 50);
|
||||
JoystickReader();
|
||||
~JoystickReader();
|
||||
|
||||
bool initialize();
|
||||
@@ -22,7 +22,6 @@ public:
|
||||
|
||||
private:
|
||||
SDL_Joystick* joystick;
|
||||
int updateIntervalMs;
|
||||
std::string joystickName;
|
||||
};
|
||||
|
||||
|
||||
@@ -6,6 +6,12 @@
|
||||
#include "joystick-reader.h"
|
||||
#include "udp-driver.h"
|
||||
|
||||
int64_t milliseconds() {
|
||||
timeval tv{};
|
||||
gettimeofday(&tv,nullptr);
|
||||
return ((tv.tv_sec * 1000000l) + tv.tv_usec) / 1000;
|
||||
}
|
||||
|
||||
int main(int argc, char* argv[]) {
|
||||
|
||||
// Парсим аргументы командной строки для частоты
|
||||
@@ -25,7 +31,8 @@ int main(int argc, char* argv[]) {
|
||||
|
||||
std::cout << "Starting joystick reader with frequency: " << frequency << " Hz" << std::endl;
|
||||
|
||||
JoystickReader reader(frequency);
|
||||
JoystickReader reader;
|
||||
const int64_t timeInterval = 1000 / frequency;
|
||||
while (!reader.initialize()) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
|
||||
}
|
||||
@@ -36,6 +43,7 @@ int main(int argc, char* argv[]) {
|
||||
std::cout << "Sending data to " << sendAddress << ":1066" << std::endl;
|
||||
|
||||
int counter = 0;
|
||||
auto now = milliseconds();
|
||||
while (reader.readData(data)) {
|
||||
if (!udp.sendFrame(data)) {
|
||||
std::cerr << "Failed to send UDP packet" << std::endl;
|
||||
@@ -45,6 +53,15 @@ int main(int argc, char* argv[]) {
|
||||
if (++counter % 100 == 0) {
|
||||
std::cout << "Sent " << counter << " packets" << std::endl;
|
||||
}
|
||||
|
||||
// вычислим время для сна
|
||||
// должно быть время now+timeInterval
|
||||
auto targetTime = now + timeInterval;
|
||||
auto nowTime = milliseconds();
|
||||
if (targetTime > nowTime) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(targetTime - nowTime));
|
||||
}
|
||||
now = milliseconds();
|
||||
}
|
||||
|
||||
std::cout << "Exiting..." << std::endl;
|
||||
|
||||
Reference in New Issue
Block a user