autopilot/misc/camera/a8_remote/remoteControl.cpp

#include "remoteControl.hpp"
#include <QCoreApplication>
#include <QDebug>
#include <QJsonDocument>
#include <QJsonObject>
#include <QThread>
#include <QTimer>
#include <fcntl.h>
#include <sys/stat.h>
#include <unistd.h>
#ifdef FIFO_TEST
#include <iostream>
#include <random>
#endif

RemoteControl::RemoteControl()
{
    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}};
    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();
}

#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