diff --git a/ai_controller/aienginegimbalserver.cpp b/ai_controller/aienginegimbalserver.cpp index 8392604..6760770 100644 --- a/ai_controller/aienginegimbalserver.cpp +++ b/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 + + 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; + } + */ +} diff --git a/ai_controller/aienginegimbalserver.h b/ai_controller/aienginegimbalserver.h index f17226f..171eec6 100644 --- a/ai_controller/aienginegimbalserver.h +++ b/ai_controller/aienginegimbalserver.h @@ -2,6 +2,7 @@ #include #include +#include #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 }; diff --git a/ai_controller/aienginegimbalserveractions.cpp b/ai_controller/aienginegimbalserveractions.cpp index 64eb482..5d814f8 100644 --- a/ai_controller/aienginegimbalserveractions.cpp +++ b/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(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(mGimbalStatus->resolutionX) / adjustedObjectWidth; - float zoomHeight = static_cast(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(mGimbalStatus->resolutionX) / adjustedObjectWidth; + float zoomHeight = static_cast(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(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(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(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(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(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; +} diff --git a/ai_controller/aienginegimbalserveractions.h b/ai_controller/aienginegimbalserveractions.h index c5a350c..377d5e0 100644 --- a/ai_controller/aienginegimbalserveractions.h +++ b/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);