From deb607237ed1b0e69b1131209782b9e4667a81b2 Mon Sep 17 00:00:00 2001 From: Nffj84 Date: Mon, 24 Mar 2025 18:01:47 +0200 Subject: [PATCH] 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 | 65 ++++++- ai_controller/aienginegimbalserver.h | 5 + ai_controller/aienginegimbalserveractions.cpp | 162 ++++++++++-------- ai_controller/aienginegimbalserveractions.h | 3 + 4 files changed, 161 insertions(+), 74 deletions(-) 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);