azaion/autopilot
1
0
Fork 0
mirror of https://github.com/azaion/autopilot.git synced 2026-04-22 13:26: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
+64 -140
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);
}
if (mFifoFdOut != -1) {
close(mFifoFdOut);
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.";
}
}
}
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;
};