autopilot/ai_controller/aienginegimbalserveractions.h

#pragma once

#include <QDebug>
#include "aienginedefinitions.h"
#include "aienginegimbalserverudpcommand.h"
#include "aienginegimbalserverudpresponse.h"
#include "aienginegimbalserverudp.h"


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


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


class AiEngineGimbalServerActions : public QObject
{
    Q_OBJECT
public:
    explicit AiEngineGimbalServerActions(QObject *parent = nullptr);

public slots:
    void setup(AiEngineGimbalServerUDP *udpSocket, AiEngineGimbalServerUDPCommand *udpCommand, AiEngineGimbalServerUDPResponse *udpResponse, AiEngineGimbalStatus *gimbalStatus);
    AiEngineRectangleProperties calculateRectangleProperties(int top, int left, int bottom, int right);
    void turnToTarget(AiEngineRectangleProperties rectangle);
    void zoomToTarget(AiEngineRectangleProperties rectangle);
    void getLocation(AiEngineDronePosition dronePosition, int targetIndex);
    void restoreOrientationAndZoom(AiEngineGimbalStatus gimbalStatus);
    void goToInitialOrientation(void);
    void setAllowCameraCommands(bool allow);

signals:
    void aiTargetZoomed(AiEngineTargetPosition);

private:
    AiEngineGimbalServerUDP *mUdpSocket;
    AiEngineGimbalServerUDPCommand *mUdpCommand;
    AiEngineGimbalServerUDPResponse *mUdpResponse;
    AiEngineGimbalStatus *mGimbalStatus;
    bool mAllowCameraCommands;

private slots:
    CameraData getCameraData(void);
    void getAnglesToOnScreenTarget(int targetX, int targetY, float &resultYaw, float &resultPitch);
    AiEngineGeoPosition calculateTargetLocation(DroneData drone, CameraData camera);
    void calculateDistancesToTarget(float altitude, float cameraPitch, float &slantDistance, float &horizontalDistance);
    float degreesToRadians(float degrees);
};