azaion/autopilot
1
0
Fork 0
mirror of https://github.com/azaion/autopilot.git synced 2026-04-22 07:16:34 +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/defines.hpp
+2 -3
View File
@@ -53,6 +53,5 @@ enum COMMAND_ID {
#define GIMBAL_PITCH_MAX 25.0f
#define SERIAL_RESPONSE_WAIT_TIME 500
#define SERIAL_PORT "/dev/ttyUSB0"
#define FIFO_WHO_AM_I "CAM"
#define FIFO_TO_GIMBAL "/tmp/fifo_to_a8_gimbal"
#define FIFO_FROM_GIMBAL "/tmp/fifo_from_a8_gimbal"
#define UDP_WHO_AM_I "CAM"
#define UDP_PORT 26662
misc/camera/a8/localControl.cpp
+3 -1
View File
@@ -10,7 +10,9 @@
LocalControl::LocalControl()
: QObject(nullptr)
{}
{
run();
}
void LocalControl::run()
{
misc/camera/a8/localControl.hpp
+2
View File
@@ -8,5 +8,7 @@ class LocalControl : public QObject
public:
LocalControl();
private:
void run();
};
misc/camera/a8/main.cpp
-2
View File
@@ -34,10 +34,8 @@ int main(int argc, char *argv[])
// Remote mode will read commands from pipe
if (useRemoteMode == true) {
RemoteControl remoteControl;
remoteControl.run();
} else {
LocalControl localControl;
localControl.run();
}
return app.exec();
misc/camera/a8/remoteControl.cpp
+63 -139
View File
@@ -12,83 +12,87 @@
#include <sys/stat.h>
#include <unistd.h>
RemoteControl::RemoteControl()
: QObject(nullptr)
RemoteControl::RemoteControl(QObject *parent)
: QObject{parent}
{
openNamedPipe();
mUdpSocket = new QUdpSocket(this);
connect(mUdpSocket, &QUdpSocket::readyRead, this, &RemoteControl::readPendingDatagrams);
mUdpSocket->bind(QHostAddress::Any, UDP_PORT);
}
RemoteControl::~RemoteControl()
void RemoteControl::readPendingDatagrams()
{
if (mFifoFdIn != -1) {
close(mFifoFdIn);
while (mUdpSocket->hasPendingDatagrams()) {
QByteArray datagram;
datagram.resize(mUdpSocket->pendingDatagramSize());
mUdpSocket->readDatagram(datagram.data(), datagram.size());
QJsonParseError jsonError;
QJsonDocument jsonDoc = QJsonDocument::fromJson(datagram, &jsonError);
if (jsonError.error != QJsonParseError::NoError) {
qWarning() << "Error parsing JSON:" << jsonError.errorString();
continue;
}
if (jsonDoc.isObject()) {
if (mIsBusy == false) {
processJSON(jsonDoc);
}
} else {
qWarning().noquote().nospace() << "Received data was not JSON object.";
}
if (mFifoFdOut != -1) {
close(mFifoFdOut);
}
}
void RemoteControl::openNamedPipe()
void RemoteControl::processJSON(QJsonDocument commandDoc)
{
struct stat statBuf;
mIsBusy = true;
qDebug().noquote().nospace() << "Received: " << commandDoc.toJson().toStdString();
// Open incoming pipe (READ 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;
}
QJsonObject commandObject = commandDoc.object();
// Exit with exit message
if (commandObject.contains("extra") == true && commandObject["extra"] == "EXIT") {
qDebug().noquote().nospace() << "Exit message received: Exiting...";
exit(EXIT_SUCCESS);
}
mFifoFdIn = open(FIFO_TO_GIMBAL, O_RDWR | O_NONBLOCK);
if (mFifoFdIn == -1) {
qCritical().noquote().nospace() << "Error opening pipe for reading: " << FIFO_TO_GIMBAL;
} else {
qInfo().noquote().nospace() << "Opened pipe: " << FIFO_TO_GIMBAL;
}
// TODO:
// Wait that these are actually available
// Without these it is impossible to calculate target location
commandObject["altitude"] = 10.5f;
commandObject["latitude"] = 55.75000000f;
commandObject["longitude"] = 37.61666670f;
commandObject["pitch"] = 0.0f;
commandObject["yaw"] = 152.5f;
commandObject["target_real_height"] = 2.5f;
commandObject["target_real_width"] = 5.0f;
// Open outgoing pipe (WRITE 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;
}
}
// Rectangle calculation for having proper zoom on group / target
RectangleProperties rectangle = calculateRectangleProperties(commandObject["top"].toInt(), commandObject["left"].toInt(), commandObject["bottom"].toInt(), commandObject["right"].toInt());
commandObject["target_x"] = rectangle.middleX;
commandObject["target_y"] = rectangle.middleY;
commandObject["target_pixel_height"] = rectangle.height;
commandObject["target_pixel_width"] = rectangle.width;
qInfo() << "target_x: " << commandObject["target_x"];
qInfo() << "target_y: " << commandObject["target_y"];
qInfo() << "target_pixel_height: " << commandObject["target_pixel_height"];
qInfo() << "target_pixel_width: " << commandObject["target_pixel_width"];
mFifoFdOut = open(FIFO_FROM_GIMBAL, O_WRONLY);
if (mFifoFdOut == -1) {
qCritical().noquote().nospace() << "Error opening pipe for writing: " << FIFO_FROM_GIMBAL;
} else {
qInfo().noquote().nospace() << "Opened pipe: " << FIFO_FROM_GIMBAL;
}
// Prepare responce object
mResponseObject = QJsonObject();
mResponseObject["sender"] = UDP_WHO_AM_I;
mResponseObject["status"] = "OK";
// Get current orientation and zoom
Config::updateState();
QTimer::singleShot(0, this, [this, commandObject]() mutable { turnToTarget(commandObject); });
}
void RemoteControl::sendResponse(void)
{
QJsonDocument responseDocument(mResponseObject);
std::string response = responseDocument.toJson(QJsonDocument::Compact).toStdString();
size_t bytesWritten = write(mFifoFdOut, response.c_str(), response.size());
if (bytesWritten < 1) {
qWarning().noquote().nospace() << "Error writing response: " << bytesWritten;
}
qDebug().noquote().nospace() << "Responded: " << response.c_str();
// TODO: Implement
mIsBusy = false;
}
@@ -242,83 +246,3 @@ RectangleProperties RemoteControl::calculateRectangleProperties(uint16_t top, ui
return properties;
}
void RemoteControl::run()
{
mIsBusy = false;
while (true) {
if (mIsBusy == false) {
char buffer[1024];
ssize_t bytesRead = read(mFifoFdIn, buffer, sizeof(buffer) - 1);
if (bytesRead > 0) {
mIsBusy = true;
buffer[bytesRead] = '\0';
QJsonDocument commandDoc = QJsonDocument::fromJson(buffer);
QString message = QString::fromUtf8(buffer);
// Ignore non json messages
if (commandDoc.isNull() == false) {
qDebug().noquote().nospace() << "Received: " << message;
QJsonObject commandObject = commandDoc.object();
// Ignore messages that don't have sender
if (commandObject.contains("sender") == false) {
qDebug().noquote().nospace() << "No sender info: " << message;
mIsBusy = false;
continue;
}
// Exit with exit message
if (commandObject.contains("extra") == true && commandObject["extra"] == "EXIT") {
qDebug().noquote().nospace() << "Exit message received: Exiting...";
exit(EXIT_SUCCESS);
}
// TODO:
// Wait that these are actually available
// Without these it is impossible to calculate target location
commandObject["altitude"] = 10.5f;
commandObject["latitude"] = 55.75000000f;
commandObject["longitude"] = 37.61666670f;
commandObject["pitch"] = 0.0f;
commandObject["yaw"] = 152.5f;
commandObject["target_real_height"] = 2.5f;
commandObject["target_real_width"] = 5.0f;
// Rectangle calculation for having proper zoom on group / target
RectangleProperties rectangle = calculateRectangleProperties(commandObject["top"].toInt(), commandObject["left"].toInt(), commandObject["bottom"].toInt(), commandObject["right"].toInt());
commandObject["target_x"] = rectangle.middleX;
commandObject["target_y"] = rectangle.middleY;
commandObject["target_pixel_height"] = rectangle.height;
commandObject["target_pixel_width"] = rectangle.width;
qInfo() << "target_x: " << commandObject["target_x"];
qInfo() << "target_y: " << commandObject["target_y"];
qInfo() << "target_pixel_height: " << commandObject["target_pixel_height"];
qInfo() << "target_pixel_width: " << commandObject["target_pixel_width"];
// Prepare responce object
mResponseObject = QJsonObject();
mResponseObject["sender"] = FIFO_WHO_AM_I;
mResponseObject["status"] = "OK";
// Get current orientation and zoom
Config::updateState();
QTimer::singleShot(0, this, [this, commandObject]() mutable { turnToTarget(commandObject); });
} else {
qDebug().noquote().nospace() << "Non JSON message received: " << message;
mIsBusy = false;
}
} else {
mIsBusy = false;
}
}
QCoreApplication::processEvents();
QThread::msleep(100);
}
}
misc/camera/a8/remoteControl.hpp
+5 -6
View File
@@ -4,6 +4,7 @@
#include <QObject>
#include <QString>
#include <QThread>
#include <QtNetwork/QUdpSocket>
struct RectangleProperties
{
@@ -18,11 +19,11 @@ class RemoteControl : public QObject
Q_OBJECT
public:
RemoteControl();
~RemoteControl();
void run();
explicit RemoteControl(QObject *parent = nullptr);
private slots:
void readPendingDatagrams();
void processJSON(QJsonDocument jsonDoc);
void sendResponse(void);
void calculateTargetPosition(QJsonObject &commandObject);
void turnToTarget(QJsonObject &commandObject);
@@ -32,9 +33,7 @@ private slots:
RectangleProperties calculateRectangleProperties(uint16_t top, uint16_t left, uint16_t bottom, uint16_t right);
private:
void openNamedPipe(void);
bool mIsBusy;
int mFifoFdIn;
int mFifoFdOut;
QUdpSocket *mUdpSocket;
QJsonObject mResponseObject;
};
misc/camera/a8_remote/a8_remote.pro
+1
View File
@@ -1,4 +1,5 @@
QT += core
QT += network
QT -= gui
CONFIG += c++17 console
misc/camera/a8_remote/main.cpp
+2 -6
View File
@@ -5,11 +5,7 @@ int main(int argc, char *argv[])
{
QCoreApplication a(argc, argv);
RemoteControl remoteControl;
#ifdef FIFO_TEST
remoteControl.startTest();
#else
remoteControl.sendData(200, 200, 400, 400);
#endif
RemoteControl::sendData(200, 200, 400, 400);
return 0;
}
misc/camera/a8_remote/remoteControl.cpp
+15 -168
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);
}
qDebug().noquote().nospace() << "Sent: " << command.c_str();
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");
}
#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);
return 0;
}
}
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
misc/camera/a8_remote/remoteControl.hpp
+3 -18
View File
@@ -2,26 +2,11 @@
#include <cstdint>
#define FIFO_WHO_AM_I "AI"
#define FIFO_TO_GIMBAL "/tmp/fifo_to_a8_gimbal"
#define FIFO_FROM_GIMBAL "/tmp/fifo_from_a8_gimbal"
//#define FIFO_TEST
#define UDP_WHO_AM_I "AI"
#define UDP_PORT 26662
class RemoteControl
{
public:
RemoteControl();
~RemoteControl();
void sendData(uint16_t top, uint16_t left, uint16_t bottom, uint16_t right);
#ifdef FIFO_TEST
void startTest(void);
#endif
private:
void createNamedPipe(void);
#ifdef FIFO_TEST
float randomFloatBetween(float min, float max);
int mFifoFdIn;
#endif
int mFifoFdOut;
static uint8_t sendData(uint16_t top, uint16_t left, uint16_t bottom, uint16_t right);
};