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,6 +163,7 @@ void AiEngineGimbalServerActions::getAnglesToOnScreenTarget(int targetX, int tar
 
 void AiEngineGimbalServerActions::turnToTarget(AiEngineRectangleProperties rectangle)
 {
+    if (mAllowCameraCommands == true) {
         qDebug().noquote().nospace() << "Turning to target";
 
         float resultYaw = 0.0f;
@@ -196,10 +182,12 @@ void AiEngineGimbalServerActions::turnToTarget(AiEngineRectangleProperties recta
 
         mUdpSocket->sendCommand(serialCommandTurn);
     }
+}
 
 
 void AiEngineGimbalServerActions::zoomToTarget(AiEngineRectangleProperties rectangle)
 {
+    if (mAllowCameraCommands == true) {
         qDebug().noquote().nospace() << "Zooming to target";
 
         float fillRatio = 0.5;
@@ -225,6 +213,7 @@ void AiEngineGimbalServerActions::zoomToTarget(AiEngineRectangleProperties recta
 
         mUdpSocket->sendCommand(serialCommandNewZoom);
     }
+}
 
 
 void AiEngineGimbalServerActions::getLocation(AiEngineDronePosition dronePosition, int targetIndex)
@@ -251,6 +240,7 @@ void AiEngineGimbalServerActions::getLocation(AiEngineDronePosition dronePositio
 
 void AiEngineGimbalServerActions::restoreOrientationAndZoom(AiEngineGimbalStatus gimbalStatus)
 {
+    if (mAllowCameraCommands == true) {
         QByteArray tempCommand;
 
         // Go to initial orientation
@@ -271,6 +261,7 @@ void AiEngineGimbalServerActions::restoreOrientationAndZoom(AiEngineGimbalStatus
         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);