mirror of
https://github.com/azaion/autopilot.git
synced 2026-04-22 23:36:34 +00:00
Changed serial connection to UDP connection.
Added ZR10 support. Added automation to choose configuration.
This commit is contained in:
Nffj84
@@ -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);
|
||||
|
||||
Reference in New Issue
Block a user