autopilot/misc/camera/a8/utilsTargetLocation.hpp

#pragma once

#include <cstdint>

const double EARTH_RADIUS = 6371000.0; // Earth's radius in meters

struct GPSData
{
    float altitude;  // Meters
    float latitude;  // Decimal degrees
    float longitude; // Decimal degrees
};

struct CameraData
{
    uint16_t height; // Pixels
    uint16_t width;  // Pixels
    float pitch;     // Degrees
    float yaw;       // Degrees
    float fow;       // Degrees
};

struct DroneData
{
    GPSData gps;
    float yaw;   // Degrees
    float pitch; // Degrees
    float roll;  // Degrees
};

class UtilsTargetLocation
{
public:
    //static GPSData getLocation(float altitude, float latitude, float lognitude, float yaw, float pitch, float roll, float targetTrueSize, uint16_t targetPixelSize);
    static GPSData getLocation(float altitude, float latitude, float lognitude, float yaw, float pitch, float roll);
    static void getAnglesToOnScreenTarget(uint16_t targetX, uint16_t targetY, float &resultYaw, float &resultPitch);

private:
    static CameraData getCameraData();
    //static float calculateTargetDistanceFromTargetSize(float targetSize, uint16_t targetPixelSize, uint16_t imageWidth, float fov);
    static void calculateDistancesToTarget(float altitude, float cameraPitch, float &slantDistance, float &horizontalDistance);
    static float degreesToRadians(float degrees);
    static GPSData calculateTargetLocation(DroneData drone, CameraData camera);
};