mirror of
https://github.com/azaion/autopilot.git
synced 2026-04-22 09:26:34 +00:00
Nffj84
104 lines
3.8 KiB
C++
104 lines
3.8 KiB
C++
#include <QDebug>
|
|
#include <QTimer>
|
|
#include "aienginegimbalserver.h"
|
|
#include "aienginegimbalserveractions.h"
|
|
|
|
|
|
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;
|
|
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);
|
|
}
|
|
|
|
|
|
// TODO!! Client doesn't really send any signal yet to this slot.
|
|
void AiEngineGimbalServer::dronePositionSlot(AiEngineDronePosition dronePosition)
|
|
{
|
|
qDebug() << "AiEngineGimbalServer::dronePositionSlot() Server got new drone position:"
|
|
<< 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() << "zoomToAiTargetSlot called";
|
|
if (mIsAvailable == true) {
|
|
mIsAvailable = false;
|
|
qDebug() << "Is available: " << mIsAvailable;
|
|
|
|
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);
|
|
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 = 3000; // 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 + 3000; // Adjust this value as needed
|
|
QTimer::singleShot(delay4, this, [this]() {
|
|
mIsAvailable = true;
|
|
qDebug() << "Is available: " << mIsAvailable;
|
|
});
|
|
}
|
|
}
|
|
|
|
|
|
bool AiEngineGimbalServer::isAvailable(void)
|
|
{
|
|
return mIsAvailable;
|
|
}
|
|
|
|
|
|
// TODO!! Not sent from the client yet.
|
|
void AiEngineGimbalServer::cameraPositionSlot(AiEngineCameraPosition position)
|
|
{
|
|
qDebug() << "AiEngineGimbalServer::cameraPositionSlot() Move camera to:" << position.pitch << position.yaw << "zoom:" << position.zoom;
|
|
}
|