mirror of
https://github.com/azaion/autopilot.git
synced 2026-04-22 10:06:34 +00:00
Changed FIFO pipes to UDP.
This commit is contained in:
Nffj84
@@ -12,83 +12,87 @@
|
||||
#include <sys/stat.h>
|
||||
#include <unistd.h>
|
||||
|
||||
RemoteControl::RemoteControl()
|
||||
: QObject(nullptr)
|
||||
RemoteControl::RemoteControl(QObject *parent)
|
||||
: QObject{parent}
|
||||
{
|
||||
openNamedPipe();
|
||||
mUdpSocket = new QUdpSocket(this);
|
||||
connect(mUdpSocket, &QUdpSocket::readyRead, this, &RemoteControl::readPendingDatagrams);
|
||||
mUdpSocket->bind(QHostAddress::Any, UDP_PORT);
|
||||
}
|
||||
|
||||
RemoteControl::~RemoteControl()
|
||||
void RemoteControl::readPendingDatagrams()
|
||||
{
|
||||
if (mFifoFdIn != -1) {
|
||||
close(mFifoFdIn);
|
||||
}
|
||||
if (mFifoFdOut != -1) {
|
||||
close(mFifoFdOut);
|
||||
while (mUdpSocket->hasPendingDatagrams()) {
|
||||
QByteArray datagram;
|
||||
datagram.resize(mUdpSocket->pendingDatagramSize());
|
||||
mUdpSocket->readDatagram(datagram.data(), datagram.size());
|
||||
|
||||
QJsonParseError jsonError;
|
||||
QJsonDocument jsonDoc = QJsonDocument::fromJson(datagram, &jsonError);
|
||||
if (jsonError.error != QJsonParseError::NoError) {
|
||||
qWarning() << "Error parsing JSON:" << jsonError.errorString();
|
||||
continue;
|
||||
}
|
||||
|
||||
if (jsonDoc.isObject()) {
|
||||
if (mIsBusy == false) {
|
||||
processJSON(jsonDoc);
|
||||
}
|
||||
} else {
|
||||
qWarning().noquote().nospace() << "Received data was not JSON object.";
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void RemoteControl::openNamedPipe()
|
||||
void RemoteControl::processJSON(QJsonDocument commandDoc)
|
||||
{
|
||||
struct stat statBuf;
|
||||
mIsBusy = true;
|
||||
qDebug().noquote().nospace() << "Received: " << commandDoc.toJson().toStdString();
|
||||
|
||||
// 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;
|
||||
}
|
||||
QJsonObject commandObject = commandDoc.object();
|
||||
|
||||
// Exit with exit message
|
||||
if (commandObject.contains("extra") == true && commandObject["extra"] == "EXIT") {
|
||||
qDebug().noquote().nospace() << "Exit message received: Exiting...";
|
||||
exit(EXIT_SUCCESS);
|
||||
}
|
||||
|
||||
mFifoFdIn = open(FIFO_TO_GIMBAL, O_RDWR | 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;
|
||||
}
|
||||
// TODO:
|
||||
// Wait that these are actually available
|
||||
// Without these it is impossible to calculate target location
|
||||
commandObject["altitude"] = 10.5f;
|
||||
commandObject["latitude"] = 55.75000000f;
|
||||
commandObject["longitude"] = 37.61666670f;
|
||||
commandObject["pitch"] = 0.0f;
|
||||
commandObject["yaw"] = 152.5f;
|
||||
commandObject["target_real_height"] = 2.5f;
|
||||
commandObject["target_real_width"] = 5.0f;
|
||||
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
// Rectangle calculation for having proper zoom on group / target
|
||||
RectangleProperties rectangle = calculateRectangleProperties(commandObject["top"].toInt(), commandObject["left"].toInt(), commandObject["bottom"].toInt(), commandObject["right"].toInt());
|
||||
commandObject["target_x"] = rectangle.middleX;
|
||||
commandObject["target_y"] = rectangle.middleY;
|
||||
commandObject["target_pixel_height"] = rectangle.height;
|
||||
commandObject["target_pixel_width"] = rectangle.width;
|
||||
qInfo() << "target_x: " << commandObject["target_x"];
|
||||
qInfo() << "target_y: " << commandObject["target_y"];
|
||||
qInfo() << "target_pixel_height: " << commandObject["target_pixel_height"];
|
||||
qInfo() << "target_pixel_width: " << commandObject["target_pixel_width"];
|
||||
|
||||
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;
|
||||
}
|
||||
// Prepare responce object
|
||||
mResponseObject = QJsonObject();
|
||||
mResponseObject["sender"] = UDP_WHO_AM_I;
|
||||
mResponseObject["status"] = "OK";
|
||||
|
||||
// Get current orientation and zoom
|
||||
Config::updateState();
|
||||
|
||||
QTimer::singleShot(0, this, [this, commandObject]() mutable { turnToTarget(commandObject); });
|
||||
}
|
||||
|
||||
void RemoteControl::sendResponse(void)
|
||||
{
|
||||
QJsonDocument responseDocument(mResponseObject);
|
||||
std::string response = responseDocument.toJson(QJsonDocument::Compact).toStdString();
|
||||
|
||||
size_t bytesWritten = write(mFifoFdOut, response.c_str(), response.size());
|
||||
if (bytesWritten < 1) {
|
||||
qWarning().noquote().nospace() << "Error writing response: " << bytesWritten;
|
||||
}
|
||||
qDebug().noquote().nospace() << "Responded: " << response.c_str();
|
||||
// TODO: Implement
|
||||
mIsBusy = false;
|
||||
}
|
||||
|
||||
@@ -242,83 +246,3 @@ RectangleProperties RemoteControl::calculateRectangleProperties(uint16_t top, ui
|
||||
|
||||
return properties;
|
||||
}
|
||||
|
||||
void RemoteControl::run()
|
||||
{
|
||||
mIsBusy = false;
|
||||
|
||||
while (true) {
|
||||
if (mIsBusy == false) {
|
||||
char buffer[1024];
|
||||
ssize_t bytesRead = read(mFifoFdIn, buffer, sizeof(buffer) - 1);
|
||||
|
||||
if (bytesRead > 0) {
|
||||
mIsBusy = true;
|
||||
buffer[bytesRead] = '\0';
|
||||
|
||||
QJsonDocument commandDoc = QJsonDocument::fromJson(buffer);
|
||||
QString message = QString::fromUtf8(buffer);
|
||||
|
||||
// Ignore non json messages
|
||||
if (commandDoc.isNull() == false) {
|
||||
qDebug().noquote().nospace() << "Received: " << message;
|
||||
|
||||
QJsonObject commandObject = commandDoc.object();
|
||||
|
||||
// Ignore messages that don't have sender
|
||||
if (commandObject.contains("sender") == false) {
|
||||
qDebug().noquote().nospace() << "No sender info: " << message;
|
||||
mIsBusy = false;
|
||||
continue;
|
||||
}
|
||||
|
||||
// Exit with exit message
|
||||
if (commandObject.contains("extra") == true && commandObject["extra"] == "EXIT") {
|
||||
qDebug().noquote().nospace() << "Exit message received: Exiting...";
|
||||
exit(EXIT_SUCCESS);
|
||||
}
|
||||
|
||||
// TODO:
|
||||
// Wait that these are actually available
|
||||
// Without these it is impossible to calculate target location
|
||||
commandObject["altitude"] = 10.5f;
|
||||
commandObject["latitude"] = 55.75000000f;
|
||||
commandObject["longitude"] = 37.61666670f;
|
||||
commandObject["pitch"] = 0.0f;
|
||||
commandObject["yaw"] = 152.5f;
|
||||
commandObject["target_real_height"] = 2.5f;
|
||||
commandObject["target_real_width"] = 5.0f;
|
||||
|
||||
// Rectangle calculation for having proper zoom on group / target
|
||||
RectangleProperties rectangle = calculateRectangleProperties(commandObject["top"].toInt(), commandObject["left"].toInt(), commandObject["bottom"].toInt(), commandObject["right"].toInt());
|
||||
commandObject["target_x"] = rectangle.middleX;
|
||||
commandObject["target_y"] = rectangle.middleY;
|
||||
commandObject["target_pixel_height"] = rectangle.height;
|
||||
commandObject["target_pixel_width"] = rectangle.width;
|
||||
qInfo() << "target_x: " << commandObject["target_x"];
|
||||
qInfo() << "target_y: " << commandObject["target_y"];
|
||||
qInfo() << "target_pixel_height: " << commandObject["target_pixel_height"];
|
||||
qInfo() << "target_pixel_width: " << commandObject["target_pixel_width"];
|
||||
|
||||
// Prepare responce object
|
||||
mResponseObject = QJsonObject();
|
||||
mResponseObject["sender"] = FIFO_WHO_AM_I;
|
||||
mResponseObject["status"] = "OK";
|
||||
|
||||
// Get current orientation and zoom
|
||||
Config::updateState();
|
||||
|
||||
QTimer::singleShot(0, this, [this, commandObject]() mutable { turnToTarget(commandObject); });
|
||||
} else {
|
||||
qDebug().noquote().nospace() << "Non JSON message received: " << message;
|
||||
mIsBusy = false;
|
||||
}
|
||||
} else {
|
||||
mIsBusy = false;
|
||||
}
|
||||
}
|
||||
|
||||
QCoreApplication::processEvents();
|
||||
QThread::msleep(100);
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user