azaion/autopilot
1
0
Fork 0
mirror of https://github.com/azaion/autopilot.git synced 2026-04-22 11:26:35 +00:00
Code Issues Packages Projects Releases Wiki Activity

Changed FIFO pipes to UDP.

Browse Source
This commit is contained in:
Nffj84
2024-07-04 15:20:48 +03:00
parent 5af90ac918
commit 89c68aeb67
10 changed files with 99 additions and 346 deletions
Download Patch File Download Diff File Expand all files Collapse all files
misc/camera/a8_remote/remoteControl.cpp
+17 -170
View File
@@ -1,185 +1,32 @@
#include "remoteControl.hpp"
#include <QCoreApplication>
#include <QDebug>
#include <QHostAddress>
#include <QJsonDocument>
#include <QJsonObject>
#include <QThread>
#include <QTimer>
#include <QUdpSocket>
#include <fcntl.h>
#include <sys/stat.h>
#include <unistd.h>
#ifdef FIFO_TEST
#include <iostream>
#include <random>
#endif
RemoteControl::RemoteControl()
uint8_t RemoteControl::sendData(uint16_t top, uint16_t left, uint16_t bottom, uint16_t right)
{
createNamedPipe();
}
RemoteControl::~RemoteControl()
{
#ifdef FIFO_TEST
if (mFifoFdIn != -1) {
close(mFifoFdIn);
}
#endif
if (mFifoFdOut != -1) {
close(mFifoFdOut);
}
}
void RemoteControl::createNamedPipe()
{
struct stat statBuf;
#ifdef FIFO_TEST
// Open incoming pipe (READ ONLY)
if (stat(FIFO_FROM_GIMBAL, &statBuf) == 0) {
if (S_ISFIFO(statBuf.st_mode)) {
qInfo().noquote().nospace() << "Named pipe already exists: " << FIFO_FROM_GIMBAL;
} else {
qCritical().noquote().nospace() << FIFO_FROM_GIMBAL << " exists but is not a FIFO";
}
} else {
if (mkfifo(FIFO_FROM_GIMBAL, 0666) == -1) {
perror("mkfifo");
qCritical().noquote().nospace() << "Failed to create named pipe: " << FIFO_FROM_GIMBAL;
} else {
qInfo().noquote().nospace() << "Created named pipe: " << FIFO_FROM_GIMBAL;
}
}
mFifoFdIn = open(FIFO_FROM_GIMBAL, O_RDONLY | O_NONBLOCK);
if (mFifoFdIn == -1) {
qCritical().noquote().nospace() << "Error opening pipe for reading: " << FIFO_FROM_GIMBAL;
} else {
qInfo().noquote().nospace() << "Opened pipe: " << FIFO_FROM_GIMBAL;
}
#endif
// Open outgoing pipe (WRITE ONLY)
if (stat(FIFO_TO_GIMBAL, &statBuf) == 0) {
if (S_ISFIFO(statBuf.st_mode)) {
qInfo().noquote().nospace() << "Named pipe already exists: " << FIFO_TO_GIMBAL;
} else {
qCritical().noquote().nospace() << FIFO_TO_GIMBAL << " exists but is not a FIFO";
}
} else {
if (mkfifo(FIFO_TO_GIMBAL, 0666) == -1) {
perror("mkfifo");
qCritical().noquote().nospace() << "Failed to create named pipe: " << FIFO_TO_GIMBAL;
} else {
qInfo().noquote().nospace() << "Created named pipe: " << FIFO_TO_GIMBAL;
}
}
mFifoFdOut = open(FIFO_TO_GIMBAL, O_WRONLY);
if (mFifoFdOut == -1) {
qCritical().noquote().nospace() << "Error opening pipe for writing: " << FIFO_TO_GIMBAL;
} else {
qInfo().noquote().nospace() << "Opened pipe: " << FIFO_TO_GIMBAL;
}
}
void RemoteControl::sendData(uint16_t top, uint16_t left, uint16_t bottom, uint16_t right)
{
QJsonObject commandObject = {{"sender", FIFO_WHO_AM_I}, {"top", top}, {"left", left}, {"bottom", bottom}, {"right", right}};
QUdpSocket udpSocket;
QJsonObject commandObject = {{"sender", UDP_WHO_AM_I}, {"top", top}, {"left", left}, {"bottom", bottom}, {"right", right}};
QJsonDocument commandDocument(commandObject);
std::string command = commandDocument.toJson(QJsonDocument::Compact).toStdString();
size_t bytesWritten = write(mFifoFdOut, command.c_str(), command.size());
if (bytesWritten < 1) {
qWarning().noquote().nospace() << "Error writing data: " << QString::number(bytesWritten);
QByteArray datagram = commandDocument.toJson(QJsonDocument::Compact);
qint64 bytesSent = udpSocket.writeDatagram(datagram, QHostAddress("127.0.0.1"), UDP_PORT);
if (bytesSent == -1) {
qWarning("Failed to send the datagram: %s", qPrintable(udpSocket.errorString()));
return 1;
} else {
qDebug("Datagram sent successfully");
}
qDebug().noquote().nospace() << "Sent: " << command.c_str();
QThread::msleep(100);
return 0;
}
#ifdef FIFO_TEST
void RemoteControl::startTest()
{
QJsonObject commandObject;
for (uint8_t i = 0; i < 5; i++) {
qInfo().noquote().nospace() << "Enter a command (EXIT to exit): ";
std::string input;
std::cin >> input;
// Send command
QJsonObject commandObject = {
{"sender", FIFO_WHO_AM_I},
{"latitude", 63.155653611},
{"longitude", 23.827191389},
{"altitude", randomFloatBetween(10, 11)},
{"yaw", randomFloatBetween(0.0f, 360.0f)},
{"pitch", 0.0},
{"target_x", (uint16_t) randomFloatBetween(300, 979)},
{"target_y", (uint16_t) randomFloatBetween(200, 519)},
{"target_pixel_width", 20},
{"target_pixel_height", 10},
{"target_real_width", 5},
{"target_real_height", 2.5},
{"extra", input.c_str()},
{"top", 100},
{"left", 100},
{"bottom", 200},
{"right", 200},
};
QJsonDocument commandDocument(commandObject);
std::string command = commandDocument.toJson(QJsonDocument::Compact).toStdString();
size_t bytesWritten = write(mFifoFdOut, command.c_str(), command.size());
if (bytesWritten < 1) {
qWarning().noquote().nospace() << "Error writing data: " << bytesWritten;
}
qDebug().noquote().nospace() << "Sent: " << command;
}
if (commandObject["extra"] == "EXIT") {
exit(EXIT_SUCCESS);
}
while (true) {
char buffer[1024];
ssize_t bytesRead = read(mFifoFdIn, buffer, sizeof(buffer) - 1);
if (bytesRead > 0) {
buffer[bytesRead] = '\0';
QJsonDocument responseDoc = QJsonDocument::fromJson(buffer);
// Ignore non json messages
if (responseDoc.isNull() == false) {
QJsonObject responseObject = responseDoc.object();
// Ignore own messages and messages that don't have sender
if (responseObject.contains("sender") == false) {
continue;
}
QString message = QString::fromUtf8(buffer);
qDebug().noquote().nospace() << "Received: " << message;
if (responseObject.contains("status")) {
qInfo().noquote().nospace() << responseObject["sender"].toString() << " says: " << responseObject["status"].toString();
return;
}
}
}
// Sleep for a while
QCoreApplication::processEvents();
QThread::msleep(100);
}
}
float RemoteControl::randomFloatBetween(float min, float max)
{
// Use modern C++ random number generator
std::random_device rd; // Will be seeded from OS randomness
std::mt19937 gen(rd()); // Seed the generator
std::uniform_real_distribution<float> dist(min, max);
// Generate a random float between min and max (inclusive)
return dist(gen);
}
#endif