diff --git a/misc/rtsp_ai_player/aienginedefinitions.h b/misc/rtsp_ai_player/aienginedefinitions.h index 2b70c05..2f6e0e2 100644 --- a/misc/rtsp_ai_player/aienginedefinitions.h +++ b/misc/rtsp_ai_player/aienginedefinitions.h @@ -1,5 +1,10 @@ #pragma once +#include +#include +#include "aienginegimbalserverdefines.h" + + // Common geoposition typedef struct { float lat; @@ -48,3 +53,34 @@ typedef struct { float pitch; float zoom; } AiEngineCameraPosition; + + +struct AiEngineRectangleProperties +{ + int width; + int height; + int middleX; + int middleY; +}; + + +typedef struct { + // Get these only once + uint resolutionX; + uint resolutionY; + float maxZoom; + + // Update these before every command + float currentYaw; + float currentPitch; + float currentRoll; + float currentZoom; +} AiEngineGimbalStatus; + + +struct AiEngineServerSerialCommandStructure +{ + SERIAL_COMMAND_ID id; + QByteArray command; + QString description; +}; diff --git a/misc/rtsp_ai_player/aienginegimbalserver.cpp b/misc/rtsp_ai_player/aienginegimbalserver.cpp index dcbfa31..44b03a7 100644 --- a/misc/rtsp_ai_player/aienginegimbalserver.cpp +++ b/misc/rtsp_ai_player/aienginegimbalserver.cpp @@ -1,31 +1,75 @@ #include +#include #include "aienginegimbalserver.h" +#include "aienginegimbalserveractions.h" + AiEngineGimbalServer::AiEngineGimbalServer(QObject *parent) : QObject{parent} { - mSerialPort = new QSerialPort(this); // TODO!! Setup and use serial port.... + mIsAvailable = true; + mActions.setup(&mSerialPort, &mSerialCommand, &mSerialResponse, &mGimbalStatus); + + connect(&mActions, &AiEngineGimbalServerActions::aiTargetZoomed, this, &AiEngineGimbalServer::aiTargetZoomed); } // TODO!! Client doesn't really send any signal yet to this slot. -void AiEngineGimbalServer::dronePositionSlot(AiEngineDronePosition position) +void AiEngineGimbalServer::dronePositionSlot(AiEngineDronePosition dronePosition) { qDebug() << "AiEngineGimbalServer::dronePositionSlot() Server got new drone position:" - << position.position.lat - << position.position.lon - << position.position.alt; + << dronePosition.position.lat + << dronePosition.position.lon + << dronePosition.position.alt + << dronePosition.pitch + << dronePosition.yaw; + + mDronePosition = dronePosition; } // This is actually called from the client. void AiEngineGimbalServer::zoomToAiTargetSlot(AiEngineCameraTarget target) { - qDebug() << "AiEngineGimbalServer::zoomToAiTargetSlot() Move camera to the new target:" - << "index:" << target.index - << "pos:" << target.rectangle.top << target.rectangle.left << target.rectangle.bottom << target.rectangle.right; + if (mIsAvailable == true) { + mIsAvailable = false; + qDebug() << "AiEngineGimbalServer::zoomToAiTargetSlot() Move camera to the new target:" + << "index:" << target.index + << "pos:" << target.rectangle.top << target.rectangle.left << target.rectangle.bottom << target.rectangle.right; + + // 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); + + // Turn + mActions.turnToTarget(rectangle); + + // Calculate location + int delay1 = 1000; // Adjust this value as needed + AiEngineDronePosition dronePosition = mDronePosition; + int targetIndex = target.index; + QTimer::singleShot(delay1, this, [this, dronePosition, targetIndex]() { mActions.getLocation(dronePosition, targetIndex); }); + + // Zoom + int delay2 = delay1 + 100; // Adjust this value as needed + QTimer::singleShot(delay2, this, [this, rectangle]() { mActions.zoomToTarget(rectangle); }); + + // Return to previous position + int delay3 = delay2 + 10000; // Adjust this value as needed + AiEngineGimbalStatus gimbalStatus = mGimbalStatus; + 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; }); + } +} + + +bool AiEngineGimbalServer::isAvailable(void) +{ + return mIsAvailable; } diff --git a/misc/rtsp_ai_player/aienginegimbalserver.h b/misc/rtsp_ai_player/aienginegimbalserver.h index d5b73c7..b470191 100644 --- a/misc/rtsp_ai_player/aienginegimbalserver.h +++ b/misc/rtsp_ai_player/aienginegimbalserver.h @@ -1,14 +1,21 @@ #pragma once #include +#include #include #include "aienginedefinitions.h" +#include "aienginegimbalserveractions.h" +#include "aienginegimbalserverserialcommand.h" +#include "aienginegimbalserverserialport.h" +#include "aienginegimbalserverserialresponse.h" + class AiEngineGimbalServer : public QObject { Q_OBJECT public: explicit AiEngineGimbalServer(QObject *parent = nullptr); + bool isAvailable(void); public slots: void dronePositionSlot(AiEngineDronePosition); @@ -20,5 +27,11 @@ signals: void newCameraPosition(AiEngineCameraPosition); private: - QSerialPort *mSerialPort; + AiEngineDronePosition mDronePosition; + AiEngineGimbalServerSerialPort mSerialPort; + AiEngineGimbalServerSerialCommand mSerialCommand; + AiEngineGimbalServerSerialResponse mSerialResponse; + AiEngineGimbalStatus mGimbalStatus; + AiEngineGimbalServerActions mActions; + bool mIsAvailable; }; diff --git a/misc/rtsp_ai_player/aienginegimbalserveractions.cpp b/misc/rtsp_ai_player/aienginegimbalserveractions.cpp new file mode 100644 index 0000000..1d8709d --- /dev/null +++ b/misc/rtsp_ai_player/aienginegimbalserveractions.cpp @@ -0,0 +1,306 @@ +#include +#include "aienginegimbalserveractions.h" + + +AiEngineGimbalServerActions::AiEngineGimbalServerActions(QObject *parent) + : QObject{parent} +{ + +} + + +void AiEngineGimbalServerActions::setup(AiEngineGimbalServerSerialPort *serialPort, AiEngineGimbalServerSerialCommand *serialCommand, AiEngineGimbalServerSerialResponse *serialResponse, AiEngineGimbalStatus *gimbalStatus) +{ + mSerialPort = serialPort; + mSerialCommand = serialCommand; + mSerialResponse = serialResponse; + mGimbalStatus = gimbalStatus; + + // Set initial position and update status + QByteArray tempCommand; + QByteArray tempResponse; + QHash responseValues; + + // 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); + 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(); + + // Go to initial orientation + tempCommand = mSerialCommand->getCommand(SERIAL_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); + + // Go to initial zoom + tempCommand = mSerialCommand->getCommand(SERIAL_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); + + 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; +} + + +AiEngineRectangleProperties AiEngineGimbalServerActions::calculateRectangleProperties(int top, int left, int bottom, int right) { + // Sanity check + // top cannot be greater than bottom + // left cannot be greater than right + if (top > bottom) { + int temp = top; + top = bottom; + bottom = temp; + qWarning().noquote().nospace() << "calculateRectangleProperties(): top and bottom mixed?"; + } + if (left > right) { + int temp = left; + left = right; + right = temp; + qWarning().noquote().nospace() << "calculateRectangleProperties(): left and right mixed?"; + } + + AiEngineRectangleProperties properties; + properties.width = right - left; + properties.height = bottom - top; + properties.middleX = static_cast(left + properties.width / 2); + properties.middleY = static_cast(top + properties.height / 2); + + // 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; + qWarning().noquote().nospace() << "calculateRectangleProperties(): Something was zero -> No zoom, no turn!"; + } + + return properties; +} + + +void AiEngineGimbalServerActions::getAnglesToOnScreenTarget(int targetX, int targetY, float &resultYaw, float &resultPitch) +{ + // Get current yaw and pitch + resultYaw = mGimbalStatus->currentYaw; + resultPitch = mGimbalStatus->currentPitch; + + // Normalize target pixel location to [-0.5, 0.5] range + float normPixelX = (targetX - mGimbalStatus->resolutionX / 2.0f) / (mGimbalStatus->resolutionX / 2.0f); + 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); + + // 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; + + // 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; + + // Make alterations to current angles + resultYaw -= turnX; + resultPitch -= turnY; +} + + +void AiEngineGimbalServerActions::turnToTarget(AiEngineRectangleProperties rectangle) +{ + qDebug().noquote().nospace() << "Turning to target"; + + float resultYaw = 0.0f; + float resultPitch = 0.0f; + getAnglesToOnScreenTarget(rectangle.middleX, rectangle.middleY, resultYaw, resultPitch); + + QByteArray serialCommandTurn = mSerialCommand->getCommand(SERIAL_COMMAND_ID::TURN_TO_PIXEL); + + int16_t degreesVal = resultYaw * 10; + serialCommandTurn[8] = degreesVal & 0xFF; + serialCommandTurn[9] = degreesVal >> 8; + + degreesVal = resultPitch * 10; + serialCommandTurn[10] = degreesVal & 0xFF; + serialCommandTurn[11] = degreesVal >> 8; + + mSerialPort->sendCommand(serialCommandTurn); +} + + +void AiEngineGimbalServerActions::zoomToTarget(AiEngineRectangleProperties rectangle) +{ + 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; + } + + QByteArray serialCommandNewZoom = mSerialCommand->getCommand(SERIAL_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; + + mSerialPort->sendCommand(serialCommandNewZoom); +} + + +void AiEngineGimbalServerActions::getLocation(AiEngineDronePosition dronePosition, int targetIndex) +{ + // From the drone and camera + CameraData cameraData = getCameraData(); + GPSData gpsData = {dronePosition.position.alt, dronePosition.position.lat, dronePosition.position.lon}; + DroneData droneData = {gpsData, dronePosition.yaw, dronePosition.pitch, 0.0f /* Roll */}; // GPS (latitude, longitude, altitude) and heading + + // Calculate the GPS location of the target + AiEngineGeoPosition targetPosition = calculateTargetLocation(droneData, cameraData); + + // TODO: Send position + AiEngineTargetPosition targetReturn; + targetReturn.position = targetPosition; + targetReturn.targetIndex = targetIndex; + emit aiTargetZoomed(targetReturn); +} + + +void AiEngineGimbalServerActions::restoreOrientationAndZoom(AiEngineGimbalStatus gimbalStatus) +{ + QByteArray tempCommand; + + // Go to initial orientation + tempCommand = mSerialCommand->getCommand(SERIAL_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); + + // Go to initial zoom + tempCommand = mSerialCommand->getCommand(SERIAL_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); + + // TODO: Maybe send signal that all done? +} + + +CameraData AiEngineGimbalServerActions::getCameraData() +{ + uint16_t height = mGimbalStatus->resolutionY; + 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}; +} + + +// Function to calculate the new GPS location +AiEngineGeoPosition AiEngineGimbalServerActions::calculateTargetLocation(DroneData drone, CameraData camera) +{ + // Calculate altitude and distance to the target + //float targetDistance = calculateTargetDistanceFromTargetSize(targetTrueSize, targetPixelSize, cameraData.width, degreesToRadians(cameraData.fow)); + 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); + float sinPitchRad = std::sin(pitchRad); + float altitudeDifference = std::round(slantDistance * sinPitchRad * 100.0) / 100.0; // Rounding to avoid weird targetAltitude + float targetAltitude = (std::round(drone.gps.altitude * 100.0) / 100.0) - altitudeDifference; // Rounding to avoid weird targetAltitude + + // Calculate the bearing from the drone orientation and camera orientation + float targetBearing = std::fmod(drone.yaw + camera.yaw, 360.0f); + + // Convert bearing and drone's latitude/longitude to radians + float bearingRad = degreesToRadians(targetBearing); + float latRad = degreesToRadians(drone.gps.latitude); + float lonRad = degreesToRadians(drone.gps.longitude); + + // Calculate new latitude using Haversine formula + float newLatRad = std::asin(std::sin(latRad) * std::cos(horizontalDistance / EARTH_RADIUS) + std::cos(latRad) * std::sin(horizontalDistance / EARTH_RADIUS) * std::cos(bearingRad)); + + // Calculate new longitude using Haversine formula + float newLonRad = lonRad + std::atan2(std::sin(bearingRad) * std::sin(horizontalDistance / EARTH_RADIUS) * std::cos(latRad), std::cos(horizontalDistance / EARTH_RADIUS) - std::sin(latRad) * std::sin(newLatRad)); + + // Convert back to degrees for latitude and longitude + AiEngineGeoPosition newLocation; + newLocation.alt = targetAltitude; + newLocation.lat = newLatRad * 180.0f / static_cast(M_PI); + newLocation.lon = newLonRad * 180.0f / static_cast(M_PI); + + return newLocation; +} + + +void AiEngineGimbalServerActions::calculateDistancesToTarget(float altitude, float cameraPitch, float &slantDistance, float &horizontalDistance) +{ + // Convert pitch angle from degrees to radians + float cameraPitchRadians = degreesToRadians(cameraPitch); + + // Value has to be between 0-90 + cameraPitchRadians = std::clamp((double)cameraPitchRadians, (double)0.0f, M_PI_2); + + // Calculate horizontal distance + horizontalDistance = altitude / tan(cameraPitchRadians); + + // Adjust for Earth's curvature: + // We need to find the horizontal distance on the curved Earth surface that corresponds to this flat distance + double centralAngle = horizontalDistance / EARTH_RADIUS; // in radians + + // Calculate the arc length on Earth's surface + horizontalDistance = EARTH_RADIUS * centralAngle; + + // Calculate slant distance considering the Earth's curvature + slantDistance = sqrt(altitude * altitude + horizontalDistance * horizontalDistance); +} + + +// Function to convert degrees to radians +float AiEngineGimbalServerActions::degreesToRadians(float degrees) +{ + return degrees * M_PI / 180.0f; +} diff --git a/misc/rtsp_ai_player/aienginegimbalserveractions.h b/misc/rtsp_ai_player/aienginegimbalserveractions.h new file mode 100644 index 0000000..0905b72 --- /dev/null +++ b/misc/rtsp_ai_player/aienginegimbalserveractions.h @@ -0,0 +1,68 @@ +#pragma once + +#include +#include "aienginedefinitions.h" +#include "aienginegimbalserverserialcommand.h" +#include "aienginegimbalserverserialport.h" +#include "aienginegimbalserverserialresponse.h" + + +const double EARTH_RADIUS = 6371000.0; // Earth's radius in meters + + +struct GPSData +{ + float altitude; // Meters + float latitude; // Decimal degrees + float longitude; // Decimal degrees +}; + + +struct CameraData +{ + uint16_t height; // Pixels + uint16_t width; // Pixels + float pitch; // Degrees + float yaw; // Degrees + float fow; // Degrees +}; + + +struct DroneData +{ + GPSData gps; + float yaw; // Degrees + float pitch; // Degrees + float roll; // Degrees +}; + + +class AiEngineGimbalServerActions : public QObject +{ + Q_OBJECT +public: + explicit AiEngineGimbalServerActions(QObject *parent = nullptr); + +public: + void setup(AiEngineGimbalServerSerialPort *serialPort, AiEngineGimbalServerSerialCommand *serialCommand, AiEngineGimbalServerSerialResponse *serialResponse, AiEngineGimbalStatus *gimbalStatus); + AiEngineRectangleProperties calculateRectangleProperties(int top, int left, int bottom, int right); + void turnToTarget(AiEngineRectangleProperties rectangle); + void zoomToTarget(AiEngineRectangleProperties rectangle); + void getLocation(AiEngineDronePosition dronePosition, int targetIndex); + void restoreOrientationAndZoom(AiEngineGimbalStatus gimbalStatus); + +signals: + void aiTargetZoomed(AiEngineTargetPosition); + +private: + AiEngineGimbalServerSerialPort *mSerialPort; + AiEngineGimbalServerSerialCommand *mSerialCommand; + AiEngineGimbalServerSerialResponse *mSerialResponse; + AiEngineGimbalStatus *mGimbalStatus; + + CameraData getCameraData(void); + void getAnglesToOnScreenTarget(int targetX, int targetY, float &resultYaw, float &resultPitch); + AiEngineGeoPosition calculateTargetLocation(DroneData drone, CameraData camera); + void calculateDistancesToTarget(float altitude, float cameraPitch, float &slantDistance, float &horizontalDistance); + float degreesToRadians(float degrees); +}; diff --git a/misc/rtsp_ai_player/aienginegimbalservercrc16.cpp b/misc/rtsp_ai_player/aienginegimbalservercrc16.cpp new file mode 100644 index 0000000..1614d36 --- /dev/null +++ b/misc/rtsp_ai_player/aienginegimbalservercrc16.cpp @@ -0,0 +1,31 @@ +#include "aienginegimbalservercrc16.h" + + +static const uint16_t crc16Table[256] = {0x0, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7, 0x8108, 0x9129, 0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef, 0x1231, 0x210, 0x3273, 0x2252, 0x52b5, 0x4294, 0x72f7, 0x62d6, 0x9339, 0x8318, 0xb37b, 0xa35a, 0xd3bd, 0xc39c, 0xf3ff, 0xe3de, 0x2462, 0x3443, 0x420, 0x1401, 0x64e6, 0x74c7, 0x44a4, 0x5485, 0xa56a, 0xb54b, 0x8528, 0x9509, 0xe5ee, 0xf5cf, 0xc5ac, 0xd58d, 0x3653, 0x2672, 0x1611, 0x630, + 0x76d7, 0x66f6, 0x5695, 0x46b4, 0xb75b, 0xa77a, 0x9719, 0x8738, 0xf7df, 0xe7fe, 0xd79d, 0xc7bc, 0x48c4, 0x58e5, 0x6886, 0x78a7, 0x840, 0x1861, 0x2802, 0x3823, 0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, 0x9969, 0xa90a, 0xb92b, 0x5af5, 0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0xa50, 0x3a33, 0x2a12, 0xdbfd, 0xcbdc, 0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b, 0xab1a, 0x6ca6, 0x7c87, 0x4ce4, 0x5cc5, 0x2c22, 0x3c03, 0xc60, 0x1c41, + 0xedae, 0xfd8f, 0xcdec, 0xddcd, 0xad2a, 0xbd0b, 0x8d68, 0x9d49, 0x7e97, 0x6eb6, 0x5ed5, 0x4ef4, 0x3e13, 0x2e32, 0x1e51, 0xe70, 0xff9f, 0xefbe, 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a, 0x9f59, 0x8f78, 0x9188, 0x81a9, 0xb1ca, 0xa1eb, 0xd10c, 0xc12d, 0xf14e, 0xe16f, 0x1080, 0xa1, 0x30c2, 0x20e3, 0x5004, 0x4025, 0x7046, 0x6067, 0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, 0xd31c, 0xe37f, 0xf35e, 0x2b1, 0x1290, 0x22f3, 0x32d2, + 0x4235, 0x5214, 0x6277, 0x7256, 0xb5ea, 0xa5cb, 0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c, 0xc50d, 0x34e2, 0x24c3, 0x14a0, 0x481, 0x7466, 0x6447, 0x5424, 0x4405, 0xa7db, 0xb7fa, 0x8799, 0x97b8, 0xe75f, 0xf77e, 0xc71d, 0xd73c, 0x26d3, 0x36f2, 0x691, 0x16b0, 0x6657, 0x7676, 0x4615, 0x5634, 0xd94c, 0xc96d, 0xf90e, 0xe92f, 0x99c8, 0x89e9, 0xb98a, 0xa9ab, 0x5844, 0x4865, 0x7806, 0x6827, 0x18c0, 0x8e1, 0x3882, 0x28a3, + 0xcb7d, 0xdb5c, 0xeb3f, 0xfb1e, 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a, 0x4a75, 0x5a54, 0x6a37, 0x7a16, 0xaf1, 0x1ad0, 0x2ab3, 0x3a92, 0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b, 0x9de8, 0x8dc9, 0x7c26, 0x6c07, 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0xcc1, 0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8, 0x6e17, 0x7e36, 0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0xed1, 0x1ef0}; + + +uint16_t AiEngineGimbalServerCrc16::calculateCRC(const uint8_t *ptr, uint32_t len, uint16_t crcInit) +{ + uint16_t crc = crcInit; + uint8_t temp; + + while (len-- != 0) { + temp = (crc >> 8) & 0xFF; + crc = (crc << 8) ^ crc16Table[*ptr ^ temp]; + ptr++; + } + + return crc; +} + + +void AiEngineGimbalServerCrc16::getCRCBytes(const QByteArray &data, int8_t *bytes) +{ + uint16_t crc16 = calculateCRC(reinterpret_cast(data.constData()), data.size(), 0); + bytes[0] = crc16 & 0xFF; // Set LSB + bytes[1] = crc16 >> 8; // Set MSB +} diff --git a/misc/rtsp_ai_player/aienginegimbalservercrc16.h b/misc/rtsp_ai_player/aienginegimbalservercrc16.h new file mode 100644 index 0000000..497f883 --- /dev/null +++ b/misc/rtsp_ai_player/aienginegimbalservercrc16.h @@ -0,0 +1,14 @@ +#pragma once + +#include +#include + + +class AiEngineGimbalServerCrc16 +{ +public: + static void getCRCBytes(const QByteArray &data, int8_t *bytes); + +private: + static uint16_t calculateCRC(const uint8_t *ptr, uint32_t len, uint16_t crcInit); +}; diff --git a/misc/rtsp_ai_player/aienginegimbalserverdefines.h b/misc/rtsp_ai_player/aienginegimbalserverdefines.h new file mode 100644 index 0000000..6787527 --- /dev/null +++ b/misc/rtsp_ai_player/aienginegimbalserverdefines.h @@ -0,0 +1,64 @@ +/** + * This is a defines header for Siyi Gimbal Cameras. + * Other cameras might need their own defines header. + */ + +#pragma once + + +#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_ROLL 0.0f +#define AI_ENGINE_GIMBAL_INITIAL_YAW 0.0f + + +enum SERIAL_COMMAND_ID { + TURN_TO_DEGREES = 1, + TURN_TO_PIXEL, + ZOOM_TO_X, + ACQUIRE_CAMERA_CODEC_SPECS, + ACQUIRE_CURRENT_ZOOM, + ACQUIRE_ATTITUDE_DATA, + AUTO_CENTER, + ZOOM_MOST, + ZOOM_LEAST, + FOCUS_MOST, + FOCUS_LEAST, + FOCUS_AUTO, + ROTATE_UP, + ROTATE_DOWN, + ROTATE_RIGHT, + ROTATE_LEFT, + ROTATE_STOP, + ACQUIRE_MAX_ZOOM_VALUE, + TAKE_PICTURES, + TAKE_VIDEO, + ROTATE_100_100, + ACQUIRE_GIMBAL_STATUS, + ACQUIRE_HW_INFO, + ACQUIRE_FIRMWARE_VERSION, + MODE_LOCK, + MODE_FOLLOW, + MODE_FPV, + ENABLE_HDMI, + ENABLE_CVBS, + DISABLE_HDMI_CVBS, + ACQUIRE_RANGE_DATA, + RUN_TARGET_LOCATION_TEST +}; diff --git a/misc/rtsp_ai_player/aienginegimbalserverserialcommand.cpp b/misc/rtsp_ai_player/aienginegimbalserverserialcommand.cpp new file mode 100644 index 0000000..c39b8ed --- /dev/null +++ b/misc/rtsp_ai_player/aienginegimbalserverserialcommand.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 "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/aienginegimbalserverserialcommand.h b/misc/rtsp_ai_player/aienginegimbalserverserialcommand.h new file mode 100644 index 0000000..9fef803 --- /dev/null +++ b/misc/rtsp_ai_player/aienginegimbalserverserialcommand.h @@ -0,0 +1,27 @@ +/** + * This is a serial command class for Siyi Gimbal Cameras. + * Other cameras might need their own serial command class. + */ + +#pragma once + +#include +#include +#include +#include +#include "aienginedefinitions.h" + +class AiEngineGimbalServerSerialCommand : public QObject +{ + Q_OBJECT +public: + explicit AiEngineGimbalServerSerialCommand(QObject *parent = nullptr); + +public: + QByteArray getCommand(SERIAL_COMMAND_ID commandId); + +private: + QByteArray createByteArray(const std::initializer_list &bytes); + int getCommandIndex(SERIAL_COMMAND_ID commandId); + std::vector mSerialCommands; +}; diff --git a/misc/rtsp_ai_player/aienginegimbalserverserialport.cpp b/misc/rtsp_ai_player/aienginegimbalserverserialport.cpp new file mode 100644 index 0000000..b737b4e --- /dev/null +++ b/misc/rtsp_ai_player/aienginegimbalserverserialport.cpp @@ -0,0 +1,92 @@ +#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 new file mode 100644 index 0000000..2600c62 --- /dev/null +++ b/misc/rtsp_ai_player/aienginegimbalserverserialport.h @@ -0,0 +1,25 @@ +#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/aienginegimbalserverserialresponse.cpp b/misc/rtsp_ai_player/aienginegimbalserverserialresponse.cpp new file mode 100644 index 0000000..58bcb9e --- /dev/null +++ b/misc/rtsp_ai_player/aienginegimbalserverserialresponse.cpp @@ -0,0 +1,95 @@ +/** + * This is a serial response class for Siyi Gimbal Cameras. + * Other cameras might need their own serial response class. + */ + +#include +#include +#include +#include +#include "aienginegimbalservercrc16.h" +#include "aienginegimbalserverserialresponse.h" + + +AiEngineGimbalServerSerialResponse::AiEngineGimbalServerSerialResponse(QObject *parent) + : QObject{parent} +{ + +} + +QHash AiEngineGimbalServerSerialResponse::getResponceValues(QByteArray response) +{ + QHash results; + + if (response.size() == 0) { + qWarning().noquote().nospace() << "Response is empty, exiting..."; + return results; + } + + // Check response data validity + int8_t crcCheck[2]; + uint8_t desiredLength = response.size() - 2; + QByteArray subData(response.data(), desiredLength); + AiEngineGimbalServerCrc16::getCRCBytes(subData, crcCheck); + + int8_t crcOriginal[2]; + crcOriginal[0] = response.at(response.size() - 2); + crcOriginal[1] = response.at(response.size() - 1); + + // Data not OK + if (crcCheck[0] != crcOriginal[0] || crcCheck[1] != crcOriginal[1]) { + qWarning().noquote().nospace() << "Response data INVALID"; + QString responseCRC = QString("0x%1,0x%2").arg(crcOriginal[0], 2, 16, QLatin1Char('0')).arg(crcOriginal[1], 2, 16, QLatin1Char('0')).toUpper(); + QString recalcCRC = QString("0x%1,0x%2").arg(crcCheck[0], 2, 16, QLatin1Char('0')).arg(crcCheck[1], 2, 16, QLatin1Char('0')).toUpper(); + qWarning().noquote().nospace() << responseCRC << "!=" << recalcCRC; + } + + uint8_t command = response.at(MESSAGE_IDX::CMD_ID); + + if (command == 0x0E) { + int16_t yaw = ((uint8_t) response.at(9) << 8) | (uint8_t) response.at(8); + int16_t pitch = ((uint8_t) response.at(11) << 8) | (uint8_t) response.at(10); + int16_t roll = ((uint8_t) response.at(13) << 8) | (uint8_t) response.at(12); + results.insert("yaw", (float) (yaw / 10)); + results.insert("pitch", (float) (pitch / 10)); + results.insert("roll", (float) (roll / 10)); + } else if (command == 0x0D) { + int16_t yaw = ((uint8_t) response.at(9) << 8) | (uint8_t) response.at(8); + int16_t pitch = ((uint8_t) response.at(11) << 8) | (uint8_t) response.at(10); + int16_t roll = ((uint8_t) response.at(13) << 8) | (uint8_t) response.at(12); + int16_t yawSpeed = ((uint8_t) response.at(15) << 8) | (uint8_t) response.at(14); + int16_t pitchSpeed = ((uint8_t) response.at(17) << 8) | (uint8_t) response.at(16); + int16_t rollSpeed = ((uint8_t) response.at(19) << 8) | (uint8_t) response.at(18); + results.insert("yaw", (float) (yaw / 10)); + results.insert("pitch", (float) (pitch / 10)); + results.insert("roll", (float) (roll / 10)); + results.insert("yaw_speed", (float) (yawSpeed / 10)); + results.insert("pitch_speed", (float) (pitchSpeed / 10)); + results.insert("roll_speed", (float) (rollSpeed / 10)); + } else if (command == 0x0F) { + int8_t zoom = (int8_t) response.at(8); + results.insert("zoom", zoom); + } else if (command == 0x16 || command == 0x18) { + float zoomInt = (float) response.at(8); + float zoomFloat = (float) ((float) response.at(9) / 10); + results.insert("zoom", zoomInt + zoomFloat); + } else if (command == 0x20) { + uint16_t width = ((uint8_t) response.at(11) << 8) | (uint8_t) response.at(10); + uint16_t height = ((uint8_t) response.at(13) << 8) | (uint8_t) response.at(12); + results.insert("width", width); + results.insert("height", height); + } else { + qWarning().noquote().nospace() << "Getting responce values not implemented yet for command " << QString("0x%1").arg(command, 2, 16, QLatin1Char('0')); + QString responseStr; + for (int i = 0; i < response.size(); i++) { + if (i > 0) { + responseStr += ","; + } + responseStr += QString("0x%1").arg(response.at(i), 2, 16, QChar('0')).toUpper(); + responseStr.replace("0X", "0x"); + } + qWarning().noquote().nospace() << "Responce byte array: " << responseStr; + } + + return results; +} diff --git a/misc/rtsp_ai_player/aienginegimbalserverserialresponse.h b/misc/rtsp_ai_player/aienginegimbalserverserialresponse.h new file mode 100644 index 0000000..32fc5a3 --- /dev/null +++ b/misc/rtsp_ai_player/aienginegimbalserverserialresponse.h @@ -0,0 +1,24 @@ +/** + * This is a serial response class for Siyi Gimbal Cameras. + * Other cameras might need their own serial response class. + */ + +#pragma once + +#include +#include +#include + + +enum MESSAGE_IDX { STX = 0, CTRL = 2, Data_len = 3, SEQ = 5, CMD_ID = 7, DATA = 8 }; + + +class AiEngineGimbalServerSerialResponse : public QObject +{ + Q_OBJECT +public: + explicit AiEngineGimbalServerSerialResponse(QObject *parent = nullptr); + +public: + QHash getResponceValues(QByteArray response); +}; diff --git a/misc/rtsp_ai_player/rtsp_ai_player.pro b/misc/rtsp_ai_player/rtsp_ai_player.pro index ce2b477..70e6c53 100644 --- a/misc/rtsp_ai_player/rtsp_ai_player.pro +++ b/misc/rtsp_ai_player/rtsp_ai_player.pro @@ -6,8 +6,7 @@ MOC_DIR = moc OBJECTS_DIR = obj SOURCES = $$PWD/*.cpp $$PWD/../../misc/camera/a8_remote/remoteControl.cpp -HEADERS = $$PWD/*.h \ - aienginedefinitions.h +HEADERS = $$PWD/*.h INCLUDEPATH += $$PWD/../../misc/camera/a8_remote opi5 {