autopilot/ai_controller/aienginegimbalserveractions.cpp

#include <QUdpSocket>
#include <QVariant>
#include "aienginegimbalserveractions.h"
#include "aienginegimbalserverudp.h"


AiEngineGimbalServerActions::AiEngineGimbalServerActions(QObject *parent)
    : QObject{parent}
{

}


void AiEngineGimbalServerActions::setup(AiEngineGimbalServerUDP *udpSocket, AiEngineGimbalServerUDPCommand *udpCommand, AiEngineGimbalServerUDPResponse *udpResponse, AiEngineGimbalStatus *gimbalStatus)
{
    mUdpSocket = udpSocket;
    mUdpCommand = udpCommand;
    mUdpResponse = udpResponse;
    mGimbalStatus = gimbalStatus;

    // Set initial position and update status
    QByteArray tempCommand;
    QByteArray tempResponse;
    QHash<QString, QVariant> responseValues;

    // Get camera ID
    tempCommand = mUdpCommand->getCommand(UDP_COMMAND_ID::ACQUIRE_HW_INFO);
    mUdpSocket->sendCommand(tempCommand);
    tempResponse = mUdpSocket->readResponse();
    responseValues = mUdpResponse->getResponceValues(tempResponse);
    mGimbalStatus->hardwareID = responseValues["hardware_id"].toInt();

    switch (mGimbalStatus->hardwareID) {
    case AI_ENGINE_CAMERA_ZR10_HID:
        mGimbalStatus->aiEngineCameraFOVHMin = AI_ENGINE_CAMERA_ZR10_FIELD_OF_VIEW_HORIZONTAL_MIN;
        mGimbalStatus->aiEngineCameraFOVHMax = AI_ENGINE_CAMERA_ZR10_FIELD_OF_VIEW_HORIZONTAL_MAX;
        mGimbalStatus->aiEngineCameraFOVVMin = AI_ENGINE_CAMERA_ZR10_FIELD_OF_VIEW_VERTICAL_MIN;
        mGimbalStatus->aiEngineCameraFOVVMax = AI_ENGINE_CAMERA_ZR10_FIELD_OF_VIEW_VERTICAL_MAX;
        mGimbalStatus->aiEngineCameraFLMin = AI_ENGINE_CAMERA_ZR10_FOCAL_LENGTH_MIN;
        mGimbalStatus->aiEngineCameraFLMax = AI_ENGINE_CAMERA_ZR10_FOCAL_LENGTH_MAX;
        break;
    case AI_ENGINE_CAMERA_A8_HID:
        mGimbalStatus->aiEngineCameraFOVHMin = AI_ENGINE_CAMERA_A8_FIELD_OF_VIEW_HORIZONTAL_MIN;
        mGimbalStatus->aiEngineCameraFOVHMax = AI_ENGINE_CAMERA_A8_FIELD_OF_VIEW_HORIZONTAL_MAX;
        mGimbalStatus->aiEngineCameraFOVVMin = AI_ENGINE_CAMERA_A8_FIELD_OF_VIEW_VERTICAL_MIN;
        mGimbalStatus->aiEngineCameraFOVVMax = AI_ENGINE_CAMERA_A8_FIELD_OF_VIEW_VERTICAL_MAX;
        mGimbalStatus->aiEngineCameraFLMin = AI_ENGINE_CAMERA_A8_FOCAL_LENGTH_MIN;
        mGimbalStatus->aiEngineCameraFLMax = AI_ENGINE_CAMERA_A8_FOCAL_LENGTH_MAX;
        break;
    default:
        qDebug().noquote().nospace() << "ERROR: Unknown HardwareID " << mGimbalStatus->hardwareID;
        break;
    }

    // Get resolution to reduce calls to camera
    tempCommand = mUdpCommand->getCommand(UDP_COMMAND_ID::ACQUIRE_CAMERA_CODEC_SPECS);
    mUdpSocket->sendCommand(tempCommand);
    tempResponse = mUdpSocket->readResponse();
    responseValues = mUdpResponse->getResponceValues(tempResponse);
    mGimbalStatus->resolutionX = responseValues["width"].toInt();
    mGimbalStatus->resolutionY = responseValues["height"].toInt();

    // Get max zoom value to reduce calls to camera
    tempCommand = mUdpCommand->getCommand(UDP_COMMAND_ID::ACQUIRE_MAX_ZOOM_VALUE);
    mUdpSocket->sendCommand(tempCommand);
    tempResponse = mUdpSocket->readResponse();
    responseValues = mUdpResponse->getResponceValues(tempResponse);
    float maxZoom = responseValues["zoom"].toFloat();
    mGimbalStatus->maxZoom = maxZoom > 10 ? 10 : maxZoom;

    // Go to initial orientation
    tempCommand = mUdpCommand->getCommand(UDP_COMMAND_ID::TURN_TO_DEGREES);
    int16_t degreesVal = AI_ENGINE_GIMBAL_INITIAL_YAW * 10;
    tempCommand[8] = degreesVal & 0xFF;
    tempCommand[9] = degreesVal >> 8;
    degreesVal = AI_ENGINE_GIMBAL_INITIAL_PITCH * 10;
    tempCommand[10] = degreesVal & 0xFF;
    tempCommand[11] = degreesVal >> 8;
    mUdpSocket->sendCommand(tempCommand);

    // Go to initial zoom
    tempCommand = mUdpCommand->getCommand(UDP_COMMAND_ID::ZOOM_TO_X);
    uint8_t integerPart = static_cast<uint8_t>(AI_ENGINE_CAMERA_INITIAL_ZOOM);
    float fractionalPart = AI_ENGINE_CAMERA_INITIAL_ZOOM - integerPart;
    uint8_t scaledFractional = uint8_t(fractionalPart * 10);
    tempCommand[8] = integerPart;
    tempCommand[9] = scaledFractional;
    mUdpSocket->sendCommand(tempCommand);

    mGimbalStatus->currentPitch = AI_ENGINE_GIMBAL_INITIAL_PITCH;
    mGimbalStatus->currentRoll = AI_ENGINE_GIMBAL_INITIAL_ROLL;
    mGimbalStatus->currentYaw = AI_ENGINE_GIMBAL_INITIAL_YAW;
    mGimbalStatus->currentZoom = AI_ENGINE_CAMERA_INITIAL_ZOOM;

    qDebug().noquote().nospace() << "mGimbalStatus->hardwareID:   " << mGimbalStatus->hardwareID;
    qDebug().noquote().nospace() << "mGimbalStatus->maxZoom:      " << mGimbalStatus->maxZoom;
    qDebug().noquote().nospace() << "mGimbalStatus->currentZoom:  " << mGimbalStatus->currentZoom;
    qDebug().noquote().nospace() << "mGimbalStatus->currentPitch: " << mGimbalStatus->currentPitch;
    qDebug().noquote().nospace() << "mGimbalStatus->currentYaw:   " << mGimbalStatus->currentYaw;
    qDebug().noquote().nospace() << "mGimbalStatus->currentRoll:  " << mGimbalStatus->currentRoll;
    qDebug().noquote().nospace() << "mGimbalStatus->resolutionX:  " << mGimbalStatus->resolutionX;
    qDebug().noquote().nospace() << "mGimbalStatus->resolutionY:  " << mGimbalStatus->resolutionY;
}


AiEngineRectangleProperties AiEngineGimbalServerActions::calculateRectangleProperties(int top, int left, int bottom, int right) {
    // AI resolution is 640 * 360, which is half of 1280 * 720
    // Multiply by 2 to get appropriate position on video
    top = top * 2;
    left = left * 2;
    bottom = bottom * 2;
    right = right * 2;

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

    AiEngineRectangleProperties properties;
    properties.width = right - left;
    properties.height = bottom - top;
    properties.middleX = static_cast<int>(left + properties.width / 2);
    properties.middleY = static_cast<int>(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 = mGimbalStatus->resolutionY;
        properties.width = mGimbalStatus->resolutionX;
        properties.middleX = mGimbalStatus->resolutionX / 2;
        properties.middleY = mGimbalStatus->resolutionY / 2;
        qWarning().noquote().nospace() << "calculateRectangleProperties(): Something was zero -> No zoom, no turn!";
    }

    return properties;
}


void AiEngineGimbalServerActions::getAnglesToOnScreenTarget(int targetX, int targetY, float &resultYaw, float &resultPitch)
{
    // Get current yaw and pitch
    resultYaw = mGimbalStatus->currentYaw;
    resultPitch = mGimbalStatus->currentPitch;

    // Normalize target pixel location to [-0.5, 0.5] range
    float normPixelX = (targetX - mGimbalStatus->resolutionX / 2.0f) / (mGimbalStatus->resolutionX / 2.0f);
    float normPixelY = (targetY - mGimbalStatus->resolutionY / 2.0f) / (mGimbalStatus->resolutionY / 2.0f);

    // Adjust horizontal field of view for zoom
    float horizontalFov = mGimbalStatus->aiEngineCameraFOVHMin + (10 - mGimbalStatus->currentZoom) * (mGimbalStatus->aiEngineCameraFOVHMax - mGimbalStatus->aiEngineCameraFOVHMin) / 9;
    float verticalFov = mGimbalStatus->aiEngineCameraFOVVMin + (10 - mGimbalStatus->currentZoom) * (mGimbalStatus->aiEngineCameraFOVVMax - mGimbalStatus->aiEngineCameraFOVVMin) / 9;
    float focalLength = mGimbalStatus->aiEngineCameraFLMin + (mGimbalStatus->currentZoom - 1) * (mGimbalStatus->aiEngineCameraFLMax - mGimbalStatus->aiEngineCameraFLMin) / 9;

    // Calculate image plane dimensions based on focal length and aspect ratio
    float imagePlaneWidth = 2.0f * focalLength * tan(degreesToRadians(horizontalFov) / 2.0f);
    float imagePlaneHeight = 2.0f * focalLength * tan(degreesToRadians(verticalFov) / 2.0f);

    // Calculate angle offsets based on normalized pixel location and image plane dimensions
    float turnX = atan2(normPixelX * imagePlaneWidth / 2.0f, mGimbalStatus->aiEngineCameraFLMin) * 180.0f / M_PI;
    float turnY = atan2(normPixelY * imagePlaneHeight / 2.0f, mGimbalStatus->aiEngineCameraFLMin) * 180.0f / M_PI;

    // Make alterations to current angles
    resultYaw -= turnX;
    resultPitch -= turnY;
}


void AiEngineGimbalServerActions::turnToTarget(AiEngineRectangleProperties rectangle)
{
    qDebug().noquote().nospace() << "Turning to target";

    float resultYaw = 0.0f;
    float resultPitch = 0.0f;
    getAnglesToOnScreenTarget(rectangle.middleX, rectangle.middleY, resultYaw, resultPitch);

    QByteArray serialCommandTurn = mUdpCommand->getCommand(UDP_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;

    mUdpSocket->sendCommand(serialCommandTurn);
}


void AiEngineGimbalServerActions::zoomToTarget(AiEngineRectangleProperties rectangle)
{
    qDebug().noquote().nospace() << "Zooming to target";

    float fillRatio = 0.5;
    float targetPixelWidth = rectangle.width;
    float targetPixelHeight = rectangle.height;
    float adjustedObjectWidth = targetPixelWidth / fillRatio;
    float adjustedObjectHeight = targetPixelHeight / fillRatio;
    float zoomWidth = static_cast<double>(mGimbalStatus->resolutionX) / adjustedObjectWidth;
    float zoomHeight = static_cast<double>(mGimbalStatus->resolutionY) / adjustedObjectHeight;
    float zoom = std::min(zoomWidth, zoomHeight);
    if (zoom < 1.0f) {
        zoom = 1.0f;
    }

    QByteArray serialCommandNewZoom = mUdpCommand->getCommand(UDP_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;

    mUdpSocket->sendCommand(serialCommandNewZoom);
}


void AiEngineGimbalServerActions::getLocation(AiEngineDronePosition dronePosition, int targetIndex)
{
    // From the drone and camera
    CameraData cameraData = getCameraData();
    GPSData gpsData = {dronePosition.position.alt, dronePosition.position.lat, dronePosition.position.lon};
    DroneData droneData = {gpsData, dronePosition.yaw, dronePosition.pitch, 0.0f /* Roll */}; // GPS (latitude, longitude, altitude) and heading

    // Calculate the GPS location of the target
    AiEngineGeoPosition targetPosition = calculateTargetLocation(droneData, cameraData);

    // TODO: Send position
    AiEngineTargetPosition targetReturn;
    targetReturn.position = targetPosition;
    targetReturn.targetIndex = targetIndex;
    qDebug() << "targetReturn.targetIndex:  " << targetReturn.targetIndex;
    qDebug() << "targetReturn.position.alt: " << targetReturn.position.alt;
    qDebug() << "targetReturn.position.lat: " << targetReturn.position.lat;
    qDebug() << "targetReturn.position.lon: " << targetReturn.position.lon;
    emit aiTargetZoomed(targetReturn);
}


void AiEngineGimbalServerActions::restoreOrientationAndZoom(AiEngineGimbalStatus gimbalStatus)
{
    QByteArray tempCommand;

    // Go to initial orientation
    tempCommand = mUdpCommand->getCommand(UDP_COMMAND_ID::TURN_TO_DEGREES);
    int16_t degreesVal = gimbalStatus.currentYaw * 10;
    tempCommand[8] = degreesVal & 0xFF;
    tempCommand[9] = degreesVal >> 8;
    degreesVal = gimbalStatus.currentPitch * 10;
    tempCommand[10] = degreesVal & 0xFF;
    tempCommand[11] = degreesVal >> 8;
    mUdpSocket->sendCommand(tempCommand);

    // Go to initial zoom
    tempCommand = mUdpCommand->getCommand(UDP_COMMAND_ID::ZOOM_TO_X);
    uint8_t integerPart = static_cast<uint8_t>(gimbalStatus.currentZoom);
    float fractionalPart = gimbalStatus.currentZoom - integerPart;
    uint8_t scaledFractional = uint8_t(fractionalPart * 10);
    tempCommand[8] = integerPart;
    tempCommand[9] = scaledFractional;
    mUdpSocket->sendCommand(tempCommand);

    // TODO: Maybe send signal that all done?
}


CameraData AiEngineGimbalServerActions::getCameraData()
{
    uint16_t height = mGimbalStatus->resolutionY;
    uint16_t width = mGimbalStatus->resolutionX;
    float yaw = 0 - mGimbalStatus->currentYaw; // Reverse value for calculation purposes
    float pitch = 0 - mGimbalStatus->currentPitch; // Reverse value for calculation purposes

    return {height, width, pitch, yaw};
}


// Function to calculate the new GPS location
AiEngineGeoPosition AiEngineGimbalServerActions::calculateTargetLocation(DroneData drone, CameraData camera)
{
    // Calculate altitude and distance to the target
    //float targetDistance = calculateTargetDistanceFromTargetSize(targetTrueSize, targetPixelSize, cameraData.width, degreesToRadians(cameraData.fow));
    float slantDistance = 0;
    float horizontalDistance = 0;
    calculateDistancesToTarget(drone.gps.altitude, camera.pitch, slantDistance, horizontalDistance);

    // Calculate new altitude using the slant distance and angle
    float pitchRad = degreesToRadians(camera.pitch);
    float sinPitchRad = std::sin(pitchRad);
    float altitudeDifference = std::round(slantDistance * sinPitchRad * 100.0) / 100.0; // Rounding to avoid weird targetAltitude
    float targetAltitude = (std::round(drone.gps.altitude * 100.0) / 100.0) - altitudeDifference; // Rounding to avoid weird targetAltitude

    // Calculate the bearing from the drone orientation and camera orientation
    float targetBearing = std::fmod(drone.yaw + camera.yaw, 360.0f);

    // Convert bearing and drone's latitude/longitude to radians
    float bearingRad = degreesToRadians(targetBearing);
    float latRad = degreesToRadians(drone.gps.latitude);
    float lonRad = degreesToRadians(drone.gps.longitude);

    // Calculate new latitude using Haversine formula
    float newLatRad = std::asin(std::sin(latRad) * std::cos(horizontalDistance / EARTH_RADIUS) + std::cos(latRad) * std::sin(horizontalDistance / EARTH_RADIUS) * std::cos(bearingRad));

    // Calculate new longitude using Haversine formula
    float newLonRad = lonRad + std::atan2(std::sin(bearingRad) * std::sin(horizontalDistance / EARTH_RADIUS) * std::cos(latRad), std::cos(horizontalDistance / EARTH_RADIUS) - std::sin(latRad) * std::sin(newLatRad));

    // Convert back to degrees for latitude and longitude
    AiEngineGeoPosition newLocation;
    newLocation.alt = targetAltitude;
    newLocation.lat = newLatRad * 180.0f / static_cast<float>(M_PI);
    newLocation.lon = newLonRad * 180.0f / static_cast<float>(M_PI);

    return newLocation;
}


void AiEngineGimbalServerActions::calculateDistancesToTarget(float altitude, float cameraPitch, float &slantDistance, float &horizontalDistance)
{
    // Convert pitch angle from degrees to radians
    float cameraPitchRadians = degreesToRadians(cameraPitch);

    // Value has to be between 0-90
    cameraPitchRadians = std::clamp((double)cameraPitchRadians, (double)0.0f, M_PI_2);

    // Calculate horizontal distance
    horizontalDistance = altitude / tan(cameraPitchRadians);

    // Adjust for Earth's curvature:
    // We need to find the horizontal distance on the curved Earth surface that corresponds to this flat distance
    double centralAngle = horizontalDistance / EARTH_RADIUS; // in radians

    // Calculate the arc length on Earth's surface
    horizontalDistance = EARTH_RADIUS * centralAngle;

    // Calculate slant distance considering the Earth's curvature
    slantDistance = sqrt(altitude * altitude + horizontalDistance * horizontalDistance);
}


// Function to convert degrees to radians
float AiEngineGimbalServerActions::degreesToRadians(float degrees)
{
    return degrees * M_PI / 180.0f;
}