autopilot/misc/camera/a8/remoteControl.cpp

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

RemoteControl::RemoteControl()
    : QObject(nullptr)
{
    openNamedPipe();
}

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

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

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

    mFifoFdIn = open(FIFO_TO_GIMBAL, O_RDONLY | 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;
    }

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

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

void RemoteControl::whenDone(void)
{
    // All is done, no it's time to rest
}

void RemoteControl::restoreOrientation(void)
{
    QByteArray serialCommandAngle = Config::getCommand()->getCommandForInternal(COMMAND_ID::TURN_TO_DEGREES);

    int16_t degreesVal = Config::getCurrentYaw() * 10;
    serialCommandAngle[8] = degreesVal & 0xFF;
    serialCommandAngle[9] = degreesVal >> 8;
    degreesVal = Config::getCurrentPitch() * 10;
    serialCommandAngle[10] = degreesVal & 0xFF;
    serialCommandAngle[11] = degreesVal >> 8;

    qDebug().noquote().nospace() << serialCommandAngle;
    Config::getSerial()->sendCommand(serialCommandAngle);
}

void RemoteControl::restoreZoom(void)
{
    QByteArray serialCommandZoom = Config::getCommand()->getCommandForInternal(COMMAND_ID::ZOOM_TO_X);

    uint8_t integerPart = static_cast<uint8_t>(Config::getCurrentZoom());
    float fractionalPart = Config::getCurrentZoom() - integerPart;
    uint8_t scaledFractional = uint8_t(fractionalPart * 10);

    serialCommandZoom[8] = integerPart;
    serialCommandZoom[9] = scaledFractional;

    qDebug().noquote().nospace() << serialCommandZoom.toStdString();
    Config::getSerial()->sendCommand(serialCommandZoom);
}

QJsonObject RemoteControl::calculateTargetPosition(QJsonObject &commandObject, QJsonObject &responseObject)
{
    qDebug().noquote().nospace() << "Calculating?";

    float latitude = commandObject["latitude"].toDouble();
    float longitude = commandObject["longitude"].toDouble();
    float altitude = commandObject["altitude"].toDouble();
    float yaw = commandObject["yaw"].toDouble();
    float pitch = commandObject["pitch"].toDouble();
    float targetPixelWidth = commandObject["target_pixel_width"].toInt();
    //float targetPixelHeight = commandObject["target_pixel_height"].toInt();
    float targetRealWidth = commandObject["target_real_width"].toDouble();
    //float targetRealHeight = commandObject["target_real_height"].toDouble();

    GPSData gpsData = UtilsTargetLocation::getLocation(altitude, latitude, longitude, yaw, pitch, 0.0f, targetRealWidth, targetPixelWidth);
    responseObject["altitude"] = gpsData.altitude;
    responseObject["latitude"] = gpsData.latitude;
    responseObject["longitude"] = gpsData.longitude;

    return responseObject;
}

void RemoteControl::turnToTarget(QJsonObject &commandObject)
{
    qDebug().noquote().nospace() << "Turning?";

    uint16_t targetX = commandObject["target_x"].toInt();
    uint16_t targetY = commandObject["target_y"].toInt();

    float resultYaw = 0.0f;
    float resultPitch = 0.0f;
    UtilsTargetLocation::getAnglesToOnScreenTarget(targetX, targetY, resultYaw, resultPitch);

    QByteArray serialCommandTurn = Config::getCommand()->getCommandForInternal(COMMAND_ID::TURN_TO_PIXEL);

    int16_t degreesVal = resultYaw * 10;
    serialCommandTurn[8] = degreesVal & 0xFF;
    serialCommandTurn[9] = degreesVal >> 8;

    degreesVal = resultPitch * 10;
    serialCommandTurn[10] = degreesVal & 0xFF;
    serialCommandTurn[11] = degreesVal >> 8;

    Config::getSerial()->sendCommand(serialCommandTurn);
}

void RemoteControl::zoomToTarget(QJsonObject &commandObject)
{
    qDebug().noquote().nospace() << "Zooming?";

    float fillRatio = 0.5;
    float targetPixelWidth = commandObject["target_pixel_width"].toInt();
    float targetPixelHeight = commandObject["target_pixel_height"].toInt();
    float adjustedObjectWidth = targetPixelWidth / fillRatio;
    float adjustedObjectHeight = targetPixelHeight / fillRatio;
    float zoomWidth = static_cast<double>(Config::getResolutionWidth()) / adjustedObjectWidth;
    float zoomHeight = static_cast<double>(Config::getResolutionHeight()) / adjustedObjectHeight;
    float zoom = std::min(zoomWidth, zoomHeight);
    if (zoom < 1.0f) {
        zoom = 1.0f;
    }

    QByteArray serialCommandNewZoom = Config::getCommand()->getCommandForInternal(COMMAND_ID::ZOOM_TO_X);

    uint8_t integerPart = static_cast<uint8_t>(zoom);
    float fractionalPart = zoom - integerPart;
    uint8_t scaledFractional = uint8_t(fractionalPart * 10);

    serialCommandNewZoom[8] = integerPart;
    serialCommandNewZoom[9] = scaledFractional;

    Config::getSerial()->sendCommand(serialCommandNewZoom);
}

void RemoteControl::run()
{
    while (true) {
        char buffer[1024];
        ssize_t bytesRead = read(mFifoFdIn, buffer, sizeof(buffer) - 1);

        if (bytesRead > 0) {
            buffer[bytesRead] = '\0';

            QJsonDocument commandDoc = QJsonDocument::fromJson(buffer);

            // Ignore non json messages
            if (commandDoc.isNull() == false) {
                QJsonObject commandObject = commandDoc.object();

                // Ignore own messages and messages that don't have sender
                if (commandObject.contains("sender") == false || commandObject["sender"] == FIFO_WHO_AM_I) {
                    continue;
                }

                QString message = QString::fromUtf8(buffer);
                qDebug().noquote().nospace() << "Received: " << message;

                // Exit with exit message
                if (commandObject.contains("EXIT") == true) {
                    return;
                }

                // Prepare responce object
                QJsonObject responseObject;
                responseObject["sender"] = FIFO_WHO_AM_I;
                responseObject["status"] = "OK";

                // Get current orientation and zoom
                Config::updateState();

                // Turn to target
                if (commandObject.contains("target_x") == true && commandObject.contains("target_y") == true) {
                    QTimer::singleShot(100, this, [this, commandObject]() mutable { turnToTarget(commandObject); });
                    QTimer::singleShot(1000, this, &RemoteControl::whenDone);
                }

                // Calculate target location
                if (commandObject.contains("latitude") == true && commandObject.contains("longitude") == true && commandObject.contains("altitude") == true && commandObject.contains("yaw") == true && commandObject.contains("pitch") == true && commandObject.contains("target_pixel_width") == true && commandObject.contains("target_pixel_height") == true && commandObject.contains("target_real_width") == true && commandObject.contains("target_real_height") == true) {
                    responseObject = calculateTargetPosition(commandObject, responseObject);
                    QTimer::singleShot(1000, this, &RemoteControl::whenDone);
                }

                // Zoom to target
                if (commandObject.contains("target_pixel_width") == true && commandObject.contains("target_pixel_height") == true) {
                    QTimer::singleShot(100, this, [this, commandObject]() mutable { zoomToTarget(commandObject); });
                    QTimer::singleShot(1000, this, &RemoteControl::whenDone);
                }

                // Restore previous zoom and orientation
                QTimer::singleShot(100, this, &RemoteControl::restoreZoom);
                QTimer::singleShot(1000, this, &RemoteControl::whenDone);

                QTimer::singleShot(100, this, &RemoteControl::restoreOrientation);
                QTimer::singleShot(1000, this, &RemoteControl::whenDone);

                // Respond after doing camera things
                QJsonDocument responseDocument(responseObject);
                std::string response = responseDocument.toJson(QJsonDocument::Compact).toStdString();
                write(mFifoFdOut, response.c_str(), response.size());
                qDebug().noquote().nospace() << "Sent: " << response;
                QCoreApplication::processEvents();
            }
        }

        // Sleep for a while
        QCoreApplication::processEvents();
    }
}