diff --git a/misc/rtsp_ai_player/aiengineconfig.h b/misc/rtsp_ai_player/aiengineconfig.h index d724696..6cc4333 100644 --- a/misc/rtsp_ai_player/aiengineconfig.h +++ b/misc/rtsp_ai_player/aiengineconfig.h @@ -3,7 +3,7 @@ #include #ifdef OPI5_BUILD -QString rtspVideoUrl = "rtsp://192.168.168.91:8554/live.stream"; +QString rtspVideoUrl = "rtsp://192.168.0.25:8554/main.264"; #else QString rtspVideoUrl = "rtsp://localhost:8554/live.stream"; #endif diff --git a/misc/rtsp_ai_player/aienginedefinitions.h b/misc/rtsp_ai_player/aienginedefinitions.h index 2f6e0e2..460c1b7 100644 --- a/misc/rtsp_ai_player/aienginedefinitions.h +++ b/misc/rtsp_ai_player/aienginedefinitions.h @@ -68,7 +68,14 @@ typedef struct { // Get these only once uint resolutionX; uint resolutionY; + uint hardwareID; float maxZoom; + float aiEngineCameraFOVHMin; + float aiEngineCameraFOVHMax; + float aiEngineCameraFOVVMin; + float aiEngineCameraFOVVMax; + float aiEngineCameraFLMin; + float aiEngineCameraFLMax; // Update these before every command float currentYaw; @@ -80,7 +87,7 @@ typedef struct { struct AiEngineServerSerialCommandStructure { - SERIAL_COMMAND_ID id; + UDP_COMMAND_ID id; QByteArray command; QString description; }; diff --git a/misc/rtsp_ai_player/aienginegimbalserver.cpp b/misc/rtsp_ai_player/aienginegimbalserver.cpp index 44b03a7..8392604 100644 --- a/misc/rtsp_ai_player/aienginegimbalserver.cpp +++ b/misc/rtsp_ai_player/aienginegimbalserver.cpp @@ -8,8 +8,22 @@ AiEngineGimbalServer::AiEngineGimbalServer(QObject *parent) : QObject{parent} { // TODO!! Setup and use serial port.... - mIsAvailable = true; - mActions.setup(&mSerialPort, &mSerialCommand, &mSerialResponse, &mGimbalStatus); + mIsAvailable = false; + qDebug() << "Initial is available: " << mIsAvailable; + + QTimer::singleShot(5000, this, [this]() { + mIsAvailable = true; + qDebug() << "Initial is available: " << mIsAvailable; + }); + + mActions.setup(&mUdpSocket, &mUdpCommand, &mUdpResponse, &mGimbalStatus); + + // TODO: Remove after testing + mDronePosition.position.alt = 1000; + mDronePosition.position.lat = 49.8397; + mDronePosition.position.lon = 24.0319; + mDronePosition.pitch = 0.0; + mDronePosition.yaw = 90.0; connect(&mActions, &AiEngineGimbalServerActions::aiTargetZoomed, this, &AiEngineGimbalServer::aiTargetZoomed); } @@ -32,8 +46,10 @@ void AiEngineGimbalServer::dronePositionSlot(AiEngineDronePosition dronePosition // This is actually called from the client. void AiEngineGimbalServer::zoomToAiTargetSlot(AiEngineCameraTarget target) { + qDebug() << "zoomToAiTargetSlot called"; if (mIsAvailable == true) { mIsAvailable = false; + qDebug() << "Is available: " << mIsAvailable; qDebug() << "AiEngineGimbalServer::zoomToAiTargetSlot() Move camera to the new target:" << "index:" << target.index @@ -41,12 +57,16 @@ void AiEngineGimbalServer::zoomToAiTargetSlot(AiEngineCameraTarget target) // Rectangle calculation for having proper zoom on group / target AiEngineRectangleProperties rectangle = mActions.calculateRectangleProperties(target.rectangle.top, target.rectangle.left, target.rectangle.bottom, target.rectangle.right); + qDebug() << "rectangle.middleX: " << rectangle.middleX; + qDebug() << "rectangle.middleY: " << rectangle.middleY; + qDebug() << "rectangle.width: " << rectangle.width; + qDebug() << "rectangle.height: " << rectangle.height; // Turn mActions.turnToTarget(rectangle); // Calculate location - int delay1 = 1000; // Adjust this value as needed + int delay1 = 3000; // Adjust this value as needed AiEngineDronePosition dronePosition = mDronePosition; int targetIndex = target.index; QTimer::singleShot(delay1, this, [this, dronePosition, targetIndex]() { mActions.getLocation(dronePosition, targetIndex); }); @@ -61,8 +81,11 @@ void AiEngineGimbalServer::zoomToAiTargetSlot(AiEngineCameraTarget target) QTimer::singleShot(delay3, this, [this, gimbalStatus]() { mActions.restoreOrientationAndZoom(gimbalStatus); }); // Allow calls - int delay4 = delay3 + 100; // Adjust this value as needed - QTimer::singleShot(delay4, this, [this]() { mIsAvailable = true; }); + int delay4 = delay3 + 3000; // Adjust this value as needed + QTimer::singleShot(delay4, this, [this]() { + mIsAvailable = true; + qDebug() << "Is available: " << mIsAvailable; + }); } } @@ -76,5 +99,5 @@ bool AiEngineGimbalServer::isAvailable(void) // TODO!! Not sent from the client yet. void AiEngineGimbalServer::cameraPositionSlot(AiEngineCameraPosition position) { - qDebug() << "SAiEngineGimbalServer::cameraPositionSlot() Move camera to:" << position.pitch << position.yaw << "zoom:" << position.zoom; + qDebug() << "AiEngineGimbalServer::cameraPositionSlot() Move camera to:" << position.pitch << position.yaw << "zoom:" << position.zoom; } diff --git a/misc/rtsp_ai_player/aienginegimbalserver.h b/misc/rtsp_ai_player/aienginegimbalserver.h index b470191..f17226f 100644 --- a/misc/rtsp_ai_player/aienginegimbalserver.h +++ b/misc/rtsp_ai_player/aienginegimbalserver.h @@ -2,12 +2,11 @@ #include #include -#include #include "aienginedefinitions.h" #include "aienginegimbalserveractions.h" -#include "aienginegimbalserverserialcommand.h" -#include "aienginegimbalserverserialport.h" -#include "aienginegimbalserverserialresponse.h" +#include "aienginegimbalserverudpcommand.h" +#include "aienginegimbalserverudpresponse.h" +#include "aienginegimbalserverudp.h" class AiEngineGimbalServer : public QObject @@ -27,11 +26,12 @@ signals: void newCameraPosition(AiEngineCameraPosition); private: - AiEngineDronePosition mDronePosition; - AiEngineGimbalServerSerialPort mSerialPort; - AiEngineGimbalServerSerialCommand mSerialCommand; - AiEngineGimbalServerSerialResponse mSerialResponse; + AiEngineGimbalServerUDP mUdpSocket; AiEngineGimbalStatus mGimbalStatus; + + AiEngineDronePosition mDronePosition; + AiEngineGimbalServerUDPCommand mUdpCommand; + AiEngineGimbalServerUDPResponse mUdpResponse; AiEngineGimbalServerActions mActions; bool mIsAvailable; }; diff --git a/misc/rtsp_ai_player/aienginegimbalserveractions.cpp b/misc/rtsp_ai_player/aienginegimbalserveractions.cpp index 1d8709d..64eb482 100644 --- a/misc/rtsp_ai_player/aienginegimbalserveractions.cpp +++ b/misc/rtsp_ai_player/aienginegimbalserveractions.cpp @@ -1,5 +1,7 @@ +#include #include #include "aienginegimbalserveractions.h" +#include "aienginegimbalserverudp.h" AiEngineGimbalServerActions::AiEngineGimbalServerActions(QObject *parent) @@ -9,11 +11,11 @@ AiEngineGimbalServerActions::AiEngineGimbalServerActions(QObject *parent) } -void AiEngineGimbalServerActions::setup(AiEngineGimbalServerSerialPort *serialPort, AiEngineGimbalServerSerialCommand *serialCommand, AiEngineGimbalServerSerialResponse *serialResponse, AiEngineGimbalStatus *gimbalStatus) +void AiEngineGimbalServerActions::setup(AiEngineGimbalServerUDP *udpSocket, AiEngineGimbalServerUDPCommand *udpCommand, AiEngineGimbalServerUDPResponse *udpResponse, AiEngineGimbalStatus *gimbalStatus) { - mSerialPort = serialPort; - mSerialCommand = serialCommand; - mSerialResponse = serialResponse; + mUdpSocket = udpSocket; + mUdpCommand = udpCommand; + mUdpResponse = udpResponse; mGimbalStatus = gimbalStatus; // Set initial position and update status @@ -21,48 +23,94 @@ void AiEngineGimbalServerActions::setup(AiEngineGimbalServerSerialPort *serialPo QByteArray tempResponse; QHash responseValues; + // Get camera ID + tempCommand = mUdpCommand->getCommand(UDP_COMMAND_ID::ACQUIRE_HW_INFO); + mUdpSocket->sendCommand(tempCommand); + tempResponse = mUdpSocket->readResponse(); + responseValues = mUdpResponse->getResponceValues(tempResponse); + mGimbalStatus->hardwareID = responseValues["hardware_id"].toInt(); + + switch (mGimbalStatus->hardwareID) { + case AI_ENGINE_CAMERA_ZR10_HID: + mGimbalStatus->aiEngineCameraFOVHMin = AI_ENGINE_CAMERA_ZR10_FIELD_OF_VIEW_HORIZONTAL_MIN; + mGimbalStatus->aiEngineCameraFOVHMax = AI_ENGINE_CAMERA_ZR10_FIELD_OF_VIEW_HORIZONTAL_MAX; + mGimbalStatus->aiEngineCameraFOVVMin = AI_ENGINE_CAMERA_ZR10_FIELD_OF_VIEW_VERTICAL_MIN; + mGimbalStatus->aiEngineCameraFOVVMax = AI_ENGINE_CAMERA_ZR10_FIELD_OF_VIEW_VERTICAL_MAX; + mGimbalStatus->aiEngineCameraFLMin = AI_ENGINE_CAMERA_ZR10_FOCAL_LENGTH_MIN; + mGimbalStatus->aiEngineCameraFLMax = AI_ENGINE_CAMERA_ZR10_FOCAL_LENGTH_MAX; + break; + case AI_ENGINE_CAMERA_A8_HID: + mGimbalStatus->aiEngineCameraFOVHMin = AI_ENGINE_CAMERA_A8_FIELD_OF_VIEW_HORIZONTAL_MIN; + mGimbalStatus->aiEngineCameraFOVHMax = AI_ENGINE_CAMERA_A8_FIELD_OF_VIEW_HORIZONTAL_MAX; + mGimbalStatus->aiEngineCameraFOVVMin = AI_ENGINE_CAMERA_A8_FIELD_OF_VIEW_VERTICAL_MIN; + mGimbalStatus->aiEngineCameraFOVVMax = AI_ENGINE_CAMERA_A8_FIELD_OF_VIEW_VERTICAL_MAX; + mGimbalStatus->aiEngineCameraFLMin = AI_ENGINE_CAMERA_A8_FOCAL_LENGTH_MIN; + mGimbalStatus->aiEngineCameraFLMax = AI_ENGINE_CAMERA_A8_FOCAL_LENGTH_MAX; + break; + default: + qDebug().noquote().nospace() << "ERROR: Unknown HardwareID " << mGimbalStatus->hardwareID; + break; + } + // Get resolution to reduce calls to camera - tempCommand = mSerialCommand->getCommand(SERIAL_COMMAND_ID::ACQUIRE_CAMERA_CODEC_SPECS); - mSerialPort->sendCommand(tempCommand); - tempResponse = mSerialPort->readResponse(); - responseValues = mSerialResponse->getResponceValues(tempResponse); + tempCommand = mUdpCommand->getCommand(UDP_COMMAND_ID::ACQUIRE_CAMERA_CODEC_SPECS); + mUdpSocket->sendCommand(tempCommand); + tempResponse = mUdpSocket->readResponse(); + responseValues = mUdpResponse->getResponceValues(tempResponse); mGimbalStatus->resolutionX = responseValues["width"].toInt(); mGimbalStatus->resolutionY = responseValues["height"].toInt(); // Get max zoom value to reduce calls to camera - tempCommand = mSerialCommand->getCommand(SERIAL_COMMAND_ID::ACQUIRE_MAX_ZOOM_VALUE); - mSerialPort->sendCommand(tempCommand); - tempResponse = mSerialPort->readResponse(); - responseValues = mSerialResponse->getResponceValues(tempResponse); - mGimbalStatus->maxZoom = responseValues["zoom"].toInt(); + tempCommand = mUdpCommand->getCommand(UDP_COMMAND_ID::ACQUIRE_MAX_ZOOM_VALUE); + mUdpSocket->sendCommand(tempCommand); + tempResponse = mUdpSocket->readResponse(); + responseValues = mUdpResponse->getResponceValues(tempResponse); + float maxZoom = responseValues["zoom"].toFloat(); + mGimbalStatus->maxZoom = maxZoom > 10 ? 10 : maxZoom; // Go to initial orientation - tempCommand = mSerialCommand->getCommand(SERIAL_COMMAND_ID::TURN_TO_DEGREES); + 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; - mSerialPort->sendCommand(tempCommand); + mUdpSocket->sendCommand(tempCommand); // Go to initial zoom - tempCommand = mSerialCommand->getCommand(SERIAL_COMMAND_ID::ZOOM_TO_X); + 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; - mSerialPort->sendCommand(tempCommand); + mUdpSocket->sendCommand(tempCommand); mGimbalStatus->currentPitch = AI_ENGINE_GIMBAL_INITIAL_PITCH; mGimbalStatus->currentRoll = AI_ENGINE_GIMBAL_INITIAL_ROLL; mGimbalStatus->currentYaw = AI_ENGINE_GIMBAL_INITIAL_YAW; mGimbalStatus->currentZoom = AI_ENGINE_CAMERA_INITIAL_ZOOM; + + qDebug().noquote().nospace() << "mGimbalStatus->hardwareID: " << mGimbalStatus->hardwareID; + qDebug().noquote().nospace() << "mGimbalStatus->maxZoom: " << mGimbalStatus->maxZoom; + qDebug().noquote().nospace() << "mGimbalStatus->currentZoom: " << mGimbalStatus->currentZoom; + qDebug().noquote().nospace() << "mGimbalStatus->currentPitch: " << mGimbalStatus->currentPitch; + qDebug().noquote().nospace() << "mGimbalStatus->currentYaw: " << mGimbalStatus->currentYaw; + qDebug().noquote().nospace() << "mGimbalStatus->currentRoll: " << mGimbalStatus->currentRoll; + qDebug().noquote().nospace() << "mGimbalStatus->resolutionX: " << mGimbalStatus->resolutionX; + qDebug().noquote().nospace() << "mGimbalStatus->resolutionY: " << mGimbalStatus->resolutionY; } AiEngineRectangleProperties AiEngineGimbalServerActions::calculateRectangleProperties(int top, int left, int bottom, int right) { + // AI resolution is 640 * 360, which is half of 1280 * 720 + // Multiply by 2 to get appropriate position on video + top = top * 2; + left = left * 2; + bottom = bottom * 2; + right = right * 2; + // Sanity check // top cannot be greater than bottom // left cannot be greater than right @@ -88,10 +136,10 @@ AiEngineRectangleProperties AiEngineGimbalServerActions::calculateRectanglePrope // Sanity check, none cannot be 0 // If that is the case, we will not turn or zoom if (properties.height == 0 || properties.width == 0 || properties.middleX == 0 || properties.middleY == 0) { - properties.height = AI_ENGINE_CAMERA_RESOLUTION_HEIGHT; - properties.width = AI_ENGINE_CAMERA_RESOLUTION_WIDTH; - properties.middleX = AI_ENGINE_CAMERA_RESOLUTION_WIDTH / 2; - properties.middleY = AI_ENGINE_CAMERA_RESOLUTION_HEIGHT / 2; + properties.height = mGimbalStatus->resolutionY; + properties.width = mGimbalStatus->resolutionX; + properties.middleX = mGimbalStatus->resolutionX / 2; + properties.middleY = mGimbalStatus->resolutionY / 2; qWarning().noquote().nospace() << "calculateRectangleProperties(): Something was zero -> No zoom, no turn!"; } @@ -110,15 +158,17 @@ void AiEngineGimbalServerActions::getAnglesToOnScreenTarget(int targetX, int tar float normPixelY = (targetY - mGimbalStatus->resolutionY / 2.0f) / (mGimbalStatus->resolutionY / 2.0f); // Adjust horizontal field of view for zoom - float horizontalFov = AI_ENGINE_CAMERA_FIELD_OF_VIEW_HORIZONTAL * (1.0f + (mGimbalStatus->currentZoom - 1.0f) / 5.0f); + float horizontalFov = mGimbalStatus->aiEngineCameraFOVHMin + (10 - mGimbalStatus->currentZoom) * (mGimbalStatus->aiEngineCameraFOVHMax - mGimbalStatus->aiEngineCameraFOVHMin) / 9; + float verticalFov = mGimbalStatus->aiEngineCameraFOVVMin + (10 - mGimbalStatus->currentZoom) * (mGimbalStatus->aiEngineCameraFOVVMax - mGimbalStatus->aiEngineCameraFOVVMin) / 9; + float focalLength = mGimbalStatus->aiEngineCameraFLMin + (mGimbalStatus->currentZoom - 1) * (mGimbalStatus->aiEngineCameraFLMax - mGimbalStatus->aiEngineCameraFLMin) / 9; // Calculate image plane dimensions based on focal length and aspect ratio - float imagePlaneWidth = 2.0f * AI_ENGINE_CAMERA_FOCAL_LENGTH * tan(degreesToRadians(horizontalFov) / 2.0f); - float imagePlaneHeight = imagePlaneWidth / AI_ENGINE_CAMERA_ASPECT_RATIO; + float imagePlaneWidth = 2.0f * focalLength * tan(degreesToRadians(horizontalFov) / 2.0f); + float imagePlaneHeight = 2.0f * focalLength * tan(degreesToRadians(verticalFov) / 2.0f); // Calculate angle offsets based on normalized pixel location and image plane dimensions - float turnX = atan2(normPixelX * imagePlaneWidth / 2.0f, AI_ENGINE_CAMERA_FOCAL_LENGTH) * 180.0f / M_PI; - float turnY = atan2(normPixelY * imagePlaneHeight / 2.0f, AI_ENGINE_CAMERA_FOCAL_LENGTH) * 180.0f / M_PI; + float turnX = atan2(normPixelX * imagePlaneWidth / 2.0f, mGimbalStatus->aiEngineCameraFLMin) * 180.0f / M_PI; + float turnY = atan2(normPixelY * imagePlaneHeight / 2.0f, mGimbalStatus->aiEngineCameraFLMin) * 180.0f / M_PI; // Make alterations to current angles resultYaw -= turnX; @@ -134,7 +184,7 @@ void AiEngineGimbalServerActions::turnToTarget(AiEngineRectangleProperties recta float resultPitch = 0.0f; getAnglesToOnScreenTarget(rectangle.middleX, rectangle.middleY, resultYaw, resultPitch); - QByteArray serialCommandTurn = mSerialCommand->getCommand(SERIAL_COMMAND_ID::TURN_TO_PIXEL); + QByteArray serialCommandTurn = mUdpCommand->getCommand(UDP_COMMAND_ID::TURN_TO_PIXEL); int16_t degreesVal = resultYaw * 10; serialCommandTurn[8] = degreesVal & 0xFF; @@ -144,7 +194,7 @@ void AiEngineGimbalServerActions::turnToTarget(AiEngineRectangleProperties recta serialCommandTurn[10] = degreesVal & 0xFF; serialCommandTurn[11] = degreesVal >> 8; - mSerialPort->sendCommand(serialCommandTurn); + mUdpSocket->sendCommand(serialCommandTurn); } @@ -164,7 +214,7 @@ void AiEngineGimbalServerActions::zoomToTarget(AiEngineRectangleProperties recta zoom = 1.0f; } - QByteArray serialCommandNewZoom = mSerialCommand->getCommand(SERIAL_COMMAND_ID::ZOOM_TO_X); + QByteArray serialCommandNewZoom = mUdpCommand->getCommand(UDP_COMMAND_ID::ZOOM_TO_X); uint8_t integerPart = static_cast(zoom); float fractionalPart = zoom - integerPart; @@ -173,7 +223,7 @@ void AiEngineGimbalServerActions::zoomToTarget(AiEngineRectangleProperties recta serialCommandNewZoom[8] = integerPart; serialCommandNewZoom[9] = scaledFractional; - mSerialPort->sendCommand(serialCommandNewZoom); + mUdpSocket->sendCommand(serialCommandNewZoom); } @@ -191,6 +241,10 @@ void AiEngineGimbalServerActions::getLocation(AiEngineDronePosition dronePositio AiEngineTargetPosition targetReturn; targetReturn.position = targetPosition; targetReturn.targetIndex = targetIndex; + qDebug() << "targetReturn.targetIndex: " << targetReturn.targetIndex; + qDebug() << "targetReturn.position.alt: " << targetReturn.position.alt; + qDebug() << "targetReturn.position.lat: " << targetReturn.position.lat; + qDebug() << "targetReturn.position.lon: " << targetReturn.position.lon; emit aiTargetZoomed(targetReturn); } @@ -200,23 +254,23 @@ void AiEngineGimbalServerActions::restoreOrientationAndZoom(AiEngineGimbalStatus QByteArray tempCommand; // Go to initial orientation - tempCommand = mSerialCommand->getCommand(SERIAL_COMMAND_ID::TURN_TO_DEGREES); + 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; - mSerialPort->sendCommand(tempCommand); + mUdpSocket->sendCommand(tempCommand); // Go to initial zoom - tempCommand = mSerialCommand->getCommand(SERIAL_COMMAND_ID::ZOOM_TO_X); + 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; - mSerialPort->sendCommand(tempCommand); + mUdpSocket->sendCommand(tempCommand); // TODO: Maybe send signal that all done? } @@ -228,10 +282,8 @@ CameraData AiEngineGimbalServerActions::getCameraData() uint16_t width = mGimbalStatus->resolutionX; float yaw = 0 - mGimbalStatus->currentYaw; // Reverse value for calculation purposes float pitch = 0 - mGimbalStatus->currentPitch; // Reverse value for calculation purposes - float zoom = mGimbalStatus->currentZoom; - float fov = AI_ENGINE_CAMERA_FIELD_OF_VIEW_HORIZONTAL / zoom; - return {height, width, pitch, yaw, fov}; + return {height, width, pitch, yaw}; } @@ -243,8 +295,6 @@ AiEngineGeoPosition AiEngineGimbalServerActions::calculateTargetLocation(DroneDa float slantDistance = 0; float horizontalDistance = 0; calculateDistancesToTarget(drone.gps.altitude, camera.pitch, slantDistance, horizontalDistance); - qInfo().noquote().nospace() << "horizontalDistance: " << horizontalDistance; - qInfo().noquote().nospace() << "slantDistance: " << slantDistance; // Calculate new altitude using the slant distance and angle float pitchRad = degreesToRadians(camera.pitch); diff --git a/misc/rtsp_ai_player/aienginegimbalserveractions.h b/misc/rtsp_ai_player/aienginegimbalserveractions.h index 0905b72..c5a350c 100644 --- a/misc/rtsp_ai_player/aienginegimbalserveractions.h +++ b/misc/rtsp_ai_player/aienginegimbalserveractions.h @@ -2,9 +2,9 @@ #include #include "aienginedefinitions.h" -#include "aienginegimbalserverserialcommand.h" -#include "aienginegimbalserverserialport.h" -#include "aienginegimbalserverserialresponse.h" +#include "aienginegimbalserverudpcommand.h" +#include "aienginegimbalserverudpresponse.h" +#include "aienginegimbalserverudp.h" const double EARTH_RADIUS = 6371000.0; // Earth's radius in meters @@ -24,7 +24,6 @@ struct CameraData uint16_t width; // Pixels float pitch; // Degrees float yaw; // Degrees - float fow; // Degrees }; @@ -43,8 +42,8 @@ class AiEngineGimbalServerActions : public QObject public: explicit AiEngineGimbalServerActions(QObject *parent = nullptr); -public: - void setup(AiEngineGimbalServerSerialPort *serialPort, AiEngineGimbalServerSerialCommand *serialCommand, AiEngineGimbalServerSerialResponse *serialResponse, AiEngineGimbalStatus *gimbalStatus); +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); @@ -55,11 +54,12 @@ signals: void aiTargetZoomed(AiEngineTargetPosition); private: - AiEngineGimbalServerSerialPort *mSerialPort; - AiEngineGimbalServerSerialCommand *mSerialCommand; - AiEngineGimbalServerSerialResponse *mSerialResponse; + AiEngineGimbalServerUDP *mUdpSocket; + AiEngineGimbalServerUDPCommand *mUdpCommand; + AiEngineGimbalServerUDPResponse *mUdpResponse; AiEngineGimbalStatus *mGimbalStatus; +private slots: CameraData getCameraData(void); void getAnglesToOnScreenTarget(int targetX, int targetY, float &resultYaw, float &resultPitch); AiEngineGeoPosition calculateTargetLocation(DroneData drone, CameraData camera); diff --git a/misc/rtsp_ai_player/aienginegimbalserverdefines.h b/misc/rtsp_ai_player/aienginegimbalserverdefines.h index 6787527..575655e 100644 --- a/misc/rtsp_ai_player/aienginegimbalserverdefines.h +++ b/misc/rtsp_ai_player/aienginegimbalserverdefines.h @@ -6,29 +6,34 @@ #pragma once +#define AI_ENGINE_CAMERA_ZR10_HID 0x6B // 107 +#define AI_ENGINE_CAMERA_ZR10_FIELD_OF_VIEW_HORIZONTAL_MIN 6.7f +#define AI_ENGINE_CAMERA_ZR10_FIELD_OF_VIEW_HORIZONTAL_MAX 47.0f +#define AI_ENGINE_CAMERA_ZR10_FIELD_OF_VIEW_VERTICAL_MIN 3.8f +#define AI_ENGINE_CAMERA_ZR10_FIELD_OF_VIEW_VERTICAL_MAX 30.0f +#define AI_ENGINE_CAMERA_ZR10_FOCAL_LENGTH_MIN 5.2f +#define AI_ENGINE_CAMERA_ZR10_FOCAL_LENGTH_MAX 47.5f + +#define AI_ENGINE_CAMERA_A8_HID 0x73 // 115 +#define AI_ENGINE_CAMERA_A8_FIELD_OF_VIEW_HORIZONTAL_MIN 85.0f +#define AI_ENGINE_CAMERA_A8_FIELD_OF_VIEW_HORIZONTAL_MAX 85.0f +#define AI_ENGINE_CAMERA_A8_FIELD_OF_VIEW_VERTICAL_MIN 58.0f +#define AI_ENGINE_CAMERA_A8_FIELD_OF_VIEW_VERTICAL_MAX 58.0f +#define AI_ENGINE_CAMERA_A8_FOCAL_LENGTH_MIN 21.0f +#define AI_ENGINE_CAMERA_A8_FOCAL_LENGTH_MAX 21.0f + #define AI_ENGINE_CAMERA_ASPECT_RATIO 1.777777778f -#define AI_ENGINE_CAMERA_FIELD_OF_VIEW_DIAGONAL 93.0f -#define AI_ENGINE_CAMERA_FIELD_OF_VIEW_HORIZONTAL 81.0f -#define AI_ENGINE_CAMERA_FIELD_OF_VIEW_VERTICAL 62.0f -#define AI_ENGINE_CAMERA_FOCAL_LENGTH 21 -#define AI_ENGINE_CAMERA_RESOLUTION_WIDTH 1280 -#define AI_ENGINE_CAMERA_RESOLUTION_HEIGHT 720 #define AI_ENGINE_GIMBAL_YAW_MIN -135.0f #define AI_ENGINE_GIMBAL_YAW_MAX 135.0f #define AI_ENGINE_GIMBAL_PITCH_MIN -90.0f #define AI_ENGINE_GIMBAL_PITCH_MAX 25.0f -#define AI_ENGINE_SERIAL_RESPONSE_WAIT_TIME 500 -#define AI_ENGINE_SERIAL_PORT "/dev/ttyUSB0" -#define AI_ENGINE_UDP_WHO_AM_I "CAM" -#define AI_ENGINE_UDP_PORT 26662 - #define AI_ENGINE_CAMERA_INITIAL_ZOOM 1.0f -#define AI_ENGINE_GIMBAL_INITIAL_PITCH -45.0f +#define AI_ENGINE_GIMBAL_INITIAL_PITCH -20.0f #define AI_ENGINE_GIMBAL_INITIAL_ROLL 0.0f #define AI_ENGINE_GIMBAL_INITIAL_YAW 0.0f -enum SERIAL_COMMAND_ID { +enum UDP_COMMAND_ID { TURN_TO_DEGREES = 1, TURN_TO_PIXEL, ZOOM_TO_X, diff --git a/misc/rtsp_ai_player/aienginegimbalserverserialcommand.cpp b/misc/rtsp_ai_player/aienginegimbalserverserialcommand.cpp deleted file mode 100644 index c39b8ed..0000000 --- a/misc/rtsp_ai_player/aienginegimbalserverserialcommand.cpp +++ /dev/null @@ -1,102 +0,0 @@ -/** - * This is a serial command class for Siyi Gimbal Cameras. - * Other cameras might need their own serial command class. - */ - -#include -#include "aienginegimbalserverserialcommand.h" - - -AiEngineGimbalServerSerialCommand::AiEngineGimbalServerSerialCommand(QObject *parent) - : QObject{parent} -{ - /* - Field Index Bytes Description - STX 0 2 0x6655: starting mark. Low byte in the front - CTRL 2 1 0: need_ack (if the current data pack need “ack”) - 1: ack_pack (if the current data pack is an “ack” package) 2-7: reserved - Data_len 3 2 Data field byte length. Low byte in the front - SEQ 5 2 Frame sequence (0 ~ 65535). Low byte in the front - CMD_ID 7 1 Command ID - DATA 8 Data_len Data - CRC16 2 CRC16 check to the complete data package. Low - byte in the front - */ - - mSerialCommands.push_back({SERIAL_COMMAND_ID::TURN_TO_DEGREES, createByteArray({0x55, 0x66, 0x01, 0x04, 0x00, 0x00, 0x00, 0x0E, 0x00, 0x00, 0x00, 0x00}), "Turn to degrees"}); - mSerialCommands.push_back({SERIAL_COMMAND_ID::TURN_TO_PIXEL, createByteArray({0x55, 0x66, 0x01, 0x04, 0x00, 0x00, 0x00, 0x0E, 0x00, 0x00, 0x00, 0x00}), "Turn to pixel"}); - mSerialCommands.push_back({SERIAL_COMMAND_ID::ZOOM_TO_X, createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x01, 0x00, 0x0F, 0x00, 0x00}), "Zoom to X"}); - mSerialCommands.push_back({SERIAL_COMMAND_ID::AUTO_CENTER, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x08, 0x01}), "Auto Centering"}); - mSerialCommands.push_back({SERIAL_COMMAND_ID::ACQUIRE_ATTITUDE_DATA, createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x0D}), "Acquire Attitude Data"}); - mSerialCommands.push_back({SERIAL_COMMAND_ID::ACQUIRE_CURRENT_ZOOM, createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x18}), "Acquire current zoom"}); - mSerialCommands.push_back({SERIAL_COMMAND_ID::ACQUIRE_CAMERA_CODEC_SPECS, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x20, 0x00}), "Acquire Camera Codec Specs"}); - mSerialCommands.push_back({SERIAL_COMMAND_ID::ZOOM_MOST, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x05, 0x01}), "Zoom +1"}); - mSerialCommands.push_back({SERIAL_COMMAND_ID::ZOOM_LEAST, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x05, 0xFF}), "Zoom -1"}); - mSerialCommands.push_back({SERIAL_COMMAND_ID::FOCUS_MOST, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x06, 0x01}), "Manual Focus +1"}); - mSerialCommands.push_back({SERIAL_COMMAND_ID::FOCUS_LEAST, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x06, 0xFF}), "Manual Focus -1"}); - mSerialCommands.push_back({SERIAL_COMMAND_ID::FOCUS_AUTO, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x04, 0x01}), "Auto Focus"}); - mSerialCommands.push_back({SERIAL_COMMAND_ID::ROTATE_UP, createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x2D}), "Rotate Up"}); - mSerialCommands.push_back({SERIAL_COMMAND_ID::ROTATE_DOWN, createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, -0x2D}), "Rotate Down"}); - mSerialCommands.push_back({SERIAL_COMMAND_ID::ROTATE_RIGHT, createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x2D, 0x00}), "Rotate Right"}); - mSerialCommands.push_back({SERIAL_COMMAND_ID::ROTATE_LEFT, createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, -0x2D, 0x00}), "Rotate Left"}); - mSerialCommands.push_back({SERIAL_COMMAND_ID::ROTATE_STOP, createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00}), "Stop rotation"}); - mSerialCommands.push_back({SERIAL_COMMAND_ID::ACQUIRE_MAX_ZOOM_VALUE, createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x16}), "Acquire the Max Zoom Value"}); - mSerialCommands.push_back({SERIAL_COMMAND_ID::TAKE_PICTURES, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0C, 0x00}), "Take Pictures"}); - mSerialCommands.push_back({SERIAL_COMMAND_ID::TAKE_VIDEO, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0C, 0x02}), "Record Video"}); - mSerialCommands.push_back({SERIAL_COMMAND_ID::ROTATE_100_100, createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x64, 0x64}), "Rotate 100 100"}); - mSerialCommands.push_back({SERIAL_COMMAND_ID::ACQUIRE_GIMBAL_STATUS, createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x0A}), "Gimbal Status Information"}); - mSerialCommands.push_back({SERIAL_COMMAND_ID::ACQUIRE_HW_INFO, createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x02}), "Acquire Hardware ID"}); - mSerialCommands.push_back({SERIAL_COMMAND_ID::ACQUIRE_FIRMWARE_VERSION, createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01}), "Acquire Firmware Version"}); - mSerialCommands.push_back({SERIAL_COMMAND_ID::MODE_LOCK, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0C, 0x03}), "Lock Mode"}); - mSerialCommands.push_back({SERIAL_COMMAND_ID::MODE_FOLLOW, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0C, 0x04}), "Follow Mode"}); - mSerialCommands.push_back({SERIAL_COMMAND_ID::MODE_FPV, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0C, 0x05}), "FPV Mode"}); - mSerialCommands.push_back({SERIAL_COMMAND_ID::ENABLE_HDMI, - createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0C, 0x06}), - "Set Video Output as HDMI (Only available on A8 mini, restart to take " - "effect)"}); - mSerialCommands.push_back({SERIAL_COMMAND_ID::ENABLE_CVBS, - createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0C, 0x07}), - "Set Video Output as CVBS (Only available on A8 mini, restart to take " - "effect)"}); - mSerialCommands.push_back({SERIAL_COMMAND_ID::DISABLE_HDMI_CVBS, - createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0C, 0x08}), - "Turn Off both CVBS and HDMI Output (Only available on A8 mini, restart " - "to take effect)"}); - mSerialCommands.push_back({SERIAL_COMMAND_ID::ACQUIRE_RANGE_DATA, - createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x15}), - "Read Range from Laser Rangefinder(Low byte in the front, high byte in " - "the back, available on ZT30)"}); - mSerialCommands.push_back({SERIAL_COMMAND_ID::RUN_TARGET_LOCATION_TEST, createByteArray({0x00, 0x00}), "TEST target location calculations"}); - - // Sort vector by SERIAL_COMMAND_ID - std::sort(mSerialCommands.begin(), mSerialCommands.end(), [](const AiEngineServerSerialCommandStructure &a, const AiEngineServerSerialCommandStructure &b) { return a.id < b.id; }); -} - -QByteArray AiEngineGimbalServerSerialCommand::createByteArray(const std::initializer_list &bytes) -{ - QByteArray byteArray; - for (int byte : bytes) { - byteArray.append(static_cast(byte)); - } - return byteArray; -} - -int AiEngineGimbalServerSerialCommand::getCommandIndex(SERIAL_COMMAND_ID commandId) -{ - for (uint i = 0; i < mSerialCommands.size(); i++) { - if (mSerialCommands.at(i).id == commandId) { - return i; - } - } - - return -1; -} - -QByteArray AiEngineGimbalServerSerialCommand::getCommand(SERIAL_COMMAND_ID commandId) -{ - int commandIndex = getCommandIndex(commandId); - if (commandIndex == -1) { - qCritical().noquote().nospace() << "Command not found for command: " << commandId; - } - return mSerialCommands.at(commandIndex).command; -} diff --git a/misc/rtsp_ai_player/aienginegimbalserverserialport.cpp b/misc/rtsp_ai_player/aienginegimbalserverserialport.cpp deleted file mode 100644 index b737b4e..0000000 --- a/misc/rtsp_ai_player/aienginegimbalserverserialport.cpp +++ /dev/null @@ -1,92 +0,0 @@ -#include -#include -#include "aienginegimbalserverserialport.h" -#include "aienginegimbalservercrc16.h" -#include "aienginegimbalserverdefines.h" - - -AiEngineGimbalServerSerialPort::AiEngineGimbalServerSerialPort(QObject *parent) - : QObject{parent} -{ - mSerialPort = new QSerialPort(); - mSerialPort->setPortName(AI_ENGINE_SERIAL_PORT); - mSerialPort->setBaudRate(QSerialPort::Baud115200); - mSerialPort->setDataBits(QSerialPort::Data8); - mSerialPort->setStopBits(QSerialPort::OneStop); - mSerialPort->setFlowControl(QSerialPort::NoFlowControl); - - // Open the serial port - if (openPort() == false) { - qCritical().noquote().nospace() << "SerialPort(): Unable to open port " << AI_ENGINE_SERIAL_PORT; - delete mSerialPort; - exit(EXIT_FAILURE); - } - - qDebug().noquote().nospace() << "SerialPort(): Opened port " << AI_ENGINE_SERIAL_PORT; -} - -AiEngineGimbalServerSerialPort::~AiEngineGimbalServerSerialPort() -{ - closePort(); - delete mSerialPort; -} - -bool AiEngineGimbalServerSerialPort::openPort() -{ - if (mSerialPort->isOpen()) { - qDebug().noquote().nospace() << "Port already open"; - return true; - } - - return mSerialPort->open(QIODevice::ReadWrite); -} - -void AiEngineGimbalServerSerialPort::closePort() -{ - if (mSerialPort->isOpen()) { - mSerialPort->close(); - } -} - -void AiEngineGimbalServerSerialPort::sendCommand(const QByteArray &command) -{ - QByteArray toSend = command; - int8_t crcBytes[2]; - AiEngineGimbalServerCrc16::getCRCBytes(toSend, crcBytes); - toSend.resize(toSend.size() + 2); // Increase array size to accommodate CRC bytes - toSend[toSend.size() - 2] = crcBytes[0]; // Set LSB - toSend[toSend.size() - 1] = crcBytes[1]; // Set MSB - - QString commandStr; - for (int i = 0; i < toSend.size(); i++) { - if (i > 0) { - commandStr += ","; - } - commandStr += QString("0x%1").arg(toSend.at(i), 2, 16, QChar('0')).toUpper(); - commandStr.replace("0X", "0x"); - } - qDebug().noquote().nospace() << "Command: " << commandStr; - - if (!mSerialPort->isOpen()) { - qCritical().noquote().nospace() << "Error: Port not open (sendCommand)"; - return; - } - - mSerialPort->write(toSend); -} - -QByteArray AiEngineGimbalServerSerialPort::readResponse() -{ - if (!mSerialPort->isOpen()) { - qDebug().noquote().nospace() << "Error: Port not open (readResponse)"; - return QByteArray(); - } - - // Read data from serial port until timeout or specific criteria met - QByteArray response; - while (mSerialPort->waitForReadyRead(AI_ENGINE_SERIAL_RESPONSE_WAIT_TIME)) { // Adjust timeout as needed - response.append(mSerialPort->readAll()); - } - - return response; -} diff --git a/misc/rtsp_ai_player/aienginegimbalserverserialport.h b/misc/rtsp_ai_player/aienginegimbalserverserialport.h deleted file mode 100644 index 2600c62..0000000 --- a/misc/rtsp_ai_player/aienginegimbalserverserialport.h +++ /dev/null @@ -1,25 +0,0 @@ -#pragma once - -#include -#include -#include - -class AiEngineGimbalServerSerialPort : public QObject -{ - Q_OBJECT -public: - explicit AiEngineGimbalServerSerialPort(QObject *parent = nullptr); - -public: - ~AiEngineGimbalServerSerialPort(); - -public slots: - void sendCommand(const QByteArray &command); - QByteArray readResponse(); - -private: - QSerialPort *mSerialPort; - - bool openPort(); - void closePort(); -}; diff --git a/misc/rtsp_ai_player/aienginegimbalserverudp.cpp b/misc/rtsp_ai_player/aienginegimbalserverudp.cpp new file mode 100644 index 0000000..c892381 --- /dev/null +++ b/misc/rtsp_ai_player/aienginegimbalserverudp.cpp @@ -0,0 +1,73 @@ +#include +#include +#include "aienginegimbalserverudp.h" +#include "aienginegimbalservercrc16.h" + + +AiEngineGimbalServerUDP::AiEngineGimbalServerUDP(QObject *parent) + : QObject{parent} +{ + // Open UDP socket + mUdpSocket = new QUdpSocket(); + mUdpSocket->connectToHost(QHostAddress(SERVER_IP), SERVER_PORT); + + if (mUdpSocket->isOpen() == false) { + qCritical().noquote().nospace() << "AiEngineGimbalServerUDP(): Unable to connect " << SERVER_IP << ":" << SERVER_PORT; + delete mUdpSocket; + exit(EXIT_FAILURE); + } + + qDebug().noquote().nospace() << "AiEngineGimbalServerUDP(): Connected " << SERVER_IP << ":" << SERVER_PORT; +} + + +AiEngineGimbalServerUDP::~AiEngineGimbalServerUDP() +{ + if (mUdpSocket->isOpen() == true) { + mUdpSocket->close(); + } + + delete mUdpSocket; +} + + +void AiEngineGimbalServerUDP::sendCommand(const QByteArray &command) +{ + QByteArray toSend = command; + int8_t crcBytes[2]; + AiEngineGimbalServerCrc16::getCRCBytes(toSend, crcBytes); + toSend.resize(toSend.size() + 2); // Increase array size to accommodate CRC bytes + toSend[toSend.size() - 2] = crcBytes[0]; // Set LSB + toSend[toSend.size() - 1] = crcBytes[1]; // Set MSB + + qDebug().noquote().nospace() << "AiEngineGimbalServerUDP(): Sent HEX data: " << toSend.toHex(); + + if (mUdpSocket->isOpen() == false) { + qCritical().noquote().nospace() << "AiEngineGimbalServerUDP(): Connection not open (sendCommand)"; + return; + } + + if (mUdpSocket->write(toSend) == -1) { + qCritical().noquote().nospace() << "AiEngineGimbalServerUDP(): Failed to send data: " << mUdpSocket->errorString(); + } +} + +QByteArray AiEngineGimbalServerUDP::readResponse() +{ + QByteArray response; + QByteArray recvBuff(RECV_BUF_SIZE, 0); + + if (mUdpSocket->waitForReadyRead(3000)) { + qint64 recvLen = mUdpSocket->read(recvBuff.data(), RECV_BUF_SIZE); + if (recvLen > 0) { + response = recvBuff.left(recvLen); + qDebug().noquote().nospace() << "AiEngineGimbalServerUDP(): Received HEX data: " << response.toHex(); + } else { + qCritical().noquote().nospace() << "AiEngineGimbalServerUDP(): Failed to receive data: " << mUdpSocket->errorString(); + } + } else { + qCritical().noquote().nospace() << "AiEngineGimbalServerUDP(): No data received."; + } + + return response; +} diff --git a/misc/rtsp_ai_player/aienginegimbalserverudp.h b/misc/rtsp_ai_player/aienginegimbalserverudp.h new file mode 100644 index 0000000..0769f51 --- /dev/null +++ b/misc/rtsp_ai_player/aienginegimbalserverudp.h @@ -0,0 +1,28 @@ +#pragma once + + +#include +#include + + +#define SERVER_IP "192.168.0.25" +#define SERVER_PORT 37260 +#define RECV_BUF_SIZE 1024 + + +class AiEngineGimbalServerUDP : public QObject +{ + Q_OBJECT +public: + explicit AiEngineGimbalServerUDP(QObject *parent = nullptr); + +public: + ~AiEngineGimbalServerUDP(); + +public slots: + void sendCommand(const QByteArray &command); + QByteArray readResponse(); + +private: + QUdpSocket *mUdpSocket; +}; diff --git a/misc/rtsp_ai_player/aienginegimbalserverudpcommand.cpp b/misc/rtsp_ai_player/aienginegimbalserverudpcommand.cpp new file mode 100644 index 0000000..ce53c72 --- /dev/null +++ b/misc/rtsp_ai_player/aienginegimbalserverudpcommand.cpp @@ -0,0 +1,102 @@ +/** + * This is a serial command class for Siyi Gimbal Cameras. + * Other cameras might need their own serial command class. + */ + +#include +#include "aienginegimbalserverudpcommand.h" + + +AiEngineGimbalServerUDPCommand::AiEngineGimbalServerUDPCommand(QObject *parent) + : QObject{parent} +{ + /* + Field Index Bytes Description + STX 0 2 0x6655: starting mark. Low byte in the front + CTRL 2 1 0: need_ack (if the current data pack need “ack”) + 1: ack_pack (if the current data pack is an “ack” package) 2-7: reserved + Data_len 3 2 Data field byte length. Low byte in the front + SEQ 5 2 Frame sequence (0 ~ 65535). Low byte in the front + CMD_ID 7 1 Command ID + DATA 8 Data_len Data + CRC16 2 CRC16 check to the complete data package. Low + byte in the front + */ + + mSerialCommands.push_back({UDP_COMMAND_ID::TURN_TO_DEGREES, createByteArray({0x55, 0x66, 0x01, 0x04, 0x00, 0x00, 0x00, 0x0E, 0x00, 0x00, 0x00, 0x00}), "Turn to degrees"}); + mSerialCommands.push_back({UDP_COMMAND_ID::TURN_TO_PIXEL, createByteArray({0x55, 0x66, 0x01, 0x04, 0x00, 0x00, 0x00, 0x0E, 0x00, 0x00, 0x00, 0x00}), "Turn to pixel"}); + mSerialCommands.push_back({UDP_COMMAND_ID::ZOOM_TO_X, createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x01, 0x00, 0x0F, 0x00, 0x00}), "Zoom to X"}); + mSerialCommands.push_back({UDP_COMMAND_ID::AUTO_CENTER, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x08, 0x01}), "Auto Centering"}); + mSerialCommands.push_back({UDP_COMMAND_ID::ACQUIRE_ATTITUDE_DATA, createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x0D}), "Acquire Attitude Data"}); + mSerialCommands.push_back({UDP_COMMAND_ID::ACQUIRE_CURRENT_ZOOM, createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x18}), "Acquire current zoom"}); + mSerialCommands.push_back({UDP_COMMAND_ID::ACQUIRE_CAMERA_CODEC_SPECS, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x20, 0x01}), "Acquire Camera Codec Specs"}); + mSerialCommands.push_back({UDP_COMMAND_ID::ZOOM_MOST, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x05, 0x01}), "Zoom +1"}); + mSerialCommands.push_back({UDP_COMMAND_ID::ZOOM_LEAST, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x05, 0xFF}), "Zoom -1"}); + mSerialCommands.push_back({UDP_COMMAND_ID::FOCUS_MOST, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x06, 0x01}), "Manual Focus +1"}); + mSerialCommands.push_back({UDP_COMMAND_ID::FOCUS_LEAST, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x06, 0xFF}), "Manual Focus -1"}); + mSerialCommands.push_back({UDP_COMMAND_ID::FOCUS_AUTO, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x04, 0x01}), "Auto Focus"}); + mSerialCommands.push_back({UDP_COMMAND_ID::ROTATE_UP, createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x2D}), "Rotate Up"}); + mSerialCommands.push_back({UDP_COMMAND_ID::ROTATE_DOWN, createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, -0x2D}), "Rotate Down"}); + mSerialCommands.push_back({UDP_COMMAND_ID::ROTATE_RIGHT, createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x2D, 0x00}), "Rotate Right"}); + mSerialCommands.push_back({UDP_COMMAND_ID::ROTATE_LEFT, createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, -0x2D, 0x00}), "Rotate Left"}); + mSerialCommands.push_back({UDP_COMMAND_ID::ROTATE_STOP, createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00}), "Stop rotation"}); + mSerialCommands.push_back({UDP_COMMAND_ID::ACQUIRE_MAX_ZOOM_VALUE, createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x16}), "Acquire the Max Zoom Value"}); + mSerialCommands.push_back({UDP_COMMAND_ID::TAKE_PICTURES, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0C, 0x00}), "Take Pictures"}); + mSerialCommands.push_back({UDP_COMMAND_ID::TAKE_VIDEO, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0C, 0x02}), "Record Video"}); + mSerialCommands.push_back({UDP_COMMAND_ID::ROTATE_100_100, createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x64, 0x64}), "Rotate 100 100"}); + mSerialCommands.push_back({UDP_COMMAND_ID::ACQUIRE_GIMBAL_STATUS, createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x0A}), "Gimbal Status Information"}); + mSerialCommands.push_back({UDP_COMMAND_ID::ACQUIRE_HW_INFO, createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x02}), "Acquire Hardware ID"}); + mSerialCommands.push_back({UDP_COMMAND_ID::ACQUIRE_FIRMWARE_VERSION, createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01}), "Acquire Firmware Version"}); + mSerialCommands.push_back({UDP_COMMAND_ID::MODE_LOCK, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0C, 0x03}), "Lock Mode"}); + mSerialCommands.push_back({UDP_COMMAND_ID::MODE_FOLLOW, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0C, 0x04}), "Follow Mode"}); + mSerialCommands.push_back({UDP_COMMAND_ID::MODE_FPV, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0C, 0x05}), "FPV Mode"}); + mSerialCommands.push_back({UDP_COMMAND_ID::ENABLE_HDMI, + createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0C, 0x06}), + "Set Video Output as HDMI (Only available on A8 mini, restart to take " + "effect)"}); + mSerialCommands.push_back({UDP_COMMAND_ID::ENABLE_CVBS, + createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0C, 0x07}), + "Set Video Output as CVBS (Only available on A8 mini, restart to take " + "effect)"}); + mSerialCommands.push_back({UDP_COMMAND_ID::DISABLE_HDMI_CVBS, + createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0C, 0x08}), + "Turn Off both CVBS and HDMI Output (Only available on A8 mini, restart " + "to take effect)"}); + mSerialCommands.push_back({UDP_COMMAND_ID::ACQUIRE_RANGE_DATA, + createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x15}), + "Read Range from Laser Rangefinder(Low byte in the front, high byte in " + "the back, available on ZT30)"}); + mSerialCommands.push_back({UDP_COMMAND_ID::RUN_TARGET_LOCATION_TEST, createByteArray({0x00, 0x00}), "TEST target location calculations"}); + + // Sort vector by SERIAL_COMMAND_ID + std::sort(mSerialCommands.begin(), mSerialCommands.end(), [](const AiEngineServerSerialCommandStructure &a, const AiEngineServerSerialCommandStructure &b) { return a.id < b.id; }); +} + +QByteArray AiEngineGimbalServerUDPCommand::createByteArray(const std::initializer_list &bytes) +{ + QByteArray byteArray; + for (int byte : bytes) { + byteArray.append(static_cast(byte)); + } + return byteArray; +} + +int AiEngineGimbalServerUDPCommand::getCommandIndex(UDP_COMMAND_ID commandId) +{ + for (uint i = 0; i < mSerialCommands.size(); i++) { + if (mSerialCommands.at(i).id == commandId) { + return i; + } + } + + return -1; +} + +QByteArray AiEngineGimbalServerUDPCommand::getCommand(UDP_COMMAND_ID commandId) +{ + int commandIndex = getCommandIndex(commandId); + if (commandIndex == -1) { + qCritical().noquote().nospace() << "Command not found for command: " << commandId; + } + return mSerialCommands.at(commandIndex).command; +} diff --git a/misc/rtsp_ai_player/aienginegimbalserverserialcommand.h b/misc/rtsp_ai_player/aienginegimbalserverudpcommand.h similarity index 64% rename from misc/rtsp_ai_player/aienginegimbalserverserialcommand.h rename to misc/rtsp_ai_player/aienginegimbalserverudpcommand.h index 9fef803..91963ff 100644 --- a/misc/rtsp_ai_player/aienginegimbalserverserialcommand.h +++ b/misc/rtsp_ai_player/aienginegimbalserverudpcommand.h @@ -11,17 +11,17 @@ #include #include "aienginedefinitions.h" -class AiEngineGimbalServerSerialCommand : public QObject +class AiEngineGimbalServerUDPCommand : public QObject { Q_OBJECT public: - explicit AiEngineGimbalServerSerialCommand(QObject *parent = nullptr); + explicit AiEngineGimbalServerUDPCommand(QObject *parent = nullptr); public: - QByteArray getCommand(SERIAL_COMMAND_ID commandId); + QByteArray getCommand(UDP_COMMAND_ID commandId); private: QByteArray createByteArray(const std::initializer_list &bytes); - int getCommandIndex(SERIAL_COMMAND_ID commandId); + int getCommandIndex(UDP_COMMAND_ID commandId); std::vector mSerialCommands; }; diff --git a/misc/rtsp_ai_player/aienginegimbalserverserialresponse.cpp b/misc/rtsp_ai_player/aienginegimbalserverudpresponse.cpp similarity index 88% rename from misc/rtsp_ai_player/aienginegimbalserverserialresponse.cpp rename to misc/rtsp_ai_player/aienginegimbalserverudpresponse.cpp index 58bcb9e..3ba81c6 100644 --- a/misc/rtsp_ai_player/aienginegimbalserverserialresponse.cpp +++ b/misc/rtsp_ai_player/aienginegimbalserverudpresponse.cpp @@ -8,16 +8,16 @@ #include #include #include "aienginegimbalservercrc16.h" -#include "aienginegimbalserverserialresponse.h" +#include "aienginegimbalserverudpresponse.h" -AiEngineGimbalServerSerialResponse::AiEngineGimbalServerSerialResponse(QObject *parent) +AiEngineGimbalServerUDPResponse::AiEngineGimbalServerUDPResponse(QObject *parent) : QObject{parent} { } -QHash AiEngineGimbalServerSerialResponse::getResponceValues(QByteArray response) +QHash AiEngineGimbalServerUDPResponse::getResponceValues(QByteArray response) { QHash results; @@ -78,6 +78,12 @@ QHash AiEngineGimbalServerSerialResponse::getResponceValues(Q uint16_t height = ((uint8_t) response.at(13) << 8) | (uint8_t) response.at(12); results.insert("width", width); results.insert("height", height); + } else if (command == 0x02) { + QByteArray hidArray; + hidArray.append((char)response.at(8)); + hidArray.append((char)response.at(9)); + uint8_t hid = static_cast(hidArray.toUShort(nullptr, 16)); + results.insert("hardware_id", hid); } else { qWarning().noquote().nospace() << "Getting responce values not implemented yet for command " << QString("0x%1").arg(command, 2, 16, QLatin1Char('0')); QString responseStr; diff --git a/misc/rtsp_ai_player/aienginegimbalserverserialresponse.h b/misc/rtsp_ai_player/aienginegimbalserverudpresponse.h similarity index 74% rename from misc/rtsp_ai_player/aienginegimbalserverserialresponse.h rename to misc/rtsp_ai_player/aienginegimbalserverudpresponse.h index 32fc5a3..85d934e 100644 --- a/misc/rtsp_ai_player/aienginegimbalserverserialresponse.h +++ b/misc/rtsp_ai_player/aienginegimbalserverudpresponse.h @@ -13,11 +13,11 @@ enum MESSAGE_IDX { STX = 0, CTRL = 2, Data_len = 3, SEQ = 5, CMD_ID = 7, DATA = 8 }; -class AiEngineGimbalServerSerialResponse : public QObject +class AiEngineGimbalServerUDPResponse : public QObject { Q_OBJECT public: - explicit AiEngineGimbalServerSerialResponse(QObject *parent = nullptr); + explicit AiEngineGimbalServerUDPResponse(QObject *parent = nullptr); public: QHash getResponceValues(QByteArray response);