mirror of
https://github.com/azaion/autopilot.git
synced 2026-04-22 13:26:34 +00:00
Refactored a8 codes and added remote testing app a8_remote.
This commit is contained in:
Nffj84
+21
-2
@@ -18,8 +18,27 @@ linux-g++ {
|
||||
QMAKE_CC = clang
|
||||
}
|
||||
|
||||
SOURCES += *.cpp
|
||||
HEADERS += *.h
|
||||
SOURCES += \
|
||||
config.cpp \
|
||||
localControl.cpp \
|
||||
main.cpp \
|
||||
remoteControl.cpp \
|
||||
serialCommand.cpp \
|
||||
serialPort.cpp \
|
||||
serialResponse.cpp \
|
||||
utilsCRC16.cpp \
|
||||
utilsTargetLocation.cpp
|
||||
|
||||
HEADERS += \
|
||||
config.hpp \
|
||||
defines.hpp \
|
||||
localControl.h \
|
||||
remoteControl.h \
|
||||
serialCommand.hpp \
|
||||
serialPort.hpp \
|
||||
serialResponse.hpp \
|
||||
utilsCRC16.hpp \
|
||||
utilsTargetLocation.hpp
|
||||
|
||||
# When using FFmpeg
|
||||
#INCLUDEPATH += /usr/include/x86_64-linux-gnu
|
||||
|
||||
@@ -0,0 +1,134 @@
|
||||
#include "config.hpp"
|
||||
#include "defines.hpp"
|
||||
#include "serialResponse.hpp"
|
||||
|
||||
SerialPort *Config::mSerial = nullptr;
|
||||
SerialCommand *Config::mCommand = nullptr;
|
||||
uint16_t Config::mResolutionX = 0;
|
||||
uint16_t Config::mResolutionY = 0;
|
||||
float Config::mMaxZoom = 0.0f;
|
||||
float Config::mCurrentYaw = 0.0f;
|
||||
float Config::mCurrentPitch = 0.0f;
|
||||
float Config::mCurrentRoll = 0.0f;
|
||||
float Config::mCurrentZoom = 0.0f;
|
||||
|
||||
void Config::sanityCheck()
|
||||
{
|
||||
if (mSerial == nullptr) {
|
||||
qCritical().noquote().nospace() << "Serial connection not available.";
|
||||
}
|
||||
|
||||
if (mCommand == nullptr) {
|
||||
qCritical().noquote().nospace() << "Commands not available.";
|
||||
}
|
||||
}
|
||||
|
||||
void Config::setInitalValues(SerialPort *serial, SerialCommand *command)
|
||||
{
|
||||
mSerial = serial;
|
||||
mCommand = command;
|
||||
|
||||
sanityCheck();
|
||||
|
||||
// Setup some values that might be needed from camera
|
||||
QByteArray tempCommand;
|
||||
QByteArray tempResponse;
|
||||
QHash<QString, QVariant> responseValues;
|
||||
|
||||
// Get resolution to reduce calls to camera
|
||||
tempCommand = mCommand->getCommandForInternal(COMMAND_ID::ACQUIRE_CAMERA_CODEC_SPECS);
|
||||
mSerial->sendCommand(tempCommand);
|
||||
tempResponse = mSerial->readResponse();
|
||||
responseValues = SerialResponse::getResponceValues(tempResponse);
|
||||
mResolutionX = responseValues["width"].toInt();
|
||||
mResolutionY = responseValues["height"].toInt();
|
||||
|
||||
// Get max zoom value to reduce calls to camera
|
||||
tempCommand = mCommand->getCommandForInternal(COMMAND_ID::ACQUIRE_MAX_ZOOM_VALUE);
|
||||
mSerial->sendCommand(tempCommand);
|
||||
tempResponse = mSerial->readResponse();
|
||||
responseValues = SerialResponse::getResponceValues(tempResponse);
|
||||
mMaxZoom = responseValues["zoom"].toInt();
|
||||
|
||||
// Debug printing
|
||||
qDebug().noquote().nospace() << "Camera resolution: " << mResolutionX << "*" << mResolutionY;
|
||||
qDebug().noquote().nospace() << "Camera max zoom: " << mMaxZoom;
|
||||
}
|
||||
|
||||
void Config::updateState()
|
||||
{
|
||||
sanityCheck();
|
||||
QHash<QString, QVariant> responseValues;
|
||||
QByteArray tempCommand;
|
||||
QByteArray tempResponse;
|
||||
|
||||
// Get current angles
|
||||
tempCommand = mCommand->getCommandForInternal(COMMAND_ID::ACQUIRE_ATTITUDE_DATA);
|
||||
Config::getSerial()->sendCommand(tempCommand);
|
||||
tempResponse = Config::getSerial()->readResponse();
|
||||
responseValues = SerialResponse::getResponceValues(tempResponse);
|
||||
mCurrentYaw = responseValues["yaw"].toFloat();
|
||||
mCurrentPitch = responseValues["pitch"].toFloat();
|
||||
mCurrentRoll = responseValues["roll"].toFloat();
|
||||
|
||||
// Get current zoom
|
||||
tempCommand = mCommand->getCommandForInternal(COMMAND_ID::ACQUIRE_CURRENT_ZOOM);
|
||||
Config::getSerial()->sendCommand(tempCommand);
|
||||
tempResponse = Config::getSerial()->readResponse();
|
||||
responseValues = SerialResponse::getResponceValues(tempResponse);
|
||||
mCurrentZoom = responseValues["zoom"].toFloat();
|
||||
}
|
||||
|
||||
SerialPort *Config::getSerial()
|
||||
{
|
||||
sanityCheck();
|
||||
return mSerial;
|
||||
}
|
||||
|
||||
SerialCommand *Config::getCommand()
|
||||
{
|
||||
sanityCheck();
|
||||
return mCommand;
|
||||
}
|
||||
|
||||
uint16_t Config::getResolutionWidth()
|
||||
{
|
||||
sanityCheck();
|
||||
return mResolutionX;
|
||||
}
|
||||
|
||||
uint16_t Config::getResolutionHeight()
|
||||
{
|
||||
sanityCheck();
|
||||
return mResolutionY;
|
||||
}
|
||||
|
||||
float Config::getMaxZoom()
|
||||
{
|
||||
sanityCheck();
|
||||
return mMaxZoom;
|
||||
}
|
||||
|
||||
float Config::getCurrentYaw()
|
||||
{
|
||||
sanityCheck();
|
||||
return mCurrentYaw;
|
||||
}
|
||||
|
||||
float Config::getCurrentPitch()
|
||||
{
|
||||
sanityCheck();
|
||||
return mCurrentPitch;
|
||||
}
|
||||
|
||||
float Config::getCurrentRoll()
|
||||
{
|
||||
sanityCheck();
|
||||
return mCurrentRoll;
|
||||
}
|
||||
|
||||
float Config::getCurrentZoom()
|
||||
{
|
||||
sanityCheck();
|
||||
return (mCurrentZoom == 0.0f ? 1.0f : mCurrentZoom);
|
||||
}
|
||||
@@ -0,0 +1,40 @@
|
||||
#pragma once
|
||||
|
||||
#include "serialCommand.hpp"
|
||||
#include "serialPort.hpp"
|
||||
#include <cstdint>
|
||||
|
||||
class Config
|
||||
{
|
||||
public:
|
||||
static void setInitalValues(SerialPort *serial, SerialCommand *command);
|
||||
static void updateState(void);
|
||||
|
||||
static SerialPort *getSerial();
|
||||
static SerialCommand *getCommand(void);
|
||||
static uint16_t getResolutionWidth(void);
|
||||
static uint16_t getResolutionHeight(void);
|
||||
static float getMaxZoom(void);
|
||||
|
||||
static float getCurrentYaw(void);
|
||||
static float getCurrentPitch(void);
|
||||
static float getCurrentRoll(void);
|
||||
static float getCurrentZoom(void);
|
||||
|
||||
private:
|
||||
static void sanityCheck(void);
|
||||
|
||||
static SerialPort *mSerial;
|
||||
static SerialCommand *mCommand;
|
||||
|
||||
// Get these only once
|
||||
static uint16_t mResolutionX;
|
||||
static uint16_t mResolutionY;
|
||||
static float mMaxZoom;
|
||||
|
||||
// Update these before every command
|
||||
static float mCurrentYaw;
|
||||
static float mCurrentPitch;
|
||||
static float mCurrentRoll;
|
||||
static float mCurrentZoom;
|
||||
};
|
||||
@@ -1,9 +1,13 @@
|
||||
#pragma once
|
||||
|
||||
#include <QHash>
|
||||
#include <QString>
|
||||
|
||||
enum MESSAGE_IDX { STX = 0, CTRL = 2, Data_len = 3, SEQ = 5, CMD_ID = 7, DATA = 8 };
|
||||
|
||||
enum COMMAND_ID {
|
||||
TURN_TO_X = 2, // Set first to 2, because 0 is reserved for EXIT_PROGRAM and 1 for running target location test
|
||||
TURN_TO_DEGREES = 1,
|
||||
TURN_TO_PIXEL,
|
||||
ZOOM_TO_X,
|
||||
ACQUIRE_CAMERA_CODEC_SPECS,
|
||||
ACQUIRE_CURRENT_ZOOM,
|
||||
@@ -33,11 +37,22 @@ enum COMMAND_ID {
|
||||
ENABLE_CVBS,
|
||||
DISABLE_HDMI_CVBS,
|
||||
ACQUIRE_RANGE_DATA,
|
||||
RUN_TARGET_LOCATION_TEST
|
||||
};
|
||||
|
||||
#define CAMERA_FIELD_OF_VIEW_DIAGONAL 93.0
|
||||
#define CAMERA_FIELD_OF_VIEW_HORIZONTAL 81.0
|
||||
#define CAMERA_RESOLUTION_HEIGHT 1080
|
||||
#define CAMERA_RESOLUTION_WIDTH 1920
|
||||
#define CAMERA_ASPECT_RATIO 1.777777778f
|
||||
#define CAMERA_FIELD_OF_VIEW_DIAGONAL 93.0f
|
||||
#define CAMERA_FIELD_OF_VIEW_HORIZONTAL 81.0f
|
||||
#define CAMERA_FIELD_OF_VIEW_VERTICAL 62.0f
|
||||
#define CAMERA_FOCAL_LENGTH 21
|
||||
#define CAMERA_RESOLUTION_WIDTH 1280
|
||||
#define CAMERA_RESOLUTION_HEIGHT 720
|
||||
#define GIMBAL_YAW_MIN -135.0f
|
||||
#define GIMBAL_YAW_MAX 135.0f
|
||||
#define GIMBAL_PITCH_MIN -90.0f
|
||||
#define GIMBAL_PITCH_MAX 25.0f
|
||||
#define SERIAL_RESPONSE_WAIT_TIME 500
|
||||
#define RTSP_ADDRESS "rtsp://192.168.144.25:8554/main.264"
|
||||
#define SERIAL_PORT "/dev/ttyUSB0"
|
||||
#define FIFO_WHO_AM_I "CAM"
|
||||
#define FIFO_TO_GIMBAL "/tmp/fifo_to_a8_gimbal"
|
||||
#define FIFO_FROM_GIMBAL "/tmp/fifo_from_a8_gimbal"
|
||||
@@ -0,0 +1,61 @@
|
||||
#include "localControl.h"
|
||||
#include <QCoreApplication>
|
||||
#include <QDebug>
|
||||
#include <QObject>
|
||||
#include <QThread>
|
||||
#include "config.hpp"
|
||||
#include "serialResponse.hpp"
|
||||
#include "utilsTargetLocation.hpp"
|
||||
#include <iostream>
|
||||
|
||||
LocalControl::LocalControl()
|
||||
: QObject(nullptr)
|
||||
{}
|
||||
|
||||
void LocalControl::run()
|
||||
{
|
||||
while (true) {
|
||||
Config::getCommand()->printCommands();
|
||||
|
||||
// Get user input
|
||||
qInfo().noquote().nospace() << "Enter a command (0 to exit): ";
|
||||
int16_t number;
|
||||
std::cin >> number;
|
||||
|
||||
// Check if the input is within the valid range for uint8_t
|
||||
if (number < 0 || number > Config::getCommand()->getCommandCount()) {
|
||||
qWarning().noquote().nospace() << "Number (" << qPrintable(QString::number(number)) << ") out of range 0 -" << qPrintable(QString::number(Config::getCommand()->getCommandCount() - 1));
|
||||
continue;
|
||||
}
|
||||
|
||||
// Exit loop if user enters 0
|
||||
if (number == 0) {
|
||||
exit(EXIT_SUCCESS);
|
||||
} else {
|
||||
Config::updateState();
|
||||
COMMAND_ID commandId = (COMMAND_ID) number;
|
||||
|
||||
if (commandId == COMMAND_ID::RUN_TARGET_LOCATION_TEST) {
|
||||
qInfo().noquote().nospace() << "Running target location test";
|
||||
GPSData gpsData = UtilsTargetLocation::getLocation(200.0f, 63.16122286887124f, 23.822053704379698f, 180.0f, 0.0f, 0.0f, 5.0f, 20);
|
||||
qInfo().noquote().nospace() << "Altitude: " << gpsData.altitude;
|
||||
qInfo().noquote().nospace() << "Latitude: " << gpsData.latitude;
|
||||
qInfo().noquote().nospace() << "Longitude: " << gpsData.longitude;
|
||||
|
||||
QCoreApplication::processEvents();
|
||||
continue;
|
||||
}
|
||||
|
||||
// Example command to send (replace with actual Siyi A8 mini camera
|
||||
// commands)
|
||||
QByteArray command = Config::getCommand()->getCommandForUI(commandId);
|
||||
Config::getSerial()->sendCommand(command);
|
||||
|
||||
// Read response from the camera
|
||||
QByteArray response = Config::getSerial()->readResponse();
|
||||
SerialResponse::printResponse(response);
|
||||
}
|
||||
|
||||
QCoreApplication::processEvents();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,12 @@
|
||||
#include <QObject>
|
||||
|
||||
#pragma once
|
||||
|
||||
class LocalControl : public QObject
|
||||
{
|
||||
Q_OBJECT
|
||||
|
||||
public:
|
||||
LocalControl();
|
||||
void run();
|
||||
};
|
||||
+21
-56
@@ -1,69 +1,34 @@
|
||||
#include <QCoreApplication>
|
||||
#include <QThread>
|
||||
#include "serialCommand.h"
|
||||
#include "serialPort.h"
|
||||
#include "serialResponse.h"
|
||||
#include "utilsTargetLocation.h"
|
||||
#include <iostream>
|
||||
#include "config.hpp"
|
||||
#include "localControl.h"
|
||||
#include "remoteControl.h"
|
||||
#include "serialCommand.hpp"
|
||||
#include "serialPort.hpp"
|
||||
|
||||
int main(int argc, char *argv[])
|
||||
{
|
||||
QCoreApplication app(argc, argv);
|
||||
SerialCommand serialCommand;
|
||||
SerialPort serialPort;
|
||||
Config::setInitalValues(&serialPort, &serialCommand);
|
||||
|
||||
// Replace with your actual port name
|
||||
const QString portName = "/dev/ttyUSB0";
|
||||
|
||||
// Create SerialPort object
|
||||
SerialPort serial(portName);
|
||||
|
||||
// Open the serial port
|
||||
if (!serial.openPort()) {
|
||||
qDebug() << "Failed to open serial port";
|
||||
return 1;
|
||||
}
|
||||
|
||||
SerialCommand commands;
|
||||
|
||||
while (true) {
|
||||
commands.printCommands();
|
||||
|
||||
// Get user input
|
||||
qInfo() << "Enter a command (0 to exit, 1 to run test): ";
|
||||
int16_t number;
|
||||
std::cin >> number;
|
||||
|
||||
// Check if the input is within the valid range for uint8_t
|
||||
if (number < 0 || number > commands.getCommandCount() - 1) {
|
||||
qWarning() << "Number (" << qPrintable(QString::number(number)) << ") out of range 0 -" << qPrintable(QString::number(commands.getCommandCount() - 1));
|
||||
continue;
|
||||
}
|
||||
|
||||
// Exit loop if user enters 0
|
||||
if (number == 0) {
|
||||
bool useRemoteMode = true;
|
||||
for (int i = 1; i < argc; ++i) {
|
||||
if (QString(argv[i]) == "-l") {
|
||||
useRemoteMode = false;
|
||||
break;
|
||||
} else if (number == 1) {
|
||||
qInfo() << "Running target location test";
|
||||
|
||||
UtilsTargetLocation locateTarget(&serial, &commands);
|
||||
GPSData gpsData = locateTarget.getLocation(200.0, 63.16122286887124, 23.822053704379698, 180.0, 0.0, 0.0, 5.0, 20);
|
||||
qInfo() << "Altitude: " << gpsData.altitude;
|
||||
qInfo() << "Latitude: " << gpsData.latitude;
|
||||
qInfo() << "Longitude: " << gpsData.longitude;
|
||||
} else {
|
||||
COMMAND_ID commandId = (COMMAND_ID) number;
|
||||
|
||||
// Example command to send (replace with actual Siyi A8 mini camera commands)
|
||||
QByteArray command = commands.getCommandByID(commandId);
|
||||
serial.sendCommand(command);
|
||||
|
||||
// Read response from the camera
|
||||
QByteArray response = serial.readResponse();
|
||||
SerialResponse::printResponse(response);
|
||||
}
|
||||
}
|
||||
|
||||
// Close the serial port
|
||||
serial.closePort();
|
||||
// Remote mode will read commands from pipe
|
||||
if (useRemoteMode == true) {
|
||||
RemoteControl remoteControl;
|
||||
remoteControl.run();
|
||||
} else {
|
||||
LocalControl localControl;
|
||||
localControl.run();
|
||||
}
|
||||
|
||||
return 0;
|
||||
return app.exec();
|
||||
}
|
||||
|
||||
@@ -0,0 +1,264 @@
|
||||
#include "remoteControl.h"
|
||||
#include <QCoreApplication>
|
||||
#include <QDebug>
|
||||
#include <QJsonDocument>
|
||||
#include <QJsonObject>
|
||||
#include <QThread>
|
||||
#include <QTimer>
|
||||
#include "config.hpp"
|
||||
#include "defines.hpp"
|
||||
#include "utilsTargetLocation.hpp"
|
||||
#include <fcntl.h>
|
||||
#include <sys/stat.h>
|
||||
#include <unistd.h>
|
||||
|
||||
RemoteControl::RemoteControl()
|
||||
: QObject(nullptr)
|
||||
{
|
||||
openNamedPipe();
|
||||
}
|
||||
|
||||
RemoteControl::~RemoteControl()
|
||||
{
|
||||
if (mFifoFdIn != -1) {
|
||||
close(mFifoFdIn);
|
||||
}
|
||||
if (mFifoFdOut != -1) {
|
||||
close(mFifoFdOut);
|
||||
}
|
||||
}
|
||||
|
||||
void RemoteControl::openNamedPipe()
|
||||
{
|
||||
struct stat statBuf;
|
||||
|
||||
// Open incoming pipe (READ ONLY)
|
||||
if (stat(FIFO_TO_GIMBAL, &statBuf) == 0) {
|
||||
if (S_ISFIFO(statBuf.st_mode)) {
|
||||
qInfo().noquote().nospace() << "Named pipe already exists: " << FIFO_TO_GIMBAL;
|
||||
} else {
|
||||
qCritical().noquote().nospace() << FIFO_TO_GIMBAL << " exists but is not a FIFO";
|
||||
}
|
||||
} else {
|
||||
if (mkfifo(FIFO_TO_GIMBAL, 0666) == -1) {
|
||||
perror("mkfifo");
|
||||
qCritical().noquote().nospace() << "Failed to create named pipe: " << FIFO_TO_GIMBAL;
|
||||
} else {
|
||||
qInfo().noquote().nospace() << "Created named pipe: " << FIFO_TO_GIMBAL;
|
||||
}
|
||||
}
|
||||
|
||||
mFifoFdIn = open(FIFO_TO_GIMBAL, O_RDONLY | O_NONBLOCK);
|
||||
if (mFifoFdIn == -1) {
|
||||
qCritical().noquote().nospace() << "Error opening pipe for reading: " << FIFO_TO_GIMBAL;
|
||||
} else {
|
||||
qInfo().noquote().nospace() << "Opened pipe: " << FIFO_TO_GIMBAL;
|
||||
}
|
||||
|
||||
// Open outgoing pipe (WRITE ONLY)
|
||||
if (stat(FIFO_FROM_GIMBAL, &statBuf) == 0) {
|
||||
if (S_ISFIFO(statBuf.st_mode)) {
|
||||
qInfo().noquote().nospace() << "Named pipe already exists: " << FIFO_FROM_GIMBAL;
|
||||
} else {
|
||||
qCritical().noquote().nospace() << FIFO_FROM_GIMBAL << " exists but is not a FIFO";
|
||||
}
|
||||
} else {
|
||||
if (mkfifo(FIFO_FROM_GIMBAL, 0666) == -1) {
|
||||
perror("mkfifo");
|
||||
qCritical().noquote().nospace() << "Failed to create named pipe: " << FIFO_FROM_GIMBAL;
|
||||
} else {
|
||||
qInfo().noquote().nospace() << "Created named pipe: " << FIFO_FROM_GIMBAL;
|
||||
}
|
||||
}
|
||||
|
||||
mFifoFdOut = open(FIFO_FROM_GIMBAL, O_WRONLY);
|
||||
if (mFifoFdOut == -1) {
|
||||
qCritical().noquote().nospace() << "Error opening pipe for writing: " << FIFO_FROM_GIMBAL;
|
||||
} else {
|
||||
qInfo().noquote().nospace() << "Opened pipe: " << FIFO_FROM_GIMBAL;
|
||||
}
|
||||
}
|
||||
|
||||
void RemoteControl::whenDone(void)
|
||||
{
|
||||
// All is done, no it's time to rest
|
||||
}
|
||||
|
||||
void RemoteControl::restoreOrientation(void)
|
||||
{
|
||||
QByteArray serialCommandAngle = Config::getCommand()->getCommandForInternal(COMMAND_ID::TURN_TO_DEGREES);
|
||||
|
||||
int16_t degreesVal = Config::getCurrentYaw() * 10;
|
||||
serialCommandAngle[8] = degreesVal & 0xFF;
|
||||
serialCommandAngle[9] = degreesVal >> 8;
|
||||
degreesVal = Config::getCurrentPitch() * 10;
|
||||
serialCommandAngle[10] = degreesVal & 0xFF;
|
||||
serialCommandAngle[11] = degreesVal >> 8;
|
||||
|
||||
qDebug().noquote().nospace() << serialCommandAngle;
|
||||
Config::getSerial()->sendCommand(serialCommandAngle);
|
||||
}
|
||||
|
||||
void RemoteControl::restoreZoom(void)
|
||||
{
|
||||
QByteArray serialCommandZoom = Config::getCommand()->getCommandForInternal(COMMAND_ID::ZOOM_TO_X);
|
||||
|
||||
uint8_t integerPart = static_cast<uint8_t>(Config::getCurrentZoom());
|
||||
float fractionalPart = Config::getCurrentZoom() - integerPart;
|
||||
uint8_t scaledFractional = uint8_t(fractionalPart * 10);
|
||||
|
||||
serialCommandZoom[8] = integerPart;
|
||||
serialCommandZoom[9] = scaledFractional;
|
||||
|
||||
qDebug().noquote().nospace() << serialCommandZoom.toStdString();
|
||||
Config::getSerial()->sendCommand(serialCommandZoom);
|
||||
}
|
||||
|
||||
QJsonObject RemoteControl::calculateTargetPosition(QJsonObject &commandObject, QJsonObject &responseObject)
|
||||
{
|
||||
qDebug().noquote().nospace() << "Calculating?";
|
||||
|
||||
float latitude = commandObject["latitude"].toDouble();
|
||||
float longitude = commandObject["longitude"].toDouble();
|
||||
float altitude = commandObject["altitude"].toDouble();
|
||||
float yaw = commandObject["yaw"].toDouble();
|
||||
float pitch = commandObject["pitch"].toDouble();
|
||||
float targetPixelWidth = commandObject["target_pixel_width"].toInt();
|
||||
//float targetPixelHeight = commandObject["target_pixel_height"].toInt();
|
||||
float targetRealWidth = commandObject["target_real_width"].toDouble();
|
||||
//float targetRealHeight = commandObject["target_real_height"].toDouble();
|
||||
|
||||
GPSData gpsData = UtilsTargetLocation::getLocation(altitude, latitude, longitude, yaw, pitch, 0.0f, targetRealWidth, targetPixelWidth);
|
||||
responseObject["altitude"] = gpsData.altitude;
|
||||
responseObject["latitude"] = gpsData.latitude;
|
||||
responseObject["longitude"] = gpsData.longitude;
|
||||
|
||||
return responseObject;
|
||||
}
|
||||
|
||||
void RemoteControl::turnToTarget(QJsonObject &commandObject)
|
||||
{
|
||||
qDebug().noquote().nospace() << "Turning?";
|
||||
|
||||
uint16_t targetX = commandObject["target_x"].toInt();
|
||||
uint16_t targetY = commandObject["target_y"].toInt();
|
||||
|
||||
float resultYaw = 0.0f;
|
||||
float resultPitch = 0.0f;
|
||||
UtilsTargetLocation::getAnglesToOnScreenTarget(targetX, targetY, resultYaw, resultPitch);
|
||||
|
||||
QByteArray serialCommandTurn = Config::getCommand()->getCommandForInternal(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;
|
||||
|
||||
Config::getSerial()->sendCommand(serialCommandTurn);
|
||||
}
|
||||
|
||||
void RemoteControl::zoomToTarget(QJsonObject &commandObject)
|
||||
{
|
||||
qDebug().noquote().nospace() << "Zooming?";
|
||||
|
||||
float fillRatio = 0.5;
|
||||
float targetPixelWidth = commandObject["target_pixel_width"].toInt();
|
||||
float targetPixelHeight = commandObject["target_pixel_height"].toInt();
|
||||
float adjustedObjectWidth = targetPixelWidth / fillRatio;
|
||||
float adjustedObjectHeight = targetPixelHeight / fillRatio;
|
||||
float zoomWidth = static_cast<double>(Config::getResolutionWidth()) / adjustedObjectWidth;
|
||||
float zoomHeight = static_cast<double>(Config::getResolutionHeight()) / adjustedObjectHeight;
|
||||
float zoom = std::min(zoomWidth, zoomHeight);
|
||||
if (zoom < 1.0f) {
|
||||
zoom = 1.0f;
|
||||
}
|
||||
|
||||
QByteArray serialCommandNewZoom = Config::getCommand()->getCommandForInternal(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;
|
||||
|
||||
Config::getSerial()->sendCommand(serialCommandNewZoom);
|
||||
}
|
||||
|
||||
void RemoteControl::run()
|
||||
{
|
||||
while (true) {
|
||||
char buffer[1024];
|
||||
ssize_t bytesRead = read(mFifoFdIn, buffer, sizeof(buffer) - 1);
|
||||
|
||||
if (bytesRead > 0) {
|
||||
buffer[bytesRead] = '\0';
|
||||
|
||||
QJsonDocument commandDoc = QJsonDocument::fromJson(buffer);
|
||||
|
||||
// Ignore non json messages
|
||||
if (commandDoc.isNull() == false) {
|
||||
QJsonObject commandObject = commandDoc.object();
|
||||
|
||||
// Ignore own messages and messages that don't have sender
|
||||
if (commandObject.contains("sender") == false || commandObject["sender"] == FIFO_WHO_AM_I) {
|
||||
continue;
|
||||
}
|
||||
|
||||
QString message = QString::fromUtf8(buffer);
|
||||
qDebug().noquote().nospace() << "Received: " << message;
|
||||
|
||||
// Exit with exit message
|
||||
if (commandObject.contains("EXIT") == true) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Prepare responce object
|
||||
QJsonObject responseObject;
|
||||
responseObject["sender"] = FIFO_WHO_AM_I;
|
||||
responseObject["status"] = "OK";
|
||||
|
||||
// Get current orientation and zoom
|
||||
Config::updateState();
|
||||
|
||||
// Turn to target
|
||||
if (commandObject.contains("target_x") == true && commandObject.contains("target_y") == true) {
|
||||
QTimer::singleShot(100, this, [this, commandObject]() mutable { turnToTarget(commandObject); });
|
||||
QTimer::singleShot(1000, this, &RemoteControl::whenDone);
|
||||
}
|
||||
|
||||
// Calculate target location
|
||||
if (commandObject.contains("latitude") == true && commandObject.contains("longitude") == true && commandObject.contains("altitude") == true && commandObject.contains("yaw") == true && commandObject.contains("pitch") == true && commandObject.contains("target_pixel_width") == true && commandObject.contains("target_pixel_height") == true && commandObject.contains("target_real_width") == true && commandObject.contains("target_real_height") == true) {
|
||||
responseObject = calculateTargetPosition(commandObject, responseObject);
|
||||
QTimer::singleShot(1000, this, &RemoteControl::whenDone);
|
||||
}
|
||||
|
||||
// Zoom to target
|
||||
if (commandObject.contains("target_pixel_width") == true && commandObject.contains("target_pixel_height") == true) {
|
||||
QTimer::singleShot(100, this, [this, commandObject]() mutable { zoomToTarget(commandObject); });
|
||||
QTimer::singleShot(1000, this, &RemoteControl::whenDone);
|
||||
}
|
||||
|
||||
// Restore previous zoom and orientation
|
||||
QTimer::singleShot(100, this, &RemoteControl::restoreZoom);
|
||||
QTimer::singleShot(1000, this, &RemoteControl::whenDone);
|
||||
|
||||
QTimer::singleShot(100, this, &RemoteControl::restoreOrientation);
|
||||
QTimer::singleShot(1000, this, &RemoteControl::whenDone);
|
||||
|
||||
// Respond after doing camera things
|
||||
QJsonDocument responseDocument(responseObject);
|
||||
std::string response = responseDocument.toJson(QJsonDocument::Compact).toStdString();
|
||||
write(mFifoFdOut, response.c_str(), response.size());
|
||||
qDebug().noquote().nospace() << "Sent: " << response;
|
||||
QCoreApplication::processEvents();
|
||||
}
|
||||
}
|
||||
|
||||
// Sleep for a while
|
||||
QCoreApplication::processEvents();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,28 @@
|
||||
#pragma once
|
||||
|
||||
#include <QJsonObject>
|
||||
#include <QObject>
|
||||
#include <QString>
|
||||
|
||||
class RemoteControl : public QObject
|
||||
{
|
||||
Q_OBJECT
|
||||
|
||||
public:
|
||||
RemoteControl();
|
||||
~RemoteControl();
|
||||
void openNamedPipe(void);
|
||||
void run();
|
||||
|
||||
private slots:
|
||||
QJsonObject calculateTargetPosition(QJsonObject &commandObject, QJsonObject &responseObject);
|
||||
void turnToTarget(QJsonObject &commandObject);
|
||||
void zoomToTarget(QJsonObject &commandObject);
|
||||
void restoreOrientation(void);
|
||||
void restoreZoom(void);
|
||||
void whenDone(void);
|
||||
|
||||
private:
|
||||
int mFifoFdIn;
|
||||
int mFifoFdOut;
|
||||
};
|
||||
@@ -1,33 +1,35 @@
|
||||
#include "serialCommand.h"
|
||||
#include "serialCommand.hpp"
|
||||
#include <QDebug>
|
||||
#include "utilsCRC16.h"
|
||||
#include "config.hpp"
|
||||
#include "utilsTargetLocation.hpp"
|
||||
#include <iostream>
|
||||
|
||||
SerialCommand::SerialCommand()
|
||||
{
|
||||
/*
|
||||
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
|
||||
*/
|
||||
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({COMMAND_ID::TURN_TO_X, createByteArray({0x55, 0x66, 0x01, 0x04, 0x00, 0x00, 0x00, 0x0E, 0x00, 0x00, 0x00, 0x00}), "Turn to X"});
|
||||
mSerialCommands.push_back({COMMAND_ID::TURN_TO_DEGREES, createByteArray({0x55, 0x66, 0x01, 0x04, 0x00, 0x00, 0x00, 0x0E, 0x00, 0x00, 0x00, 0x00}), "Turn to degrees"});
|
||||
mSerialCommands.push_back({COMMAND_ID::TURN_TO_PIXEL, createByteArray({0x55, 0x66, 0x01, 0x04, 0x00, 0x00, 0x00, 0x0E, 0x00, 0x00, 0x00, 0x00}), "Turn to pixel"});
|
||||
mSerialCommands.push_back({COMMAND_ID::ZOOM_TO_X, createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x01, 0x00, 0x0F, 0x00, 0x00}), "Zoom to X"});
|
||||
mSerialCommands.push_back({COMMAND_ID::ACQUIRE_CAMERA_CODEC_SPECS, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x20, 0x00}), "Acquire Camera Codec Specs"});
|
||||
mSerialCommands.push_back({COMMAND_ID::ACQUIRE_CURRENT_ZOOM, createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x18}), "Acquire current zoom"});
|
||||
mSerialCommands.push_back({COMMAND_ID::ACQUIRE_ATTITUDE_DATA, createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x0d}), "Acquire Attitude Data"});
|
||||
mSerialCommands.push_back({COMMAND_ID::AUTO_CENTER, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x08, 0x01}), "Auto Centering"});
|
||||
mSerialCommands.push_back({COMMAND_ID::ACQUIRE_ATTITUDE_DATA, createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x0D}), "Acquire Attitude Data"});
|
||||
mSerialCommands.push_back({COMMAND_ID::ACQUIRE_CURRENT_ZOOM, createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x18}), "Acquire current zoom"});
|
||||
mSerialCommands.push_back({COMMAND_ID::ACQUIRE_CAMERA_CODEC_SPECS, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x20, 0x00}), "Acquire Camera Codec Specs"});
|
||||
mSerialCommands.push_back({COMMAND_ID::ZOOM_MOST, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x05, 0x01}), "Zoom +1"});
|
||||
mSerialCommands.push_back({COMMAND_ID::ZOOM_LEAST, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x05, 0xFF}), "Zoom -1"});
|
||||
mSerialCommands.push_back({COMMAND_ID::FOCUS_MOST, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x06, 0x01}), "Manual Focus +1"});
|
||||
mSerialCommands.push_back({COMMAND_ID::FOCUS_LEAST, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x06, 0xff}), "Manual Focus -1"});
|
||||
mSerialCommands.push_back({COMMAND_ID::FOCUS_LEAST, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x06, 0xFF}), "Manual Focus -1"});
|
||||
mSerialCommands.push_back({COMMAND_ID::FOCUS_AUTO, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x04, 0x01}), "Auto Focus"});
|
||||
mSerialCommands.push_back({COMMAND_ID::ROTATE_UP, createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x2D}), "Rotate Up"});
|
||||
mSerialCommands.push_back({COMMAND_ID::ROTATE_DOWN, createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, -0x2D}), "Rotate Down"});
|
||||
@@ -35,33 +37,41 @@ byte in the front
|
||||
mSerialCommands.push_back({COMMAND_ID::ROTATE_LEFT, createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, -0x2D, 0x00}), "Rotate Left"});
|
||||
mSerialCommands.push_back({COMMAND_ID::ROTATE_STOP, createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00}), "Stop rotation"});
|
||||
mSerialCommands.push_back({COMMAND_ID::ACQUIRE_MAX_ZOOM_VALUE, createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x16}), "Acquire the Max Zoom Value"});
|
||||
mSerialCommands.push_back({COMMAND_ID::TAKE_PICTURES, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x00}), "Take Pictures"});
|
||||
mSerialCommands.push_back({COMMAND_ID::TAKE_VIDEO, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x02}), "Record Video"});
|
||||
mSerialCommands.push_back({COMMAND_ID::TAKE_PICTURES, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0C, 0x00}), "Take Pictures"});
|
||||
mSerialCommands.push_back({COMMAND_ID::TAKE_VIDEO, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0C, 0x02}), "Record Video"});
|
||||
mSerialCommands.push_back({COMMAND_ID::ROTATE_100_100, createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x64, 0x64}), "Rotate 100 100"});
|
||||
mSerialCommands.push_back({COMMAND_ID::ACQUIRE_GIMBAL_STATUS, createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x0a}), "Gimbal Status Information"});
|
||||
mSerialCommands.push_back({COMMAND_ID::ACQUIRE_GIMBAL_STATUS, createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x0A}), "Gimbal Status Information"});
|
||||
mSerialCommands.push_back({COMMAND_ID::ACQUIRE_HW_INFO, createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x02}), "Acquire Hardware ID"});
|
||||
mSerialCommands.push_back({COMMAND_ID::ACQUIRE_FIRMWARE_VERSION, createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01}), "Acquire Firmware Version"});
|
||||
mSerialCommands.push_back({COMMAND_ID::MODE_LOCK, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x03}), "Lock Mode"});
|
||||
mSerialCommands.push_back({COMMAND_ID::MODE_FOLLOW, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x04}), "Follow Mode"});
|
||||
mSerialCommands.push_back({COMMAND_ID::MODE_FPV, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x05}), "FPV Mode"});
|
||||
mSerialCommands.push_back({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({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({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({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({COMMAND_ID::MODE_LOCK, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0C, 0x03}), "Lock Mode"});
|
||||
mSerialCommands.push_back({COMMAND_ID::MODE_FOLLOW, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0C, 0x04}), "Follow Mode"});
|
||||
mSerialCommands.push_back({COMMAND_ID::MODE_FPV, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0C, 0x05}), "FPV Mode"});
|
||||
mSerialCommands.push_back({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({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({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({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({COMMAND_ID::RUN_TARGET_LOCATION_TEST, createByteArray({0x00, 0x00}), "TEST target location calculations"});
|
||||
|
||||
// Sort vector by COMMAND_ID
|
||||
std::sort(mSerialCommands.begin(), mSerialCommands.end(), [](const Command& a, const Command& b) { return a.id < b.id; });
|
||||
std::sort(mSerialCommands.begin(), mSerialCommands.end(), [](const Command &a, const Command &b) { return a.id < b.id; });
|
||||
}
|
||||
|
||||
SerialCommand::~SerialCommand()
|
||||
{
|
||||
// Do something?
|
||||
}
|
||||
|
||||
QByteArray SerialCommand::createByteArray(const std::initializer_list<int>& bytes)
|
||||
QByteArray SerialCommand::createByteArray(const std::initializer_list<int> &bytes)
|
||||
{
|
||||
QByteArray byteArray;
|
||||
for (int byte : bytes) {
|
||||
for (int8_t byte : bytes) {
|
||||
byteArray.append(static_cast<char>(byte));
|
||||
}
|
||||
return byteArray;
|
||||
@@ -69,8 +79,10 @@ QByteArray SerialCommand::createByteArray(const std::initializer_list<int>& byte
|
||||
|
||||
void SerialCommand::printCommands()
|
||||
{
|
||||
qInfo().noquote().nospace() << " ";
|
||||
|
||||
for (uint8_t i = 0; i < mSerialCommands.size(); i++) {
|
||||
qInfo().noquote() << QString::number(mSerialCommands.at(i).id) + ": " + mSerialCommands.at(i).description;
|
||||
qInfo().noquote().nospace() << QString::number(mSerialCommands.at(i).id) + ": " + mSerialCommands.at(i).description;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -79,14 +91,14 @@ void SerialCommand::setExtraValues(COMMAND_ID commandId)
|
||||
uint16_t commandIndex = getCommandIndex(commandId);
|
||||
QByteArray command = mSerialCommands.at(commandIndex).command;
|
||||
|
||||
if (commandId == COMMAND_ID::TURN_TO_X) {
|
||||
if (commandId == COMMAND_ID::TURN_TO_DEGREES) {
|
||||
float degrees;
|
||||
|
||||
qInfo() << "Enter yaw degrees (-135.0 -> 135.0): ";
|
||||
qInfo().noquote().nospace() << "Enter yaw degrees (" << GIMBAL_YAW_MIN << " -> " << GIMBAL_YAW_MAX << "): ";
|
||||
std::cin >> degrees;
|
||||
|
||||
if (std::cin.fail() || degrees < -135.0f || degrees > 135.0f) {
|
||||
qWarning() << "Invalid value, using 0.0";
|
||||
if (std::cin.fail() || degrees < GIMBAL_YAW_MIN || degrees > GIMBAL_YAW_MAX) {
|
||||
qWarning().noquote().nospace() << "Invalid value, using 0.0";
|
||||
degrees = 0.0f;
|
||||
}
|
||||
|
||||
@@ -94,31 +106,61 @@ void SerialCommand::setExtraValues(COMMAND_ID commandId)
|
||||
command[8] = degreesVal & 0xFF;
|
||||
command[9] = degreesVal >> 8;
|
||||
|
||||
qInfo() << "Enter pitch degrees (-90.0 -> 25.0): ";
|
||||
qInfo().noquote().nospace() << "Enter pitch degrees (" << GIMBAL_PITCH_MIN << " -> " << GIMBAL_PITCH_MAX << "): ";
|
||||
std::cin >> degrees;
|
||||
|
||||
if (std::cin.fail() || degrees < -90.0f || degrees > 25.0f) {
|
||||
qWarning() << "Invalid value, using 0.0";
|
||||
if (std::cin.fail() || degrees < GIMBAL_PITCH_MIN || degrees > GIMBAL_PITCH_MAX) {
|
||||
qWarning().noquote().nospace() << "Invalid value, using 0.0";
|
||||
degrees = 0.0f;
|
||||
}
|
||||
|
||||
degreesVal = degrees * 10;
|
||||
command[10] = degreesVal & 0xFF;
|
||||
command[11] = degreesVal >> 8;
|
||||
} else if (commandId == COMMAND_ID::TURN_TO_PIXEL) {
|
||||
float targetX, targetY;
|
||||
|
||||
qInfo().noquote().nospace() << "Enter left_to_right (1 - " << Config::getResolutionWidth() << "): ";
|
||||
std::cin >> targetX;
|
||||
|
||||
if (std::cin.fail() || targetX < 1 || targetX > Config::getResolutionWidth()) {
|
||||
qWarning().noquote().nospace() << "Invalid value, using " << Config::getResolutionWidth() / 2;
|
||||
targetX = Config::getResolutionWidth() / 2;
|
||||
}
|
||||
|
||||
qInfo().noquote().nospace() << "Enter top_to_bottom (1 -> " << Config::getResolutionHeight() << "): ";
|
||||
std::cin >> targetY;
|
||||
|
||||
if (std::cin.fail() || targetY < 1 || targetY > Config::getResolutionHeight()) {
|
||||
qWarning().noquote().nospace() << "Invalid value, using " << Config::getResolutionHeight() / 2;
|
||||
targetY = Config::getResolutionHeight() / 2;
|
||||
}
|
||||
|
||||
float resultYaw = 0.0f;
|
||||
float resultPitch = 0.0f;
|
||||
UtilsTargetLocation::getAnglesToOnScreenTarget(targetX, targetY, resultYaw, resultPitch);
|
||||
|
||||
int16_t degreesVal = resultYaw * 10;
|
||||
command[8] = degreesVal & 0xFF;
|
||||
command[9] = degreesVal >> 8;
|
||||
|
||||
degreesVal = resultPitch * 10;
|
||||
command[10] = degreesVal & 0xFF;
|
||||
command[11] = degreesVal >> 8;
|
||||
} else if (commandId == COMMAND_ID::ZOOM_TO_X) {
|
||||
float zoom;
|
||||
|
||||
qInfo() << "Enter zoom (1.0 -> 6.0): ";
|
||||
qInfo().noquote().nospace() << "Enter zoom (1.0 -> " << Config::getMaxZoom() << "): ";
|
||||
std::cin >> zoom;
|
||||
|
||||
if (std::cin.fail() || zoom < 1.0 || zoom > 6.0) {
|
||||
qWarning() << "Invalid value, using 1.0";
|
||||
if (std::cin.fail() || zoom < 1.0f || zoom > Config::getMaxZoom()) {
|
||||
qWarning().noquote().nospace() << "Invalid value, using 1.0";
|
||||
zoom = 1.0f;
|
||||
}
|
||||
|
||||
uint8_t integerPart = static_cast<uint8_t>(zoom);
|
||||
float fractionalPart = zoom - integerPart;
|
||||
uint8_t scaledFractional = int(fractionalPart * 10);
|
||||
uint8_t scaledFractional = uint8_t(fractionalPart * 10);
|
||||
command[8] = integerPart;
|
||||
command[9] = scaledFractional;
|
||||
}
|
||||
@@ -142,25 +184,35 @@ int16_t SerialCommand::getCommandIndex(COMMAND_ID commandId)
|
||||
return -1;
|
||||
}
|
||||
|
||||
QByteArray SerialCommand::getCommandByID(COMMAND_ID commandId)
|
||||
QByteArray SerialCommand::getCommandForUI(COMMAND_ID commandId)
|
||||
{
|
||||
setExtraValues(commandId);
|
||||
|
||||
uint16_t commandIndex = getCommandIndex(commandId);
|
||||
int16_t commandIndex = getCommandIndex(commandId);
|
||||
if (commandIndex == -1) {
|
||||
qCritical().noquote().nospace() << "Command not found for command: " << commandId;
|
||||
}
|
||||
|
||||
QByteArray command = mSerialCommands.at(commandIndex).command;
|
||||
|
||||
int8_t crcBytes[2];
|
||||
UtilsCRC16::getCRCBytes(command, crcBytes);
|
||||
|
||||
command.resize(command.size() + 2); // Increase array size to accommodate CRC bytes
|
||||
command[command.size() - 2] = crcBytes[0]; // Set LSB
|
||||
command[command.size() - 1] = crcBytes[1]; // Set MSB
|
||||
|
||||
QString commandStr;
|
||||
for (int i = 0; i < command.size(); i++) {
|
||||
commandStr += QString("%1").arg(command.at(i), 2, 16, QChar('0')).toUpper();
|
||||
if (i > 0) {
|
||||
commandStr += ",";
|
||||
}
|
||||
commandStr += QString("0x%1").arg(command.at(i), 2, 16, QChar('0')).toUpper();
|
||||
commandStr.replace("0X", "0x");
|
||||
}
|
||||
qDebug() << "Command: " << commandStr;
|
||||
qDebug().noquote().nospace() << "Command: " << commandStr;
|
||||
|
||||
return command;
|
||||
}
|
||||
|
||||
QByteArray SerialCommand::getCommandForInternal(COMMAND_ID commandId)
|
||||
{
|
||||
int16_t commandIndex = getCommandIndex(commandId);
|
||||
if (commandIndex == -1) {
|
||||
qCritical().noquote().nospace() << "Command not found for command: " << commandId;
|
||||
}
|
||||
return mSerialCommands.at(commandIndex).command;
|
||||
}
|
||||
|
||||
@@ -3,9 +3,10 @@
|
||||
#include <QByteArray>
|
||||
#include <QList>
|
||||
#include <QString>
|
||||
#include "defines.h"
|
||||
#include "defines.hpp"
|
||||
|
||||
struct Command {
|
||||
struct Command
|
||||
{
|
||||
COMMAND_ID id;
|
||||
QByteArray command;
|
||||
QString description;
|
||||
@@ -14,9 +15,9 @@ struct Command {
|
||||
class SerialCommand {
|
||||
public:
|
||||
SerialCommand();
|
||||
~SerialCommand();
|
||||
void printCommands(void);
|
||||
QByteArray getCommandByID(COMMAND_ID commandId);
|
||||
QByteArray getCommandForUI(COMMAND_ID commandId);
|
||||
QByteArray getCommandForInternal(COMMAND_ID commandId);
|
||||
uint8_t getCommandCount();
|
||||
|
||||
private:
|
||||
@@ -1,60 +1,87 @@
|
||||
#include "serialPort.h"
|
||||
#include "serialPort.hpp"
|
||||
#include <QCoreApplication>
|
||||
#include <QDebug>
|
||||
#include "defines.h"
|
||||
#include "defines.hpp"
|
||||
#include "utilsCRC16.hpp"
|
||||
|
||||
SerialPort::SerialPort(const QString &portName)
|
||||
: mSerialPort(portName)
|
||||
SerialPort::SerialPort()
|
||||
: QObject(nullptr)
|
||||
{
|
||||
mSerialPort.setPortName(portName);
|
||||
mSerialPort.setBaudRate(QSerialPort::Baud115200);
|
||||
mSerialPort.setDataBits(QSerialPort::Data8);
|
||||
mSerialPort.setStopBits(QSerialPort::OneStop);
|
||||
mSerialPort.setFlowControl(QSerialPort::NoFlowControl);
|
||||
mSerialPort = new QSerialPort();
|
||||
mSerialPort->setPortName(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() << "SerialPort(): Unable to open port";
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
SerialPort::~SerialPort()
|
||||
{
|
||||
closePort(); // Close port if open on destruction
|
||||
closePort();
|
||||
delete mSerialPort;
|
||||
}
|
||||
|
||||
bool SerialPort::openPort()
|
||||
{
|
||||
if (mSerialPort.isOpen()) {
|
||||
qDebug() << "Port already open";
|
||||
if (mSerialPort->isOpen()) {
|
||||
qDebug().noquote().nospace() << "Port already open";
|
||||
return true;
|
||||
}
|
||||
|
||||
return mSerialPort.open(QIODevice::ReadWrite);
|
||||
return mSerialPort->open(QIODevice::ReadWrite);
|
||||
}
|
||||
|
||||
void SerialPort::closePort()
|
||||
{
|
||||
if (mSerialPort.isOpen()) {
|
||||
mSerialPort.close();
|
||||
if (mSerialPort->isOpen()) {
|
||||
mSerialPort->close();
|
||||
}
|
||||
}
|
||||
|
||||
void SerialPort::sendCommand(const QByteArray &command)
|
||||
{
|
||||
if (!mSerialPort.isOpen()) {
|
||||
qDebug() << "Error: Port not open";
|
||||
QByteArray toSend = command;
|
||||
int8_t crcBytes[2];
|
||||
UtilsCRC16::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(command);
|
||||
mSerialPort->write(toSend);
|
||||
}
|
||||
|
||||
QByteArray SerialPort::readResponse()
|
||||
{
|
||||
if (!mSerialPort.isOpen()) {
|
||||
qDebug() << "Error: Port not open";
|
||||
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(SERIAL_RESPONSE_WAIT_TIME)) { // Adjust timeout as needed
|
||||
response.append(mSerialPort.readAll());
|
||||
while (mSerialPort->waitForReadyRead(SERIAL_RESPONSE_WAIT_TIME)) { // Adjust timeout as needed
|
||||
response.append(mSerialPort->readAll());
|
||||
}
|
||||
|
||||
return response;
|
||||
|
||||
@@ -1,18 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <QByteArray>
|
||||
#include <QSerialPort>
|
||||
|
||||
class SerialPort {
|
||||
public:
|
||||
SerialPort(const QString& portName);
|
||||
~SerialPort();
|
||||
|
||||
bool openPort();
|
||||
void closePort();
|
||||
void sendCommand(const QByteArray& command);
|
||||
QByteArray readResponse();
|
||||
|
||||
private:
|
||||
QSerialPort mSerialPort;
|
||||
};
|
||||
@@ -0,0 +1,24 @@
|
||||
#pragma once
|
||||
|
||||
#include <QByteArray>
|
||||
#include <QObject>
|
||||
#include <QSerialPort>
|
||||
|
||||
class SerialPort : public QObject
|
||||
{
|
||||
Q_OBJECT
|
||||
|
||||
public:
|
||||
SerialPort();
|
||||
~SerialPort();
|
||||
|
||||
bool openPort();
|
||||
void closePort();
|
||||
|
||||
public slots:
|
||||
void sendCommand(const QByteArray &command);
|
||||
QByteArray readResponse();
|
||||
|
||||
private:
|
||||
QSerialPort *mSerialPort;
|
||||
};
|
||||
@@ -1,15 +1,10 @@
|
||||
#include "serialResponse.h"
|
||||
#include "serialResponse.hpp"
|
||||
#include <QDebug>
|
||||
#include "defines.h"
|
||||
#include "utilsCRC16.h"
|
||||
#include "defines.hpp"
|
||||
#include "utilsCRC16.hpp"
|
||||
|
||||
void SerialResponse::printResponse(QByteArray response)
|
||||
{
|
||||
if (response.size() == 0) {
|
||||
qWarning().noquote() << "Response is empty";
|
||||
return;
|
||||
}
|
||||
|
||||
QHash<QString, QVariant> results = getResponceValues(response);
|
||||
|
||||
QList<QString> keys = results.keys();
|
||||
@@ -37,6 +32,12 @@ void SerialResponse::printResponse(QByteArray response)
|
||||
|
||||
QHash<QString, QVariant> SerialResponse::getResponceValues(QByteArray response)
|
||||
{
|
||||
QHash<QString, QVariant> results;
|
||||
|
||||
if (response.size() == 0) {
|
||||
qCritical().noquote().nospace() << "Response is empty, exiting...";
|
||||
}
|
||||
|
||||
// Check response data validity
|
||||
int8_t crcCheck[2];
|
||||
uint8_t desiredLength = response.size() - 2;
|
||||
@@ -49,61 +50,57 @@ QHash<QString, QVariant> SerialResponse::getResponceValues(QByteArray response)
|
||||
|
||||
// Data not OK
|
||||
if (crcCheck[0] != crcOriginal[0] || crcCheck[1] != crcOriginal[1]) {
|
||||
qWarning() << "Response data INVALID";
|
||||
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() << responseCRC << "!=" << recalcCRC;
|
||||
qWarning().noquote().nospace() << responseCRC << "!=" << recalcCRC;
|
||||
}
|
||||
|
||||
QHash<QString, QVariant> results;
|
||||
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);
|
||||
results.insert("yaw", (float) (yaw / 10));
|
||||
|
||||
int16_t pitch = ((uint8_t) response.at(11) << 8) | (uint8_t) response.at(10);
|
||||
results.insert("pitch", (float) (pitch / 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);
|
||||
results.insert("yaw", (float) (yaw / 10));
|
||||
|
||||
int16_t pitch = ((uint8_t) response.at(11) << 8) | (uint8_t) response.at(10);
|
||||
results.insert("pitch", (float) (pitch / 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));
|
||||
|
||||
int16_t yaw_speed = ((uint8_t) response.at(15) << 8) | (uint8_t) response.at(14);
|
||||
results.insert("yaw_speed", (float) (yaw_speed / 10));
|
||||
|
||||
int16_t pitch_speed = ((uint8_t) response.at(17) << 8) | (uint8_t) response.at(16);
|
||||
results.insert("pitch_speed", (float) (pitch_speed / 10));
|
||||
|
||||
int16_t roll_speed = ((uint8_t) response.at(19) << 8) | (uint8_t) response.at(18);
|
||||
results.insert("roll_speed", (float) (roll_speed / 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 == 0x18) {
|
||||
} 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);
|
||||
results.insert("width", width);
|
||||
|
||||
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++) {
|
||||
responseStr += QString("%1").arg(response.at(i), 2, 16, QChar('0')).toUpper();
|
||||
if (i > 0) {
|
||||
responseStr += ",";
|
||||
}
|
||||
responseStr += QString("0x%1").arg(response.at(i), 2, 16, QChar('0')).toUpper();
|
||||
responseStr.replace("0X", "0x");
|
||||
}
|
||||
results.insert("Response", responseStr);
|
||||
qWarning().noquote().nospace() << "Responce byte array: " << responseStr;
|
||||
}
|
||||
|
||||
return results;
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
#include "utilsCRC16.h"
|
||||
#include "utilsCRC16.hpp"
|
||||
|
||||
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,
|
||||
|
||||
@@ -1,48 +1,12 @@
|
||||
#include "utilsTargetLocation.h"
|
||||
#include "utilsTargetLocation.hpp"
|
||||
#include <QDebug>
|
||||
#include <QImage>
|
||||
#include "serialCommand.h"
|
||||
#include "serialResponse.h"
|
||||
|
||||
#define USE_OPENCV
|
||||
//#define USE_FFMPEG
|
||||
|
||||
#ifdef USE_OPENCV
|
||||
#include "config.hpp"
|
||||
#include "defines.hpp"
|
||||
#include <opencv2/opencv.hpp>
|
||||
#endif
|
||||
|
||||
#ifdef USE_FFMPEG
|
||||
extern "C" {
|
||||
#include <libavcodec/avcodec.h>
|
||||
#include <libavformat/avformat.h>
|
||||
#include <libavutil/imgutils.h>
|
||||
#include <libswscale/swscale.h>
|
||||
}
|
||||
#endif
|
||||
|
||||
UtilsTargetLocation::UtilsTargetLocation(SerialPort *serial, SerialCommand *command)
|
||||
: mSerial(serial)
|
||||
, mCommand(command)
|
||||
{
|
||||
// ... Optional: Additional initialization logic here
|
||||
}
|
||||
|
||||
GPSData UtilsTargetLocation::getLocation(float altitude, float latitude, float lognitude, float yaw, float pitch, float roll, float targetTrueSize, uint16_t targetPixelSize)
|
||||
{
|
||||
if (mSerial == nullptr) {
|
||||
qWarning() << "Cannot get geo location: Serial connection not available.";
|
||||
}
|
||||
|
||||
if (mCommand == nullptr) {
|
||||
qWarning() << "Cannot get geo location: Commands not available.";
|
||||
}
|
||||
|
||||
// Capture image from rtsp stream
|
||||
captureImageFromStream();
|
||||
|
||||
// TODO?
|
||||
// Send image to AI for target identification and/or target choosing
|
||||
|
||||
// From the drone and camera
|
||||
CameraData cameraData = getCameraData();
|
||||
GPSData gpsData = {latitude, lognitude, altitude};
|
||||
@@ -60,33 +24,12 @@ GPSData UtilsTargetLocation::getLocation(float altitude, float latitude, float l
|
||||
|
||||
CameraData UtilsTargetLocation::getCameraData()
|
||||
{
|
||||
uint16_t height = CAMERA_RESOLUTION_HEIGHT;
|
||||
uint16_t width = CAMERA_RESOLUTION_WIDTH;
|
||||
float pitch = 0;
|
||||
float yaw = 0;
|
||||
float fov = CAMERA_FIELD_OF_VIEW_HORIZONTAL;
|
||||
|
||||
QByteArray response;
|
||||
QHash<QString, QVariant> responseValues;
|
||||
mSerial->sendCommand(mCommand->getCommandByID(COMMAND_ID::ACQUIRE_CURRENT_ZOOM)); // Field of view in degrees
|
||||
response = mSerial->readResponse();
|
||||
responseValues = SerialResponse::getResponceValues(response);
|
||||
SerialResponse::printResponse(response);
|
||||
fov = CAMERA_FIELD_OF_VIEW_HORIZONTAL / (responseValues["zoom"].toFloat() == 0.0 ? 1.0 : responseValues["zoom"].toFloat());
|
||||
|
||||
mSerial->sendCommand(mCommand->getCommandByID(COMMAND_ID::ACQUIRE_ATTITUDE_DATA)); // Yaw and pitch in degrees
|
||||
response = mSerial->readResponse();
|
||||
responseValues = SerialResponse::getResponceValues(response);
|
||||
SerialResponse::printResponse(response);
|
||||
yaw = responseValues["yaw"].toFloat();
|
||||
pitch = responseValues["pitch"].toFloat();
|
||||
|
||||
mSerial->sendCommand(mCommand->getCommandByID(COMMAND_ID::ACQUIRE_CAMERA_CODEC_SPECS)); // Camera resolution in pixels
|
||||
response = mSerial->readResponse();
|
||||
responseValues = SerialResponse::getResponceValues(response);
|
||||
SerialResponse::printResponse(response);
|
||||
height = responseValues["height"].toUInt();
|
||||
width = responseValues["width"].toUInt();
|
||||
uint16_t height = Config::getResolutionHeight();
|
||||
uint16_t width = Config::getResolutionWidth();
|
||||
float yaw = Config::getCurrentYaw();
|
||||
float pitch = Config::getCurrentPitch();
|
||||
float zoom = Config::getCurrentZoom();
|
||||
float fov = CAMERA_FIELD_OF_VIEW_HORIZONTAL / zoom;
|
||||
|
||||
return {height, width, pitch, yaw, fov};
|
||||
}
|
||||
@@ -107,7 +50,7 @@ float UtilsTargetLocation::degreesToRadians(float degrees)
|
||||
// Function to calculate the new GPS location
|
||||
GPSData UtilsTargetLocation::calculateTargetLocation(DroneData drone, CameraData camera, float distance, float bearing)
|
||||
{
|
||||
constexpr float R = 6371000.0; // Earth radius in meters
|
||||
constexpr float R = 6371000.0f; // Earth radius in meters
|
||||
|
||||
float bearingRad = degreesToRadians(bearing);
|
||||
float latRad = degreesToRadians(drone.gps.latitude);
|
||||
@@ -116,7 +59,7 @@ GPSData UtilsTargetLocation::calculateTargetLocation(DroneData drone, CameraData
|
||||
float newLatRad = asin(sin(latRad) * cos(distance / R) + cos(latRad) * sin(distance / R) * cos(bearingRad));
|
||||
float newLonRad = lonRad + atan2(sin(bearingRad) * sin(distance / R) * cos(latRad), cos(distance / R) - sin(latRad) * sin(newLatRad));
|
||||
|
||||
float angleRad = camera.pitch * M_PI / 180.0;
|
||||
float angleRad = camera.pitch * M_PI / 180.0f;
|
||||
float horizontalDistance = distance * cos(angleRad);
|
||||
float newAltitude = drone.gps.altitude + (horizontalDistance * tan(angleRad));
|
||||
|
||||
@@ -128,157 +71,27 @@ GPSData UtilsTargetLocation::calculateTargetLocation(DroneData drone, CameraData
|
||||
return newLocation;
|
||||
}
|
||||
|
||||
void UtilsTargetLocation::captureImageFromStream()
|
||||
void UtilsTargetLocation::getAnglesToOnScreenTarget(uint16_t targetX, uint16_t targetY, float &resultYaw, float &resultPitch)
|
||||
{
|
||||
#ifdef USE_OPENCV
|
||||
cv::VideoCapture cap;
|
||||
cap.open(RTSP_ADDRESS);
|
||||
resultYaw = Config::getCurrentYaw();
|
||||
resultPitch = Config::getCurrentPitch();
|
||||
|
||||
if (!cap.isOpened()) {
|
||||
qWarning() << "Error: Could not open RTSP stream";
|
||||
return;
|
||||
}
|
||||
// Normalize target pixel location to [-0.5, 0.5] range
|
||||
double normPixelX = (targetX - Config::getResolutionWidth() / 2.0f) / (Config::getResolutionWidth() / 2.0f);
|
||||
double normPixelY = (targetY - Config::getResolutionHeight() / 2.0f) / (Config::getResolutionHeight() / 2.0f);
|
||||
|
||||
// Hack to capture proper image
|
||||
// For some reason first frames get corrupted
|
||||
uint8_t frameCount = 0;
|
||||
// Adjust horizontal field of view for zoom
|
||||
double horizontalFov = CAMERA_FIELD_OF_VIEW_HORIZONTAL * (1.0f + (Config::getCurrentZoom() - 1.0f) / 5.0f);
|
||||
|
||||
while (true) {
|
||||
cv::Mat frame;
|
||||
// Calculate image plane dimensions based on focal length and aspect ratio
|
||||
double imagePlaneWidth = 2.0f * CAMERA_FOCAL_LENGTH * tan(horizontalFov / 2.0f * M_PI / 180.0f);
|
||||
double imagePlaneHeight = imagePlaneWidth / CAMERA_ASPECT_RATIO;
|
||||
|
||||
if (cap.grab() && cap.retrieve(frame)) {
|
||||
if (frame.empty() || frame.total() == 0) {
|
||||
qInfo() << "Warning: Invalid frame captured, retrying...";
|
||||
} else {
|
||||
// Convert the frame to BGR if needed
|
||||
if (frame.channels() == 1) {
|
||||
qInfo() << "Captured frame is grayscale, converting to BGR";
|
||||
cv::cvtColor(frame, frame, cv::COLOR_GRAY2BGR);
|
||||
} else if (frame.channels() == 4) {
|
||||
qInfo() << "Captured frame is RGBA, converting to BGR";
|
||||
cv::cvtColor(frame, frame, cv::COLOR_RGBA2BGR);
|
||||
}
|
||||
// Calculate angle offsets based on normalized pixel location and image plane
|
||||
// dimensions
|
||||
float turnX = atan2(normPixelX * imagePlaneWidth / 2.0f, CAMERA_FOCAL_LENGTH) * 180.0f / M_PI;
|
||||
float turnY = atan2(normPixelY * imagePlaneHeight / 2.0f, CAMERA_FOCAL_LENGTH) * 180.0f / M_PI;
|
||||
|
||||
frameCount++;
|
||||
if (frameCount > 10) {
|
||||
// Save the captured frame as an image
|
||||
std::string filename = "output_opencv.jpg";
|
||||
if (cv::imwrite(filename, frame)) {
|
||||
qInfo() << "Image captured and saved as" << QString::fromStdString(filename);
|
||||
break;
|
||||
} else {
|
||||
qInfo() << "Error: Failed to save the captured image";
|
||||
}
|
||||
} else if (frameCount > 100) {
|
||||
qInfo() << "Error: Tried 100 times and still fails, giving up";
|
||||
break;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
qInfo() << "Warning: Could not grab or retrieve frame, retrying...";
|
||||
}
|
||||
}
|
||||
|
||||
// Release the video capture object
|
||||
cap.release();
|
||||
#endif
|
||||
|
||||
#ifdef USE_FFMPEG
|
||||
// Initialize FFmpeg
|
||||
avformat_network_init();
|
||||
|
||||
// Open RTSP stream
|
||||
AVFormatContext *formatContext = avformat_alloc_context();
|
||||
if (avformat_open_input(&formatContext, RTSP_ADDRESS, NULL, NULL) != 0) {
|
||||
qDebug() << "Error: Couldn't open RTSP stream";
|
||||
return;
|
||||
}
|
||||
|
||||
// Find stream info
|
||||
if (avformat_find_stream_info(formatContext, NULL) < 0) {
|
||||
qDebug() << "Error: Couldn't find stream info";
|
||||
return;
|
||||
}
|
||||
|
||||
// Find video stream
|
||||
int videoStreamIndex = -1;
|
||||
for (uint i = 0; i < formatContext->nb_streams; i++) {
|
||||
if (formatContext->streams[i]->codecpar->codec_type == AVMEDIA_TYPE_VIDEO) {
|
||||
videoStreamIndex = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (videoStreamIndex == -1) {
|
||||
qDebug() << "Error: Couldn't find video stream";
|
||||
return;
|
||||
}
|
||||
|
||||
// Initialize video decoder
|
||||
AVCodecParameters *codecParameters = formatContext->streams[videoStreamIndex]->codecpar;
|
||||
AVCodec *codec = avcodec_find_decoder(codecParameters->codec_id);
|
||||
AVCodecContext *codecContext = avcodec_alloc_context3(codec);
|
||||
avcodec_parameters_to_context(codecContext, codecParameters);
|
||||
avcodec_open2(codecContext, codec, NULL);
|
||||
|
||||
// Allocate frame
|
||||
AVFrame *frame = av_frame_alloc();
|
||||
|
||||
bool shouldBreak = false;
|
||||
|
||||
// Read frames
|
||||
AVPacket packet;
|
||||
|
||||
while (shouldBreak == false && av_read_frame(formatContext, &packet) >= 0) {
|
||||
if (packet.stream_index == videoStreamIndex) {
|
||||
if (avcodec_send_packet(codecContext, &packet) >= 0) {
|
||||
while (avcodec_receive_frame(codecContext, frame) >= 0) {
|
||||
// Convert frame to RGB if necessary
|
||||
if (codecParameters->format != AV_PIX_FMT_RGB24) {
|
||||
AVFrame *rgbFrame = av_frame_alloc();
|
||||
int numBytes = av_image_get_buffer_size(AV_PIX_FMT_RGB24, codecParameters->width, codecParameters->height, 1);
|
||||
uint8_t *buffer = (uint8_t *) av_malloc(numBytes * sizeof(uint8_t));
|
||||
if (av_image_fill_arrays(rgbFrame->data, rgbFrame->linesize, buffer, AV_PIX_FMT_RGB24, codecParameters->width, codecParameters->height, 1) > 0) {
|
||||
// Convert format to enum AVPixelFormat
|
||||
const AVPixFmtDescriptor *pixDesc = av_pix_fmt_desc_get((AVPixelFormat) codecParameters->format);
|
||||
if (!pixDesc) {
|
||||
qDebug() << "Error: Unsupported pixel format";
|
||||
return;
|
||||
}
|
||||
|
||||
// Get the AVPixelFormat enum value directly from the
|
||||
descriptor AVPixelFormat pixelFormat = (AVPixelFormat) codecParameters->format;
|
||||
|
||||
// Create the sws context
|
||||
struct SwsContext *swsContext = sws_getContext(codecParameters->width, codecParameters->height, pixelFormat, codecParameters->width, codecParameters->height, AV_PIX_FMT_RGB24, SWS_BILINEAR, NULL, NULL, NULL);
|
||||
sws_scale(swsContext, frame->data, frame->linesize, 0, codecParameters->height, rgbFrame->data, rgbFrame->linesize);
|
||||
sws_freeContext(swsContext);
|
||||
|
||||
// Save frame as JPEG
|
||||
QImage image(rgbFrame->data[0], codecParameters->width, codecParameters->height, QImage::Format_RGB888);
|
||||
image.save("output_ffmpeg.jpg");
|
||||
|
||||
av_free(buffer);
|
||||
av_frame_free(&rgbFrame);
|
||||
shouldBreak = true;
|
||||
break;
|
||||
}
|
||||
} else {
|
||||
// Save frame as JPEG
|
||||
QImage image(frame->data[0], codecParameters->width, codecParameters->height, QImage::Format_RGB888);
|
||||
image.save("output_ffmpeg.jpg");
|
||||
shouldBreak = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
av_packet_unref(&packet);
|
||||
}
|
||||
|
||||
// Clean up
|
||||
avformat_close_input(&formatContext);
|
||||
avcodec_free_context(&codecContext);
|
||||
av_frame_free(&frame);
|
||||
#endif
|
||||
resultYaw -= turnX;
|
||||
resultPitch -= turnY;
|
||||
}
|
||||
|
||||
@@ -1,47 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include "serialCommand.h"
|
||||
#include "serialPort.h"
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
struct GPSData
|
||||
{
|
||||
float latitude; // Decimal degrees
|
||||
float longitude; // Decimal degrees
|
||||
float altitude; // Meters
|
||||
};
|
||||
|
||||
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 UtilsTargetLocation
|
||||
{
|
||||
public:
|
||||
UtilsTargetLocation(SerialPort *serial, SerialCommand *command);
|
||||
GPSData getLocation(float altitude, float latitude, float lognitude, float yaw, float pitch, float roll, float targetTrueSize, uint16_t targetPixelSize);
|
||||
|
||||
private:
|
||||
CameraData getCameraData();
|
||||
float calculateTargetDistance(float targetSize, uint16_t targetPixelSize, uint16_t imageWidth, float fov);
|
||||
float degreesToRadians(float degrees);
|
||||
GPSData calculateTargetLocation(DroneData drone, CameraData camera, float distance, float bearing);
|
||||
void captureImageFromStream();
|
||||
|
||||
SerialPort *mSerial;
|
||||
SerialCommand *mCommand;
|
||||
};
|
||||
@@ -0,0 +1,40 @@
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
struct GPSData
|
||||
{
|
||||
float latitude; // Decimal degrees
|
||||
float longitude; // Decimal degrees
|
||||
float altitude; // Meters
|
||||
};
|
||||
|
||||
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 UtilsTargetLocation
|
||||
{
|
||||
public:
|
||||
static GPSData getLocation(float altitude, float latitude, float lognitude, float yaw, float pitch, float roll, float targetTrueSize, uint16_t targetPixelSize);
|
||||
static void getAnglesToOnScreenTarget(uint16_t targetX, uint16_t targetY, float &resultYaw, float &resultPitch);
|
||||
|
||||
private:
|
||||
static CameraData getCameraData();
|
||||
static float calculateTargetDistance(float targetSize, uint16_t targetPixelSize, uint16_t imageWidth, float fov);
|
||||
static float degreesToRadians(float degrees);
|
||||
static GPSData calculateTargetLocation(DroneData drone, CameraData camera, float distance, float bearing);
|
||||
};
|
||||
Reference in New Issue
Block a user