autopilot/misc/camera/a8/utilsTargetLocation.cpp

#include "utilsTargetLocation.hpp"
#include <QDebug>
#include <QImage>
#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)
{
    // From the drone and camera
    CameraData cameraData = getCameraData();
    GPSData gpsData = {latitude, lognitude, altitude};
    DroneData droneData = {gpsData, yaw, pitch, roll}; // GPS (latitude, longitude, altitude) and heading

    // Calculate altitude and distance to the target
    float targetDistance = calculateTargetDistance(targetTrueSize, targetPixelSize, cameraData.width, degreesToRadians(cameraData.fow));

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

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

CameraData UtilsTargetLocation::getCameraData()
{
    uint16_t height = Config::getResolutionHeight();
    uint16_t width = Config::getResolutionWidth();
    float yaw = Config::getCurrentYaw();
    float pitch = Config::getCurrentPitch();
    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::calculateTargetDistance(float targetTrueSize, uint16_t targetPixelSize, uint16_t imageWidth, float fov)
{
    float focalLength = (imageWidth / 2) / tan(fov / 2);
    return (targetTrueSize * focalLength) / targetPixelSize;
}

// 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, float distance, float bearing)
{
    constexpr float R = 6371000.0f; // Earth radius in meters

    float bearingRad = degreesToRadians(bearing);
    float latRad = degreesToRadians(drone.gps.latitude);
    float lonRad = degreesToRadians(drone.gps.longitude);

    float newLatRad = asin(sin(latRad) * cos(distance / R) + cos(latRad) * sin(distance / R) * cos(bearingRad));
    float newLonRad = lonRad + atan2(sin(bearingRad) * sin(distance / R) * cos(latRad), cos(distance / R) - sin(latRad) * sin(newLatRad));

    float angleRad = camera.pitch * M_PI / 180.0f;
    float horizontalDistance = distance * cos(angleRad);
    float newAltitude = drone.gps.altitude + (horizontalDistance * tan(angleRad));

    GPSData newLocation;
    newLocation.latitude = newLatRad * 180.0f / M_PI;
    newLocation.longitude = newLonRad * 180.0f / M_PI;
    newLocation.altitude = newAltitude;

    return newLocation;
}

void UtilsTargetLocation::getAnglesToOnScreenTarget(uint16_t targetX, uint16_t targetY, float &resultYaw, float &resultPitch)
{
    resultYaw = Config::getCurrentYaw();
    resultPitch = Config::getCurrentPitch();

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

    // Adjust horizontal field of view for zoom
    double 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
    double imagePlaneWidth = 2.0f * CAMERA_FOCAL_LENGTH * tan(horizontalFov / 2.0f * M_PI / 180.0f);
    double 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;

    resultYaw -= turnX;
    resultPitch -= turnY;
}