autopilot/misc/camera/a8_remote/remoteControl.cpp

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

RemoteControl::RemoteControl() {}

RemoteControl::~RemoteControl()
{
    if (mFifoFdIn != -1) {
        close(mFifoFdIn);
    }
    if (mFifoFdOut != -1) {
        close(mFifoFdOut);
    }
}

void RemoteControl::createNamedPipe()
{
    struct stat statBuf;

    // 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;
    }

    // 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::startCommunication()
{
    qInfo().noquote().nospace() << "Enter a command (0 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(100, 500)},
        {"yaw", randomFloatBetween(0.0f, 360.0f)},
        {"pitch", 0.0},
        {"target_x", (uint16_t) randomFloatBetween(0, 1279)},
        {"target_y", (uint16_t) randomFloatBetween(0, 719)},
        {"target_pixel_width", 20},
        {"target_pixel_height", 10},
        {"target_real_width", 5},
        {"target_real_height", 2.5},
        {"something", input.c_str()},
    };

    QJsonDocument commandDocument(commandObject);
    std::string command = commandDocument.toJson(QJsonDocument::Compact).toStdString();
    write(mFifoFdOut, command.c_str(), command.size());
    qDebug().noquote().nospace() << "Sent: " << command;

    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();
        QTimer::singleShot();
    }

    return;
}

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);
}