autopilot/misc/rtsp_ai_player/aienginegimbalserveractions.cpp

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


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

}


void AiEngineGimbalServerActions::setup(AiEngineGimbalServerSerialPort *serialPort, AiEngineGimbalServerSerialCommand *serialCommand, AiEngineGimbalServerSerialResponse *serialResponse, AiEngineGimbalStatus *gimbalStatus)
{
    mSerialPort = serialPort;
    mSerialCommand = serialCommand;
    mSerialResponse = serialResponse;
    mGimbalStatus = gimbalStatus;

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

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

    // Get max zoom value to reduce calls to camera
    tempCommand = mSerialCommand->getCommand(SERIAL_COMMAND_ID::ACQUIRE_MAX_ZOOM_VALUE);
    mSerialPort->sendCommand(tempCommand);
    tempResponse = mSerialPort->readResponse();
    responseValues = mSerialResponse->getResponceValues(tempResponse);
    mGimbalStatus->maxZoom = responseValues["zoom"].toInt();

    // Go to initial orientation
    tempCommand = mSerialCommand->getCommand(SERIAL_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;
    mSerialPort->sendCommand(tempCommand);

    // Go to initial zoom
    tempCommand = mSerialCommand->getCommand(SERIAL_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;
    mSerialPort->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;
}


AiEngineRectangleProperties AiEngineGimbalServerActions::calculateRectangleProperties(int top, int left, int bottom, int right) {
    // 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 = AI_ENGINE_CAMERA_RESOLUTION_HEIGHT;
        properties.width = AI_ENGINE_CAMERA_RESOLUTION_WIDTH;
        properties.middleX = AI_ENGINE_CAMERA_RESOLUTION_WIDTH / 2;
        properties.middleY = AI_ENGINE_CAMERA_RESOLUTION_HEIGHT / 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 = AI_ENGINE_CAMERA_FIELD_OF_VIEW_HORIZONTAL * (1.0f + (mGimbalStatus->currentZoom - 1.0f) / 5.0f);

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

    // Calculate angle offsets based on normalized pixel location and image plane dimensions
    float turnX = atan2(normPixelX * imagePlaneWidth / 2.0f, AI_ENGINE_CAMERA_FOCAL_LENGTH) * 180.0f / M_PI;
    float turnY = atan2(normPixelY * imagePlaneHeight / 2.0f, AI_ENGINE_CAMERA_FOCAL_LENGTH) * 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 = mSerialCommand->getCommand(SERIAL_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;

    mSerialPort->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 = mSerialCommand->getCommand(SERIAL_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;

    mSerialPort->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;
    emit aiTargetZoomed(targetReturn);
}


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

    // Go to initial orientation
    tempCommand = mSerialCommand->getCommand(SERIAL_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;
    mSerialPort->sendCommand(tempCommand);

    // Go to initial zoom
    tempCommand = mSerialCommand->getCommand(SERIAL_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;
    mSerialPort->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
    float zoom = mGimbalStatus->currentZoom;
    float fov = AI_ENGINE_CAMERA_FIELD_OF_VIEW_HORIZONTAL / zoom;

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


// 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);
    qInfo().noquote().nospace() << "horizontalDistance: " << horizontalDistance;
    qInfo().noquote().nospace() << "slantDistance: " << slantDistance;

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