mirror of
https://github.com/azaion/autopilot.git
synced 2026-04-22 08:36:33 +00:00
Added server side code.
This commit is contained in:
Nffj84
@@ -1,5 +1,10 @@
|
||||
#pragma once
|
||||
|
||||
#include <QByteArray>
|
||||
#include <QString>
|
||||
#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;
|
||||
};
|
||||
|
||||
@@ -1,31 +1,75 @@
|
||||
#include <QDebug>
|
||||
#include <QTimer>
|
||||
#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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -1,14 +1,21 @@
|
||||
#pragma once
|
||||
|
||||
#include <QObject>
|
||||
#include <QMap>
|
||||
#include <QSerialPort>
|
||||
#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;
|
||||
};
|
||||
|
||||
@@ -0,0 +1,306 @@
|
||||
#include <QVariant>
|
||||
#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<QString, QVariant> 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<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);
|
||||
|
||||
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<int>(left + properties.width / 2);
|
||||
properties.middleY = static_cast<int>(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<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 = mSerialCommand->getCommand(SERIAL_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;
|
||||
|
||||
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<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);
|
||||
|
||||
// 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<float>(M_PI);
|
||||
newLocation.lon = newLonRad * 180.0f / static_cast<float>(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;
|
||||
}
|
||||
@@ -0,0 +1,68 @@
|
||||
#pragma once
|
||||
|
||||
#include <QDebug>
|
||||
#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);
|
||||
};
|
||||
@@ -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<const uint8_t *>(data.constData()), data.size(), 0);
|
||||
bytes[0] = crc16 & 0xFF; // Set LSB
|
||||
bytes[1] = crc16 >> 8; // Set MSB
|
||||
}
|
||||
@@ -0,0 +1,14 @@
|
||||
#pragma once
|
||||
|
||||
#include <QByteArray>
|
||||
#include <cstdint>
|
||||
|
||||
|
||||
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);
|
||||
};
|
||||
@@ -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
|
||||
};
|
||||
@@ -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 "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;
|
||||
}
|
||||
@@ -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 <QByteArray>
|
||||
#include <QList>
|
||||
#include <QObject>
|
||||
#include <QString>
|
||||
#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<int> &bytes);
|
||||
int getCommandIndex(SERIAL_COMMAND_ID commandId);
|
||||
std::vector<AiEngineServerSerialCommandStructure> mSerialCommands;
|
||||
};
|
||||
@@ -0,0 +1,92 @@
|
||||
#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;
|
||||
}
|
||||
@@ -0,0 +1,25 @@
|
||||
#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,95 @@
|
||||
/**
|
||||
* This is a serial response class for Siyi Gimbal Cameras.
|
||||
* Other cameras might need their own serial response class.
|
||||
*/
|
||||
|
||||
#include <QDebug>
|
||||
#include <QHash>
|
||||
#include <QString>
|
||||
#include <QVariant>
|
||||
#include "aienginegimbalservercrc16.h"
|
||||
#include "aienginegimbalserverserialresponse.h"
|
||||
|
||||
|
||||
AiEngineGimbalServerSerialResponse::AiEngineGimbalServerSerialResponse(QObject *parent)
|
||||
: QObject{parent}
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
QHash<QString, QVariant> AiEngineGimbalServerSerialResponse::getResponceValues(QByteArray response)
|
||||
{
|
||||
QHash<QString, QVariant> 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;
|
||||
}
|
||||
@@ -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 <QString>
|
||||
#include <QByteArray>
|
||||
#include <QObject>
|
||||
|
||||
|
||||
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<QString, QVariant> getResponceValues(QByteArray response);
|
||||
};
|
||||
@@ -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 {
|
||||
|
||||
Reference in New Issue
Block a user