Set camera ready for lift and drop

Added functionality to set camera ready for bringing it down or up.
Camera will be made available for AI after bringCameraDown command is given via UDPSocket.
Camera will be made unavailable for AI after bringCameraUp command is given via UDPSocket.

ai_controller/aienginegimbalserver.cpp

@@ -11,10 +11,12 @@ AiEngineGimbalServer::AiEngineGimbalServer(QObject *parent)
     mIsAvailable = false;
     qDebug() << "Initial is available: " << mIsAvailable;
 
-    QTimer::singleShot(5000, this, [this]() {
-        mIsAvailable = true;
-        qDebug() << "Initial is available: " << mIsAvailable;
-    });
+    // Commented out, might be required later
+    // Making camera available is currently do in processUdpCommands() with command bringCameraDown
+    //QTimer::singleShot(5000, this, [this]() {
+    //    mIsAvailable = true;
+    //    qDebug() << "Initial is available: " << mIsAvailable;
+    //});
 
     mActions.setup(&mUdpSocket, &mUdpCommand, &mUdpResponse, &mGimbalStatus);
 
@@ -26,6 +28,13 @@ AiEngineGimbalServer::AiEngineGimbalServer(QObject *parent)
     mDronePosition.yaw = 90.0;
 
     connect(&mActions, &AiEngineGimbalServerActions::aiTargetZoomed, this, &AiEngineGimbalServer::aiTargetZoomed);
+
+    // Create and bind the new UDP socket for receiving commands
+    mReceiveUdpSocket = new QUdpSocket(this);
+    mReceiveUdpSocket->bind(QHostAddress::LocalHost, 45454);
+    
+    // Connect the socket to handle incoming messages
+    connect(mReceiveUdpSocket, &QUdpSocket::readyRead, this, &AiEngineGimbalServer::processUdpCommands);
 }
 
 
@@ -101,3 +110,51 @@ void AiEngineGimbalServer::cameraPositionSlot(AiEngineCameraPosition position)
 {
     qDebug() << "AiEngineGimbalServer::cameraPositionSlot() Move camera to:" << position.pitch <<  position.yaw << "zoom:" << position.zoom;
 }
+
+
+void AiEngineGimbalServer::processUdpCommands(void) {
+    while (mReceiveUdpSocket->hasPendingDatagrams()) {
+        QByteArray datagram;
+        datagram.resize(mReceiveUdpSocket->pendingDatagramSize());
+        mReceiveUdpSocket->readDatagram(datagram.data(), datagram.size());
+
+        QString command = QString::fromUtf8(datagram);
+        qDebug() << "Received command:" << command;
+
+        if (command == "bringCameraDown") {
+            mActions.setAllowCameraCommands(true); // This will allow other camera commands
+            mActions.goToInitialOrientation();
+
+            // 10 second delay to let camera get ready for AI
+            QTimer::singleShot(10000, this, [this]() {
+                mIsAvailable = true;
+                qDebug() << "Camera set available and to initial position";
+            });
+        }
+        else if (command == "bringCameraUp") {
+            // No delay, set camera unavailable
+            mActions.setAllowCameraCommands(false); // This will prevent any ongoing commands
+            mIsAvailable = false;
+            mActions.goToInitialOrientation();
+            qDebug() << "Camera set unavailable and to initial position";
+        }
+    }
+
+    /*
+    // How to call this:
+
+    #include <QUdpSocket>
+
+    void sendCommand(const QString &command) {
+        QUdpSocket udpSocket;
+        QByteArray datagram = command.toUtf8();
+        udpSocket.writeDatagram(datagram, QHostAddress::LocalHost, 45454);
+    }
+
+    int main() {
+        sendCommand("bringCameraDown");
+        sendCommand("bringCameraUp");
+        return 0;
+    }
+    */
+}

ai_controller/aienginegimbalserver.h

@@ -2,6 +2,7 @@
 
 #include <QObject>
 #include <QMap>
+#include <QUdpSocket>
 #include "aienginedefinitions.h"
 #include "aienginegimbalserveractions.h"
 #include "aienginegimbalserverudpcommand.h"
@@ -34,4 +35,8 @@ private:
     AiEngineGimbalServerUDPResponse mUdpResponse;
     AiEngineGimbalServerActions mActions;
     bool mIsAvailable;
+    QUdpSocket mReceiveUdpSocket; // UDP socket for receiving commands
+
+private slots:
+    void processPendingDatagrams(void); // Handles incoming UDP messages
 };

ai_controller/aienginegimbalserveractions.cpp

@@ -17,6 +17,7 @@ void AiEngineGimbalServerActions::setup(AiEngineGimbalServerUDP *udpSocket, AiEn
     mUdpCommand = udpCommand;
     mUdpResponse = udpResponse;
     mGimbalStatus = gimbalStatus;
+    mAllowCameraCommands = false;
 
     // Set initial position and update status
     QByteArray tempCommand;
@@ -68,24 +69,8 @@ void AiEngineGimbalServerActions::setup(AiEngineGimbalServerUDP *udpSocket, AiEn
     float maxZoom = responseValues["zoom"].toFloat();
     mGimbalStatus->maxZoom = maxZoom > 10 ? 10 : maxZoom;
 
-    // Go to initial orientation
-    tempCommand = mUdpCommand->getCommand(UDP_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;
-    mUdpSocket->sendCommand(tempCommand);
-
-    // Go to initial zoom
-    tempCommand = mUdpCommand->getCommand(UDP_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;
-    mUdpSocket->sendCommand(tempCommand);
+    // Go to initial orientation and zoom
+    goToInitialOrientation();
 
     mGimbalStatus->currentPitch = AI_ENGINE_GIMBAL_INITIAL_PITCH;
     mGimbalStatus->currentRoll = AI_ENGINE_GIMBAL_INITIAL_ROLL;
@@ -178,52 +163,56 @@ void AiEngineGimbalServerActions::getAnglesToOnScreenTarget(int targetX, int tar
 
 void AiEngineGimbalServerActions::turnToTarget(AiEngineRectangleProperties rectangle)
 {
-    qDebug().noquote().nospace() << "Turning to target";
+    if (mAllowCameraCommands == true) {
+        qDebug().noquote().nospace() << "Turning to target";
 
-    float resultYaw = 0.0f;
-    float resultPitch = 0.0f;
-    getAnglesToOnScreenTarget(rectangle.middleX, rectangle.middleY, resultYaw, resultPitch);
+        float resultYaw = 0.0f;
+        float resultPitch = 0.0f;
+        getAnglesToOnScreenTarget(rectangle.middleX, rectangle.middleY, resultYaw, resultPitch);
 
-    QByteArray serialCommandTurn = mUdpCommand->getCommand(UDP_COMMAND_ID::TURN_TO_PIXEL);
+        QByteArray serialCommandTurn = mUdpCommand->getCommand(UDP_COMMAND_ID::TURN_TO_PIXEL);
 
-    int16_t degreesVal = resultYaw * 10;
-    serialCommandTurn[8] = degreesVal & 0xFF;
-    serialCommandTurn[9] = degreesVal >> 8;
+        int16_t degreesVal = resultYaw * 10;
+        serialCommandTurn[8] = degreesVal & 0xFF;
+        serialCommandTurn[9] = degreesVal >> 8;
 
-    degreesVal = resultPitch * 10;
-    serialCommandTurn[10] = degreesVal & 0xFF;
-    serialCommandTurn[11] = degreesVal >> 8;
+        degreesVal = resultPitch * 10;
+        serialCommandTurn[10] = degreesVal & 0xFF;
+        serialCommandTurn[11] = degreesVal >> 8;
 
-    mUdpSocket->sendCommand(serialCommandTurn);
+        mUdpSocket->sendCommand(serialCommandTurn);
+    }
 }
 
 
 void AiEngineGimbalServerActions::zoomToTarget(AiEngineRectangleProperties rectangle)
 {
-    qDebug().noquote().nospace() << "Zooming to target";
+    if (mAllowCameraCommands == true) {
+        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;
+        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 = mUdpCommand->getCommand(UDP_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;
+
+        mUdpSocket->sendCommand(serialCommandNewZoom);
     }
-
-    QByteArray serialCommandNewZoom = mUdpCommand->getCommand(UDP_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;
-
-    mUdpSocket->sendCommand(serialCommandNewZoom);
 }
 
 
@@ -251,26 +240,28 @@ void AiEngineGimbalServerActions::getLocation(AiEngineDronePosition dronePositio
 
 void AiEngineGimbalServerActions::restoreOrientationAndZoom(AiEngineGimbalStatus gimbalStatus)
 {
-    QByteArray tempCommand;
+    if (mAllowCameraCommands == true) {
+        QByteArray tempCommand;
 
-    // Go to initial orientation
-    tempCommand = mUdpCommand->getCommand(UDP_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;
-    mUdpSocket->sendCommand(tempCommand);
+        // Go to initial orientation
+        tempCommand = mUdpCommand->getCommand(UDP_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;
+        mUdpSocket->sendCommand(tempCommand);
 
-    // Go to initial zoom
-    tempCommand = mUdpCommand->getCommand(UDP_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;
-    mUdpSocket->sendCommand(tempCommand);
+        // Go to initial zoom
+        tempCommand = mUdpCommand->getCommand(UDP_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;
+        mUdpSocket->sendCommand(tempCommand);
+    }
 
     // TODO: Maybe send signal that all done?
 }
@@ -354,3 +345,34 @@ float AiEngineGimbalServerActions::degreesToRadians(float degrees)
 {
     return degrees * M_PI / 180.0f;
 }
+
+
+void AiEngineGimbalServerActions::goToInitialOrientation(void)
+{
+    // These camera commands should always be allowed
+
+    // Go to initial orientation
+    tempCommand = mUdpCommand->getCommand(UDP_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;
+    mUdpSocket->sendCommand(tempCommand);
+
+    // Go to initial zoom
+    tempCommand = mUdpCommand->getCommand(UDP_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;
+    mUdpSocket->sendCommand(tempCommand);
+}
+
+
+void AiEngineGimbalServerActions::setAllowCameraCommands(bool allow)
+{
+    mAllowCameraCommands = allow;
+}

ai_controller/aienginegimbalserveractions.h

@@ -49,6 +49,8 @@ public slots:
     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);
@@ -58,6 +60,7 @@ private:
     AiEngineGimbalServerUDPCommand *mUdpCommand;
     AiEngineGimbalServerUDPResponse *mUdpResponse;
     AiEngineGimbalStatus *mGimbalStatus;
+    bool mAllowCameraCommands;
 
 private slots:
     CameraData getCameraData(void);