autopilot/misc/camera/a8/utilsTargetLocation.cpp

#include <QDebug>
#include <QImage>
#include "utilsTargetLocation.hpp"
#include "config.hpp"
#include "defines.hpp"

//GPSData UtilsTargetLocation::getLocation(float altitude, float latitude, float lognitude, float yaw, float pitch, float roll, float targetTrueSize, uint16_t targetPixelSize)
GPSData UtilsTargetLocation::getLocation(float altitude, float latitude, float lognitude, float yaw, float pitch, float roll)
{
    // From the drone and camera
    CameraData cameraData = getCameraData();
    GPSData gpsData = {altitude, latitude, lognitude};
    DroneData droneData = {gpsData, yaw, pitch, roll}; // GPS (latitude, longitude, altitude) and heading

    // Calculate the GPS location of the target
    return calculateTargetLocation(droneData, cameraData);
}

CameraData UtilsTargetLocation::getCameraData()
{
    uint16_t height = Config::getResolutionHeight();
    uint16_t width = Config::getResolutionWidth();
    float yaw = 0 - Config::getCurrentYaw(); // Reverse value for calculation purposes
    float pitch = 0 - Config::getCurrentPitch(); // Reverse value for calculation purposes
    float zoom = Config::getCurrentZoom();
    float fov = CAMERA_FIELD_OF_VIEW_HORIZONTAL / zoom;

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

// Function to calculate distance from pixel size and target size
/*
float UtilsTargetLocation::calculateTargetDistanceFromTargetSize(float targetTrueSize, uint16_t targetPixelSize, uint16_t imageWidth, float fov)
{
    float focalLength = (imageWidth / 2) / tan(fov / 2);
    return (targetTrueSize * focalLength) / targetPixelSize;
}
*/

void UtilsTargetLocation::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 UtilsTargetLocation::degreesToRadians(float degrees)
{
    return degrees * M_PI / 180.0f;
}

// Function to calculate the new GPS location
GPSData UtilsTargetLocation::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
    GPSData newLocation;
    newLocation.altitude = targetAltitude;
    newLocation.latitude = newLatRad * 180.0f / static_cast<float>(M_PI);
    newLocation.longitude = newLonRad * 180.0f / static_cast<float>(M_PI);

    return newLocation;
}

void UtilsTargetLocation::getAnglesToOnScreenTarget(uint16_t targetX, uint16_t targetY, float &resultYaw, float &resultPitch)
{
    // Get current yaw and pitch
    resultYaw = Config::getCurrentYaw();
    resultPitch = Config::getCurrentPitch();

    // Normalize target pixel location to [-0.5, 0.5] range
    float normPixelX = (targetX - Config::getResolutionWidth() / 2.0f) / (Config::getResolutionWidth() / 2.0f);
    float normPixelY = (targetY - Config::getResolutionHeight() / 2.0f) / (Config::getResolutionHeight() / 2.0f);

    // Adjust horizontal field of view for zoom
    float horizontalFov = CAMERA_FIELD_OF_VIEW_HORIZONTAL * (1.0f + (Config::getCurrentZoom() - 1.0f) / 5.0f);

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

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

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