mirror of
https://github.com/azaion/autopilot.git
synced 2026-04-22 11:46:34 +00:00
Refactored a8 codes and added remote testing app a8_remote.
This commit is contained in:
Nffj84
@@ -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();
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user