autopilot/misc/camera/a8/remoteControl.cpp

#include "remoteControl.h"
#include <QCoreApplication>
#include <QDebug>
#include <QJsonDocument>
#include <QJsonObject>
#include <QObject>
#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_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;
    }

    // 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::sendResponse(void)
{
    QJsonDocument responseDocument(mResponseObject);
    std::string response = responseDocument.toJson(QJsonDocument::Compact).toStdString();

    qDebug().noquote().nospace() << "Sending responce: " << response;

    write(mFifoFdOut, response.c_str(), response.size());
    mIsBusy = false;
}

void RemoteControl::restoreOrientation(void)
{
    qDebug().noquote().nospace() << "Restoring orientation";

    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;

    Config::getSerial()->sendCommand(serialCommandAngle);

    QTimer::singleShot(0, this, [this]() mutable { sendResponse(); });
}

void RemoteControl::restoreZoom(void)
{
    qDebug().noquote().nospace() << "Restoring zoom";

    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;

    Config::getSerial()->sendCommand(serialCommandZoom);

    QTimer::singleShot(100, this, [this]() mutable { restoreOrientation(); });
}

void RemoteControl::calculateTargetPosition(QJsonObject &commandObject)
{
    qDebug().noquote().nospace() << "Getting target location";

    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);
    mResponseObject["altitude"] = gpsData.altitude;
    mResponseObject["latitude"] = gpsData.latitude;
    mResponseObject["longitude"] = gpsData.longitude;

    QTimer::singleShot(100, this, [this, commandObject]() mutable { zoomToTarget(commandObject); });
}

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

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

    QTimer::singleShot(1000, this, [this, commandObject]() mutable { calculateTargetPosition(commandObject); });
}

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

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

    QTimer::singleShot(5000, this, [this, commandObject]() mutable { restoreZoom(); });
}

RectangleProperties RemoteControl::calculateRectangleProperties(uint16_t top, uint16_t left, uint16_t bottom, uint16_t right)
{
    // Sanity check
    // top cannot be greater than bottom
    // left cannot be greater than right
    if (top > bottom) {
        uint16_t temp = top;
        top = bottom;
        bottom = temp;
        qWarning().noquote().nospace() << "calculateRectangleProperties(): top and bottom mixed?";
    }
    if (left > right) {
        uint16_t temp = left;
        left = right;
        right = temp;
        qWarning().noquote().nospace() << "calculateRectangleProperties(): left and right mixed?";
    }

    RectangleProperties properties;
    properties.width = right - left;
    properties.height = bottom - top;
    properties.middleX = static_cast<uint16_t>(left + properties.width / 2);
    properties.middleY = static_cast<uint16_t>(top + properties.height / 2);

    // Sanity check, none cannot be 0
    // If that is the case, we will not turn or zoom
    if (properties.height == 0 || properties.width == 0 || properties.middleX == 0 || properties.middleY == 0) {
        properties.height = CAMERA_RESOLUTION_HEIGHT;
        properties.width = CAMERA_RESOLUTION_WIDTH;
        properties.middleX = CAMERA_RESOLUTION_WIDTH / 2;
        properties.middleY = CAMERA_RESOLUTION_HEIGHT / 2;
        qWarning().noquote().nospace() << "calculateRectangleProperties(): Something was zero -> No zoom, no turn!";
    }

    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) {
                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("extra") == true && commandObject["extra"] == "EXIT") {
                        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"];

                    mIsBusy = true;

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

        QCoreApplication::processEvents();
    }
}