diff --git a/misc/camera/a8/defines.hpp b/misc/camera/a8/defines.hpp index 517100b..980c3ef 100644 --- a/misc/camera/a8/defines.hpp +++ b/misc/camera/a8/defines.hpp @@ -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 diff --git a/misc/camera/a8/localControl.cpp b/misc/camera/a8/localControl.cpp index c7561b4..ea5c8ac 100644 --- a/misc/camera/a8/localControl.cpp +++ b/misc/camera/a8/localControl.cpp @@ -10,7 +10,9 @@ LocalControl::LocalControl() : QObject(nullptr) -{} +{ + run(); +} void LocalControl::run() { diff --git a/misc/camera/a8/localControl.hpp b/misc/camera/a8/localControl.hpp index 3a40a86..e0a01e0 100644 --- a/misc/camera/a8/localControl.hpp +++ b/misc/camera/a8/localControl.hpp @@ -8,5 +8,7 @@ class LocalControl : public QObject public: LocalControl(); + +private: void run(); }; diff --git a/misc/camera/a8/main.cpp b/misc/camera/a8/main.cpp index 34d3868..9274192 100644 --- a/misc/camera/a8/main.cpp +++ b/misc/camera/a8/main.cpp @@ -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(); diff --git a/misc/camera/a8/remoteControl.cpp b/misc/camera/a8/remoteControl.cpp index a0563e9..979585b 100644 --- a/misc/camera/a8/remoteControl.cpp +++ b/misc/camera/a8/remoteControl.cpp @@ -12,83 +12,87 @@ #include #include -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); - } -} diff --git a/misc/camera/a8/remoteControl.hpp b/misc/camera/a8/remoteControl.hpp index e0d3317..5ecda8e 100644 --- a/misc/camera/a8/remoteControl.hpp +++ b/misc/camera/a8/remoteControl.hpp @@ -4,6 +4,7 @@ #include #include #include +#include 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; }; diff --git a/misc/camera/a8_remote/a8_remote.pro b/misc/camera/a8_remote/a8_remote.pro index 93436ea..6822f91 100644 --- a/misc/camera/a8_remote/a8_remote.pro +++ b/misc/camera/a8_remote/a8_remote.pro @@ -1,4 +1,5 @@ QT += core +QT += network QT -= gui CONFIG += c++17 console diff --git a/misc/camera/a8_remote/main.cpp b/misc/camera/a8_remote/main.cpp index a501f3c..90f6635 100644 --- a/misc/camera/a8_remote/main.cpp +++ b/misc/camera/a8_remote/main.cpp @@ -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; } diff --git a/misc/camera/a8_remote/remoteControl.cpp b/misc/camera/a8_remote/remoteControl.cpp index 0d3ef76..ebc9a84 100644 --- a/misc/camera/a8_remote/remoteControl.cpp +++ b/misc/camera/a8_remote/remoteControl.cpp @@ -1,185 +1,32 @@ #include "remoteControl.hpp" #include #include +#include #include #include #include #include +#include #include #include #include -#ifdef FIFO_TEST -#include -#include -#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 dist(min, max); - - // Generate a random float between min and max (inclusive) - return dist(gen); -} -#endif diff --git a/misc/camera/a8_remote/remoteControl.hpp b/misc/camera/a8_remote/remoteControl.hpp index 96c5b0f..9d43175 100644 --- a/misc/camera/a8_remote/remoteControl.hpp +++ b/misc/camera/a8_remote/remoteControl.hpp @@ -2,26 +2,11 @@ #include -#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); };