azaion/autopilot
1
0
Fork 0
mirror of https://github.com/azaion/autopilot.git synced 2026-04-22 08:36:33 +00:00
Code Issues Packages Projects Releases Wiki Activity

Set camera ready for lift and drop

Browse Source 
Added functionality to set camera ready for bringing it down or up.
Camera will be made available for AI after bringCameraDown command is given via UDPSocket.
Camera will be made unavailable for AI after bringCameraUp command is given via UDPSocket.
This commit is contained in:
Nffj84
2025-03-24 18:01:47 +02:00
parent f1023788e5
commit deb607237e
4 changed files with 161 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ai_controller/aienginegimbalserveractions.cpp
+92 -70
View File
@@ -17,6 +17,7 @@ void AiEngineGimbalServerActions::setup(AiEngineGimbalServerUDP *udpSocket, AiEn
mUdpCommand = udpCommand;
mUdpResponse = udpResponse;
mGimbalStatus = gimbalStatus;
mAllowCameraCommands = false;
// Set initial position and update status
QByteArray tempCommand;
@@ -68,24 +69,8 @@ void AiEngineGimbalServerActions::setup(AiEngineGimbalServerUDP *udpSocket, AiEn
float maxZoom = responseValues["zoom"].toFloat();
mGimbalStatus->maxZoom = maxZoom > 10 ? 10 : maxZoom;
// Go to initial orientation
tempCommand = mUdpCommand->getCommand(UDP_COMMAND_ID::TURN_TO_DEGREES);
int16_t degreesVal = AI_ENGINE_GIMBAL_INITIAL_YAW * 10;
tempCommand[8] = degreesVal & 0xFF;
tempCommand[9] = degreesVal >> 8;
degreesVal = AI_ENGINE_GIMBAL_INITIAL_PITCH * 10;
tempCommand[10] = degreesVal & 0xFF;
tempCommand[11] = degreesVal >> 8;
mUdpSocket->sendCommand(tempCommand);
// Go to initial zoom
tempCommand = mUdpCommand->getCommand(UDP_COMMAND_ID::ZOOM_TO_X);
uint8_t integerPart = static_cast<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;
mUdpSocket->sendCommand(tempCommand);
// Go to initial orientation and zoom
goToInitialOrientation();
mGimbalStatus->currentPitch = AI_ENGINE_GIMBAL_INITIAL_PITCH;
mGimbalStatus->currentRoll = AI_ENGINE_GIMBAL_INITIAL_ROLL;
@@ -178,52 +163,56 @@ void AiEngineGimbalServerActions::getAnglesToOnScreenTarget(int targetX, int tar
void AiEngineGimbalServerActions::turnToTarget(AiEngineRectangleProperties rectangle)
{
qDebug().noquote().nospace() << "Turning to target";
if (mAllowCameraCommands == true) {
qDebug().noquote().nospace() << "Turning to target";
float resultYaw = 0.0f;
float resultPitch = 0.0f;
getAnglesToOnScreenTarget(rectangle.middleX, rectangle.middleY, resultYaw, resultPitch);
float resultYaw = 0.0f;
float resultPitch = 0.0f;
getAnglesToOnScreenTarget(rectangle.middleX, rectangle.middleY, resultYaw, resultPitch);
QByteArray serialCommandTurn = mUdpCommand->getCommand(UDP_COMMAND_ID::TURN_TO_PIXEL);
QByteArray serialCommandTurn = mUdpCommand->getCommand(UDP_COMMAND_ID::TURN_TO_PIXEL);
int16_t degreesVal = resultYaw * 10;
serialCommandTurn[8] = degreesVal & 0xFF;
serialCommandTurn[9] = degreesVal >> 8;
int16_t degreesVal = resultYaw * 10;
serialCommandTurn[8] = degreesVal & 0xFF;
serialCommandTurn[9] = degreesVal >> 8;
degreesVal = resultPitch * 10;
serialCommandTurn[10] = degreesVal & 0xFF;
serialCommandTurn[11] = degreesVal >> 8;
degreesVal = resultPitch * 10;
serialCommandTurn[10] = degreesVal & 0xFF;
serialCommandTurn[11] = degreesVal >> 8;
mUdpSocket->sendCommand(serialCommandTurn);
mUdpSocket->sendCommand(serialCommandTurn);
}
}
void AiEngineGimbalServerActions::zoomToTarget(AiEngineRectangleProperties rectangle)
{
qDebug().noquote().nospace() << "Zooming to target";
if (mAllowCameraCommands == true) {
qDebug().noquote().nospace() << "Zooming to target";
float fillRatio = 0.5;
float targetPixelWidth = rectangle.width;
float targetPixelHeight = rectangle.height;
float adjustedObjectWidth = targetPixelWidth / fillRatio;
float adjustedObjectHeight = targetPixelHeight / fillRatio;
float zoomWidth = static_cast<double>(mGimbalStatus->resolutionX) / adjustedObjectWidth;
float zoomHeight = static_cast<double>(mGimbalStatus->resolutionY) / adjustedObjectHeight;
float zoom = std::min(zoomWidth, zoomHeight);
if (zoom < 1.0f) {
zoom = 1.0f;
float fillRatio = 0.5;
float targetPixelWidth = rectangle.width;
float targetPixelHeight = rectangle.height;
float adjustedObjectWidth = targetPixelWidth / fillRatio;
float adjustedObjectHeight = targetPixelHeight / fillRatio;
float zoomWidth = static_cast<double>(mGimbalStatus->resolutionX) / adjustedObjectWidth;
float zoomHeight = static_cast<double>(mGimbalStatus->resolutionY) / adjustedObjectHeight;
float zoom = std::min(zoomWidth, zoomHeight);
if (zoom < 1.0f) {
zoom = 1.0f;
}
QByteArray serialCommandNewZoom = mUdpCommand->getCommand(UDP_COMMAND_ID::ZOOM_TO_X);
uint8_t integerPart = static_cast<uint8_t>(zoom);
float fractionalPart = zoom - integerPart;
uint8_t scaledFractional = uint8_t(fractionalPart * 10);
serialCommandNewZoom[8] = integerPart;
serialCommandNewZoom[9] = scaledFractional;
mUdpSocket->sendCommand(serialCommandNewZoom);
}
QByteArray serialCommandNewZoom = mUdpCommand->getCommand(UDP_COMMAND_ID::ZOOM_TO_X);
uint8_t integerPart = static_cast<uint8_t>(zoom);
float fractionalPart = zoom - integerPart;
uint8_t scaledFractional = uint8_t(fractionalPart * 10);
serialCommandNewZoom[8] = integerPart;
serialCommandNewZoom[9] = scaledFractional;
mUdpSocket->sendCommand(serialCommandNewZoom);
}
@@ -251,26 +240,28 @@ void AiEngineGimbalServerActions::getLocation(AiEngineDronePosition dronePositio
void AiEngineGimbalServerActions::restoreOrientationAndZoom(AiEngineGimbalStatus gimbalStatus)
{
QByteArray tempCommand;
if (mAllowCameraCommands == true) {
QByteArray tempCommand;
// Go to initial orientation
tempCommand = mUdpCommand->getCommand(UDP_COMMAND_ID::TURN_TO_DEGREES);
int16_t degreesVal = gimbalStatus.currentYaw * 10;
tempCommand[8] = degreesVal & 0xFF;
tempCommand[9] = degreesVal >> 8;
degreesVal = gimbalStatus.currentPitch * 10;
tempCommand[10] = degreesVal & 0xFF;
tempCommand[11] = degreesVal >> 8;
mUdpSocket->sendCommand(tempCommand);
// Go to initial orientation
tempCommand = mUdpCommand->getCommand(UDP_COMMAND_ID::TURN_TO_DEGREES);
int16_t degreesVal = gimbalStatus.currentYaw * 10;
tempCommand[8] = degreesVal & 0xFF;
tempCommand[9] = degreesVal >> 8;
degreesVal = gimbalStatus.currentPitch * 10;
tempCommand[10] = degreesVal & 0xFF;
tempCommand[11] = degreesVal >> 8;
mUdpSocket->sendCommand(tempCommand);
// Go to initial zoom
tempCommand = mUdpCommand->getCommand(UDP_COMMAND_ID::ZOOM_TO_X);
uint8_t integerPart = static_cast<uint8_t>(gimbalStatus.currentZoom);
float fractionalPart = gimbalStatus.currentZoom - integerPart;
uint8_t scaledFractional = uint8_t(fractionalPart * 10);
tempCommand[8] = integerPart;
tempCommand[9] = scaledFractional;
mUdpSocket->sendCommand(tempCommand);
// Go to initial zoom
tempCommand = mUdpCommand->getCommand(UDP_COMMAND_ID::ZOOM_TO_X);
uint8_t integerPart = static_cast<uint8_t>(gimbalStatus.currentZoom);
float fractionalPart = gimbalStatus.currentZoom - integerPart;
uint8_t scaledFractional = uint8_t(fractionalPart * 10);
tempCommand[8] = integerPart;
tempCommand[9] = scaledFractional;
mUdpSocket->sendCommand(tempCommand);
}
// TODO: Maybe send signal that all done?
}
@@ -354,3 +345,34 @@ float AiEngineGimbalServerActions::degreesToRadians(float degrees)
{
return degrees * M_PI / 180.0f;
}
void AiEngineGimbalServerActions::goToInitialOrientation(void)
{
// These camera commands should always be allowed
// Go to initial orientation
tempCommand = mUdpCommand->getCommand(UDP_COMMAND_ID::TURN_TO_DEGREES);
int16_t degreesVal = AI_ENGINE_GIMBAL_INITIAL_YAW * 10;
tempCommand[8] = degreesVal & 0xFF;
tempCommand[9] = degreesVal >> 8;
degreesVal = AI_ENGINE_GIMBAL_INITIAL_PITCH * 10;
tempCommand[10] = degreesVal & 0xFF;
tempCommand[11] = degreesVal >> 8;
mUdpSocket->sendCommand(tempCommand);
// Go to initial zoom
tempCommand = mUdpCommand->getCommand(UDP_COMMAND_ID::ZOOM_TO_X);
uint8_t integerPart = static_cast<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;
mUdpSocket->sendCommand(tempCommand);
}
void AiEngineGimbalServerActions::setAllowCameraCommands(bool allow)
{
mAllowCameraCommands = allow;
}