mirror of
https://github.com/azaion/autopilot.git
synced 2026-04-22 07:16:34 +00:00
Changed serial connection to UDP connection.
Added ZR10 support. Added automation to choose configuration.
This commit is contained in:
Nffj84
@@ -3,7 +3,7 @@
|
||||
#include <QString>
|
||||
|
||||
#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
|
||||
|
||||
@@ -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;
|
||||
};
|
||||
|
||||
@@ -8,8 +8,22 @@ AiEngineGimbalServer::AiEngineGimbalServer(QObject *parent)
|
||||
: QObject{parent}
|
||||
{
|
||||
// TODO!! Setup and use serial port....
|
||||
mIsAvailable = false;
|
||||
qDebug() << "Initial is available: " << mIsAvailable;
|
||||
|
||||
QTimer::singleShot(5000, this, [this]() {
|
||||
mIsAvailable = true;
|
||||
mActions.setup(&mSerialPort, &mSerialCommand, &mSerialResponse, &mGimbalStatus);
|
||||
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;
|
||||
}
|
||||
|
||||
@@ -2,12 +2,11 @@
|
||||
|
||||
#include <QObject>
|
||||
#include <QMap>
|
||||
#include <QSerialPort>
|
||||
#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;
|
||||
};
|
||||
|
||||
@@ -1,5 +1,7 @@
|
||||
#include <QUdpSocket>
|
||||
#include <QVariant>
|
||||
#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<QString, QVariant> 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<uint8_t>(AI_ENGINE_CAMERA_INITIAL_ZOOM);
|
||||
float fractionalPart = AI_ENGINE_CAMERA_INITIAL_ZOOM - integerPart;
|
||||
uint8_t scaledFractional = uint8_t(fractionalPart * 10);
|
||||
tempCommand[8] = integerPart;
|
||||
tempCommand[9] = scaledFractional;
|
||||
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<uint8_t>(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<uint8_t>(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);
|
||||
|
||||
@@ -2,9 +2,9 @@
|
||||
|
||||
#include <QDebug>
|
||||
#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);
|
||||
|
||||
@@ -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,
|
||||
|
||||
@@ -1,102 +0,0 @@
|
||||
/**
|
||||
* This is a serial command class for Siyi Gimbal Cameras.
|
||||
* Other cameras might need their own serial command class.
|
||||
*/
|
||||
|
||||
#include <QDebug>
|
||||
#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<int> &bytes)
|
||||
{
|
||||
QByteArray byteArray;
|
||||
for (int byte : bytes) {
|
||||
byteArray.append(static_cast<char>(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;
|
||||
}
|
||||
@@ -1,92 +0,0 @@
|
||||
#include <QDebug>
|
||||
#include <QTimer>
|
||||
#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;
|
||||
}
|
||||
@@ -1,25 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <QByteArray>
|
||||
#include <QObject>
|
||||
#include <QSerialPort>
|
||||
|
||||
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();
|
||||
};
|
||||
@@ -0,0 +1,73 @@
|
||||
#include <QDebug>
|
||||
#include <QTimer>
|
||||
#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;
|
||||
}
|
||||
@@ -0,0 +1,28 @@
|
||||
#pragma once
|
||||
|
||||
|
||||
#include <QObject>
|
||||
#include <QUdpSocket>
|
||||
|
||||
|
||||
#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;
|
||||
};
|
||||
@@ -0,0 +1,102 @@
|
||||
/**
|
||||
* This is a serial command class for Siyi Gimbal Cameras.
|
||||
* Other cameras might need their own serial command class.
|
||||
*/
|
||||
|
||||
#include <QDebug>
|
||||
#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<int> &bytes)
|
||||
{
|
||||
QByteArray byteArray;
|
||||
for (int byte : bytes) {
|
||||
byteArray.append(static_cast<char>(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;
|
||||
}
|
||||
+4
-4
@@ -11,17 +11,17 @@
|
||||
#include <QString>
|
||||
#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<int> &bytes);
|
||||
int getCommandIndex(SERIAL_COMMAND_ID commandId);
|
||||
int getCommandIndex(UDP_COMMAND_ID commandId);
|
||||
std::vector<AiEngineServerSerialCommandStructure> mSerialCommands;
|
||||
};
|
||||
+9
-3
@@ -8,16 +8,16 @@
|
||||
#include <QString>
|
||||
#include <QVariant>
|
||||
#include "aienginegimbalservercrc16.h"
|
||||
#include "aienginegimbalserverserialresponse.h"
|
||||
#include "aienginegimbalserverudpresponse.h"
|
||||
|
||||
|
||||
AiEngineGimbalServerSerialResponse::AiEngineGimbalServerSerialResponse(QObject *parent)
|
||||
AiEngineGimbalServerUDPResponse::AiEngineGimbalServerUDPResponse(QObject *parent)
|
||||
: QObject{parent}
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
QHash<QString, QVariant> AiEngineGimbalServerSerialResponse::getResponceValues(QByteArray response)
|
||||
QHash<QString, QVariant> AiEngineGimbalServerUDPResponse::getResponceValues(QByteArray response)
|
||||
{
|
||||
QHash<QString, QVariant> results;
|
||||
|
||||
@@ -78,6 +78,12 @@ QHash<QString, QVariant> 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<uint8_t>(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;
|
||||
+2
-2
@@ -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<QString, QVariant> getResponceValues(QByteArray response);
|
||||
Reference in New Issue
Block a user