VladislavOstapov/sdrpi-fpv-control
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 2 Wiki Activity

попытки сделать crsf, не взлетела часть с пультом

Browse Source
This commit is contained in:
Vladislav Ostapov
2025-11-24 16:24:59 +03:00
parent 0f14fd0155
commit 3eaea1b966
20 changed files with 769 additions and 535 deletions
Download Patch File Download Diff File Expand all files Collapse all files

80
ground/joystick-reader.cpp
View File

@@ -1,80 +0,0 @@
#include "joystick-reader.h"
#include <iostream>
#include <thread>
#include <chrono>
JoystickReader::JoystickReader()
: joystick(nullptr) {}
JoystickReader::~JoystickReader() {
if (joystick) {
SDL_JoystickClose(joystick);
}
SDL_Quit();
}
bool JoystickReader::initialize() {
if (SDL_Init(SDL_INIT_JOYSTICK) < 0) {
std::cerr << "SDL initialization failed: " << SDL_GetError() << std::endl;
return false;
}
int numJoysticks = SDL_NumJoysticks();
if (numJoysticks < 1) {
std::cerr << "No joysticks found" << std::endl;
return false;
}
std::cout << "Found " << numJoysticks << " joystick(s)" << std::endl;
// Открываем первый джойстик
joystick = SDL_JoystickOpen(0);
if (!joystick) {
std::cerr << "Failed to open joystick: " << SDL_GetError() << std::endl;
return false;
}
joystickName = SDL_JoystickName(joystick);
std::cout << "Opened joystick: " << joystickName << std::endl;
std::cout << "Axes: " << SDL_JoystickNumAxes(joystick)
<< ", Buttons: " << SDL_JoystickNumButtons(joystick)
<< ", Balls: " << SDL_JoystickNumBalls(joystick)
<< ", Hats: " << SDL_JoystickNumHats(joystick) << std::endl;
return true;
}
bool JoystickReader::readData(std::vector<uint16_t>& data) {
if (!joystick) return false;
if (SDL_JoystickGetAttached(joystick) != SDL_TRUE) { return false; }
SDL_JoystickUpdate();
data.resize(24); // Заполняем нейтральным значением
for (auto& i: data) { i = 1500; }
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) {
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 < 950) i = 950;
if (i > 2050) i = 2050;
}
return true;
}
std::string JoystickReader::getJoystickName() const {
return joystickName;
}

28
ground/joystick-reader.h
View File

@@ -1,28 +0,0 @@
#ifndef SDRPI_FPV_CONTROL_GROUND_JOYSTICK_READER_H
#define SDRPI_FPV_CONTROL_GROUND_JOYSTICK_READER_H
#include <cstdint>
#include <vector>
#include <string>
#ifdef _WIN32
#include <SDL.h>
#else
#include <SDL2/SDL.h>
#endif
class JoystickReader {
public:
JoystickReader();
~JoystickReader();
bool initialize();
bool readData(std::vector<uint16_t>& data);
std::string getJoystickName() const;
private:
SDL_Joystick* joystick;
std::string joystickName;
};
#endif //SDRPI_FPV_CONTROL_GROUND_JOYSTICK_READER_H

102
ground/main.cpp
View File

@@ -1,58 +1,86 @@
#include <iostream>
#include <vector>
#include <thread>
#include <csignal>
#include <port/poller.h>
#include <port/uart.h>
#include <port/udp.h>
#include <crsf.h>
#include "joystick-reader.h"
#include "udp-driver.h"
#include "port/poller.h"
#include "port/uart.h"
#include "port/udp.h"
static constexpr uint16_t UDP_PORT = 1067;
int main(int argc, char* argv[]) {
// Парсим аргументы командной строки для частоты
int frequency = 5;
const char* sendAddress;
if (argc == 1) {
sendAddress = "127.0.0.1";
} else if (argc == 2) {
sendAddress = argv[1];
} else if (argc == 3) {
sendAddress = argv[1];
frequency = std::atoi(argv[2]);
} else {
std::cerr << "Usage: " << argv[0] << " send.ip.addr.v4 [Frequency]" << std::endl;
if (argc < 3 || argc > 4) {
std::cerr << "Usage: " << argv[0]
<< " serial_port remote.ip.addr.4 [control packets frequency]\n";
return 1;
}
std::cout << "Starting joystick reader with frequency: " << frequency << " Hz" << std::endl;
const std::string serialPort = argv[1];
const std::string remoteIp = argv[2];
const int controlFreq = (argc == 4) ? std::atoi(argv[3]) : 50;
JoystickReader reader;
const int64_t timeInterval = 1000 / frequency;
while (!reader.initialize()) {
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
}
// UART speed fixed at 400k per your earlier specification
constexpr int UART_BAUD = 400000;
UDPSocket udp(sendAddress, 1066);
std::vector<uint16_t> data;
std::cout << "Opening UART: " << serialPort << " @ " << UART_BAUD << "\n";
std::cout << "Remote CRSF UDP endpoint: " << remoteIp << ":14550\n";
std::cout << "Control packets frequency: " << controlFreq << " Hz\n";
std::cout << "Sending data to " << sendAddress << ":1066" << std::endl;
poller::PollWrapper poller;
int counter = 0;
while (reader.readData(data)) {
if (!udp.sendFrame(data)) {
std::cerr << "Failed to send UDP packet" << std::endl;
auto uart = std::make_shared<drivers::UartDriver>(serialPort, UART_BAUD);
auto udp = std::make_shared<drivers::UdpDriver>(UDP_PORT);
poller.objects.push_back(uart);
poller.objects.push_back(udp);
crsf::CrsfParser parser;
// CrsfTxProcessor processor(udp, uart, remoteIp, UDP_PORT, controlFreq);
std::vector<uint8_t> tmpBuffer;
auto lastTick = std::chrono::steady_clock::now();
while (true) {
poller.loop(1000);
if (uart->isPollHup()) {
std::cerr << "UART disconnected!\n";
break;
}
if (udp->isPollHup()) {
std::cerr << "UDP socket error!\n";
break;
}
// Выводим прогресс каждые 100 пакетов
if (++counter % 100 == 0) {
std::cout << "Sent " << counter << " packets" << std::endl;
bool eventReceived = false;
// UART input
if (uart->isPollIn()) {
eventReceived = true;
size_t read = uart->readChunk(tmpBuffer);
if (read > 0) {
parser.parseBytes(tmpBuffer);
}
}
// вычислим время для сна
// должно быть время now+timeInterval
std::this_thread::sleep_for(std::chrono::milliseconds(timeInterval));
while (true) {
auto pkt = parser.pullPacket();
if (pkt == nullptr) {
break;
}
pkt->writeToBuffer(tmpBuffer);
udp->sendTo(std::span<const uint8_t>(tmpBuffer), remoteIp, UDP_PORT);
}
if (!eventReceived) {
std::cout << "[W]: No actions received in last 1s!" << std::endl;
}
}
std::cout << "Exiting..." << std::endl;
return 0;
}

111
ground/udp-driver.cpp
View File

@@ -1,111 +0,0 @@
#include "udp-driver.h"
#include <iostream>
#include <cstring>
UDPSocket::UDPSocket(const std::string& ip, uint16_t port) {
#ifdef _WIN32
sockfd = INVALID_SOCKET;
#else
sockfd = -1;
#endif
if (!initialize()) {
return;
}
// Создание сокета
#ifdef _WIN32
sockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
if (sockfd == INVALID_SOCKET) {
std::cerr << "Socket creation failed: " << WSAGetLastError() << std::endl;
return;
}
#else
sockfd = socket(AF_INET, SOCK_DGRAM, 0);
if (sockfd < 0) {
std::cerr << "Socket creation failed" << std::endl;
return;
}
#endif
// Настройка адреса назначения
memset(&dest_addr, 0, sizeof(dest_addr));
dest_addr.sin_family = AF_INET;
dest_addr.sin_port = htons(port);
#ifdef _WIN32
dest_addr.sin_addr.s_addr = inet_addr(ip.c_str());
if (dest_addr.sin_addr.s_addr == INADDR_NONE) {
std::cerr << "Invalid address: " << ip << std::endl;
closesocket(sockfd);
sockfd = INVALID_SOCKET;
}
#else
if (inet_pton(AF_INET, ip.c_str(), &dest_addr.sin_addr) <= 0) {
std::cerr << "Invalid address: " << ip << std::endl;
close(sockfd);
sockfd = -1;
}
#endif
}
bool UDPSocket::initialize() {
#ifdef _WIN32
// Инициализация Winsock
if (WSAStartup(MAKEWORD(2, 2), &wsaData) != 0) {
std::cerr << "WSAStartup failed: " << WSAGetLastError() << std::endl;
return false;
}
#endif
return true;
}
UDPSocket::~UDPSocket() {
#ifdef _WIN32
if (sockfd != INVALID_SOCKET) {
closesocket(sockfd);
}
WSACleanup();
#else
if (sockfd >= 0) {
close(sockfd);
}
#endif
}
bool UDPSocket::sendFrame(const std::vector<uint16_t>& data) {
#ifdef _WIN32
if (sockfd == INVALID_SOCKET) return false;
#else
if (sockfd < 0) return false;
#endif
if (data.empty()) return false;
// Отправка данных
#ifdef _WIN32
int result = sendto(sockfd,
reinterpret_cast<const char*>(data.data()),
data.size() * sizeof(uint16_t),
0,
(sockaddr*)&dest_addr,
sizeof(dest_addr));
if (result == SOCKET_ERROR) {
std::cerr << "Send failed: " << WSAGetLastError() << std::endl;
return false;
}
#else
ssize_t result = sendto(sockfd,
data.data(),
data.size() * sizeof(uint16_t),
0,
(sockaddr*)&dest_addr,
sizeof(dest_addr));
if (result < 0) {
std::cerr << "Send failed" << std::endl;
return false;
}
#endif
return true;
}

37
ground/udp-driver.h
View File

@@ -1,37 +0,0 @@
#ifndef SDRPI_FPV_CONTROL_GROUND_UDP_DRIVER_H
#define SDRPI_FPV_CONTROL_GROUND_UDP_DRIVER_H
#include <string>
#include <cstdint>
#include <vector>
#ifdef _WIN32
#include <winsock2.h>
#include <ws2tcpip.h>
#pragma comment(lib, "ws2_32.lib")
#else
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <unistd.h>
#endif
class UDPSocket {
public:
UDPSocket(const std::string& ip, uint16_t port);
~UDPSocket();
bool sendFrame(const std::vector<uint16_t>& data);
private:
#ifdef _WIN32
SOCKET sockfd;
WSADATA wsaData;
#else
int sockfd;
#endif
sockaddr_in dest_addr;
bool initialize();
};
#endif //SDRPI_FPV_CONTROL_GROUND_UDP_DRIVER_H