commit 6d540f8302d23080141174b83cccdc009ab56ad7 Author: zhangxusong <328180668@qq.com> Date: Tue Sep 16 19:08:32 2025 +0800 基本通讯框架构建 diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..1119bbb --- /dev/null +++ b/.gitignore @@ -0,0 +1,6 @@ +*.pro.* +*.bak +*.asv +build/ +*~ + diff --git a/FlightTap.pro b/FlightTap.pro new file mode 100644 index 0000000..98244f8 --- /dev/null +++ b/FlightTap.pro @@ -0,0 +1,44 @@ +QT += core gui + +greaterThan(QT_MAJOR_VERSION, 4): QT += widgets core network serialport + + +CONFIG += c++17 + +# You can make your code fail to compile if it uses deprecated APIs. +# In order to do so, uncomment the following line. +#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000 # disables all the APIs deprecated before Qt 6.0.0 + +# 头文件搜索路径 +INCLUDEPATH += \ + $$PWD/src \ + $$PWD/src/core \ + $$PWD/src/ui + +SOURCES += \ + src/core/Injector.cpp \ + src/core/MavlinkTap.cpp \ + src/core/ProxyConnection.cpp \ + src/core/ProxyService.cpp \ + src/core/SerialEndpoint.cpp \ + src/core/UdpHubEndpoint.cpp \ + src/main.cpp \ + src/ui/mainwindow.cpp + +HEADERS += \ + src/core/Endpoint.hpp \ + src/core/Injector.hpp \ + src/core/MavlinkTap.hpp \ + src/core/ProxyConnection.hpp \ + src/core/ProxyService.hpp \ + src/core/SerialEndpoint.hpp \ + src/core/UdpHubEndpoint.hpp \ + src/ui/mainwindow.h + +FORMS += \ + src/ui/mainwindow.ui + +# Default rules for deployment. +qnx: target.path = /tmp/$${TARGET}/bin +else: unix:!android: target.path = /opt/$${TARGET}/bin +!isEmpty(target.path): INSTALLS += target diff --git a/src/core/Endpoint.hpp b/src/core/Endpoint.hpp new file mode 100644 index 0000000..6b5f56e --- /dev/null +++ b/src/core/Endpoint.hpp @@ -0,0 +1,25 @@ +#pragma once +#include +#include +#include + +class Endpoint : public QObject { + Q_OBJECT +public: + using RxHandler = std::function; + explicit Endpoint(QObject* parent = nullptr) : QObject(parent) {} + virtual ~Endpoint() = default; + + virtual bool open() = 0; + virtual void close() = 0; + virtual bool isOpen() const = 0; + virtual qint64 writeBytes(const QByteArray& data) = 0; + + void setRxHandler(RxHandler cb) { rx_ = std::move(cb); } + +protected: + void emitRx(const QByteArray& data) { if (rx_) rx_(data); } + +private: + RxHandler rx_; +}; diff --git a/src/core/Injector.cpp b/src/core/Injector.cpp new file mode 100644 index 0000000..bfe37fc --- /dev/null +++ b/src/core/Injector.cpp @@ -0,0 +1,14 @@ +#include "Injector.hpp" + +Injector::Injector(Endpoint* target, uint8_t sysid, uint8_t compid, QObject* parent) + : QObject(parent), target_(target), sysid_(sysid), compid_(compid) {} + +bool Injector::sendHeartbeat(uint8_t type, uint8_t autopilot, uint8_t base_mode, + uint32_t custom_mode, uint8_t system_status) { + if (!target_ || !target_->isOpen()) return false; + mavlink_message_t msg; + mavlink_msg_heartbeat_pack(sysid_, compid_, &msg, type, autopilot, base_mode, custom_mode, system_status); + uint8_t buf[MAVLINK_MAX_PACKET_LEN]; + const int len = mavlink_msg_to_send_buffer(buf, &msg); + return target_->writeBytes(QByteArray(reinterpret_cast(buf), len)) == len; +} diff --git a/src/core/Injector.hpp b/src/core/Injector.hpp new file mode 100644 index 0000000..ee18cfa --- /dev/null +++ b/src/core/Injector.hpp @@ -0,0 +1,19 @@ +#pragma once +#include +#include "Endpoint.hpp" +extern "C" { +#include "mavlink/common/mavlink.h" +} + +class Injector : public QObject { + Q_OBJECT +public: + Injector(Endpoint* target, uint8_t sysid, uint8_t compid, QObject* parent = nullptr); + bool sendHeartbeat(uint8_t type, uint8_t autopilot, uint8_t base_mode, + uint32_t custom_mode, uint8_t system_status); + +private: + Endpoint* target_; + uint8_t sysid_; + uint8_t compid_; +}; diff --git a/src/core/MavlinkTap.cpp b/src/core/MavlinkTap.cpp new file mode 100644 index 0000000..21b33cd --- /dev/null +++ b/src/core/MavlinkTap.cpp @@ -0,0 +1,16 @@ +#include "MavlinkTap.hpp" +#include + +MavlinkTap::MavlinkTap(QObject* parent) : QObject(parent) { + std::memset(&status_, 0, sizeof(status_)); +} + +void MavlinkTap::feed(const QByteArray& data) { + QMutexLocker locker(&mtx_); + mavlink_message_t msg; + for (unsigned char c : data) { + if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status_)) { + emit messageParsed(msg); + } + } +} diff --git a/src/core/MavlinkTap.hpp b/src/core/MavlinkTap.hpp new file mode 100644 index 0000000..1219919 --- /dev/null +++ b/src/core/MavlinkTap.hpp @@ -0,0 +1,22 @@ +#pragma once +#include +#include +#include + +extern "C" { +#include "mavlink/common/mavlink.h" +} + +class MavlinkTap : public QObject { + Q_OBJECT +public: + explicit MavlinkTap(QObject* parent = nullptr); + void feed(const QByteArray& data); // 喂入原始字节（任意方向） + +signals: + void messageParsed(const mavlink_message_t& msg); + +private: + QMutex mtx_; + mavlink_status_t status_; +}; diff --git a/src/core/ProxyConnection.cpp b/src/core/ProxyConnection.cpp new file mode 100644 index 0000000..0b3d237 --- /dev/null +++ b/src/core/ProxyConnection.cpp @@ -0,0 +1,36 @@ +#include "ProxyConnection.hpp" +#include + +ProxyConnection::ProxyConnection(Endpoint* a, Endpoint* b, MavlinkTap* tap, QObject* parent) + : QObject(parent), A_(a), B_(b), tap_(tap) {} + +bool ProxyConnection::start() { + if (!A_ || !B_) return false; + if (!A_->open() || !B_->open()) { + qWarning() << "ProxyConnection: failed to open endpoints"; + return false; + } + running_ = true; + + A_->setRxHandler([this](const QByteArray& data){ + if (!running_) return; + if (tap_) tap_->feed(data); // 仅监控解析 + auto n = B_->writeBytes(data); // 原样转发 + emit bytesForwardedAB(n); + }); + + B_->setRxHandler([this](const QByteArray& data){ + if (!running_) return; + if (tap_) tap_->feed(data); + auto n = A_->writeBytes(data); + emit bytesForwardedBA(n); + }); + + return true; +} + +void ProxyConnection::stop() { + running_ = false; + if (A_->isOpen()) A_->close(); + if (B_->isOpen()) B_->close(); +} diff --git a/src/core/ProxyConnection.hpp b/src/core/ProxyConnection.hpp new file mode 100644 index 0000000..e62cda9 --- /dev/null +++ b/src/core/ProxyConnection.hpp @@ -0,0 +1,24 @@ +#pragma once +#include +#include "Endpoint.hpp" +#include "MavlinkTap.hpp" + +class ProxyConnection : public QObject { + Q_OBJECT +public: + ProxyConnection(Endpoint* a, Endpoint* b, MavlinkTap* tap = nullptr, QObject* parent = nullptr); + ~ProxyConnection() override = default; + + bool start(); + void stop(); + +signals: + void bytesForwardedAB(qint64 n); + void bytesForwardedBA(qint64 n); + +private: + Endpoint* A_; + Endpoint* B_; + MavlinkTap* tap_; + bool running_ = false; +}; diff --git a/src/core/ProxyService.cpp b/src/core/ProxyService.cpp new file mode 100644 index 0000000..6e7e2ea --- /dev/null +++ b/src/core/ProxyService.cpp @@ -0,0 +1,117 @@ +#include "ProxyService.hpp" +#include +#include + +ProxyService::ProxyService(const QString& serialPort, qint32 baudRate, quint16 gcsListenPort, QObject* parent) + : QObject(parent), + serialPort_(serialPort), + baudRate_(baudRate), + gcsPort_(gcsListenPort) +{ + // 预创建对象（仍在主线程），随后统一迁移到IO线程 + serial_ = new SerialEndpoint(serialPort_, baudRate_); + udpHub_ = new UdpHubEndpoint(gcsPort_); + tap_ = new MavlinkTap; + proxy_ = new ProxyConnection(serial_, udpHub_, tap_); + injector_ = new Injector(udpHub_, /*sysid*/245, /*compid*/190); + + // 迁移到 IO 线程（这里只是“归属”变更，不做 open/bind） + serial_->moveToThread(&ioThread_); + udpHub_->moveToThread(&ioThread_); + tap_->moveToThread(&ioThread_); + proxy_->moveToThread(&ioThread_); + injector_->moveToThread(&ioThread_); + + // 将 IO 线程产生的信号桥接到主线程 + connect(tap_, &MavlinkTap::messageParsed, this, &ProxyService::messageParsed, Qt::QueuedConnection); + connect(udpHub_, &UdpHubEndpoint::peerAdded, this, &ProxyService::peerAdded, Qt::QueuedConnection); + connect(udpHub_, &UdpHubEndpoint::peerRemoved,this,&ProxyService::peerRemoved, Qt::QueuedConnection); + connect(proxy_, &ProxyConnection::bytesForwardedAB, this, &ProxyService::bytesForwardedAB, Qt::QueuedConnection); + connect(proxy_, &ProxyConnection::bytesForwardedBA, this, &ProxyService::bytesForwardedBA, Qt::QueuedConnection); + + // 线程结束时通知 + connect(&ioThread_, &QThread::finished, this, [this](){ + emit stopped(); + }); + + // 线程启动时在 IO 线程执行 ioStart_ + connect(&ioThread_, &QThread::started, this, &ProxyService::ioStart_, Qt::QueuedConnection); +} + +ProxyService::~ProxyService() { + stop(); + ioThread_.wait(); + // 对象都在IO线程归属；此时线程已退出，可安全删除 + delete serial_; + delete udpHub_; + delete tap_; + delete proxy_; + delete injector_; +} + +void ProxyService::start() { + if (running_) return; + ioThread_.start(); +} + +void ProxyService::stop() { + if (!running_) return; + // 在 IO 线程里做关闭 + QMetaObject::invokeMethod(this, "ioStop_", Qt::QueuedConnection); + ioThread_.quit(); +} + +void ProxyService::setPeerIdleTimeoutMs(int ms) { + peerIdleMs_ = ms; + if (udpHub_) { + QMetaObject::invokeMethod(udpHub_, [this](){ + udpHub_->setPeerIdleTimeoutMs(peerIdleMs_); + }, Qt::QueuedConnection); + } +} + +void ProxyService::addStaticPeer(const QHostAddress& addr, quint16 port) { + if (udpHub_) { + QMetaObject::invokeMethod(udpHub_, [this, addr, port](){ + udpHub_->addStaticPeer(addr, port); + }, Qt::QueuedConnection); + } +} + +void ProxyService::sendHeartbeat(uint8_t type, uint8_t autopilot, + uint8_t base_mode, uint32_t custom_mode, + uint8_t system_status) { + if (injector_) { + QMetaObject::invokeMethod(injector_, [=](){ + injector_->sendHeartbeat(type, autopilot, base_mode, custom_mode, system_status); + }, Qt::QueuedConnection); + } +} + +// ---------------- IO 线程内的实际开启/关闭 ---------------- + +void ProxyService::ioStart_() { + // 应用 UDP Hub 的超时配置 + udpHub_->setPeerIdleTimeoutMs(peerIdleMs_); + + // 在 IO 线程里启动透明转发（内部会 open 串口/绑定 UDP） + const bool ok = proxy_->start(); + running_ = ok; + if (!ok) { + qCritical() << "[ProxyService] start failed (serial or udp bind open failed)"; + emit started(false); + return; + } + + // 可选：给所有 GCS 发送一个心跳，证明代理在线（可注释） + injector_->sendHeartbeat(6 /*MAV_TYPE_GCS*/, 8 /*MAV_AUTOPILOT_INVALID*/, + 0 /*base_mode*/, 0 /*custom*/, 4 /*system_status:active*/); + + emit started(true); +} + +void ProxyService::ioStop_() { + if (!running_) return; + proxy_->stop(); // 内部会 close 串口与 UDP + running_ = false; +} diff --git a/src/core/ProxyService.hpp b/src/core/ProxyService.hpp new file mode 100644 index 0000000..4d88792 --- /dev/null +++ b/src/core/ProxyService.hpp @@ -0,0 +1,72 @@ +#pragma once +#include +#include +#include +#include + +#include "SerialEndpoint.hpp" +#include "UdpHubEndpoint.hpp" +#include "ProxyConnection.hpp" +#include "MavlinkTap.hpp" +#include "Injector.hpp" + +class ProxyService : public QObject { + Q_OBJECT +public: + // 构造：只在主线程调用；不做IO操作 + explicit ProxyService(const QString& serialPort, + qint32 baudRate, + quint16 gcsListenPort, + QObject* parent = nullptr); + ~ProxyService() override; + + // 在调用线程（通常主线程）触发启动/停止；真正的开启关闭在IO线程里执行 + Q_INVOKABLE void start(); + Q_INVOKABLE void stop(); + + // 运行时控制（跨线程排队到IO线程） + Q_INVOKABLE void setPeerIdleTimeoutMs(int ms); + Q_INVOKABLE void addStaticPeer(const QHostAddress& addr, quint16 port); + + // 可选：注入一个心跳（排队到IO线程执行） + Q_INVOKABLE void sendHeartbeat(uint8_t type, uint8_t autopilot, + uint8_t base_mode, uint32_t custom_mode, + uint8_t system_status); + +signals: + // 生命周期 + void started(bool ok); + void stopped(); + + // 观察数据 + void messageParsed(const mavlink_message_t& msg); // 从IO线程跨到主线程（Queued） + void bytesForwardedAB(qint64 n); // drone->GCS + void bytesForwardedBA(qint64 n); // GCS->drone + + // GCS 动态 + void peerAdded(const QHostAddress& addr, quint16 port); + void peerRemoved(const QHostAddress& addr, quint16 port); + +private slots: + // 这些槽运行在 IO 线程中 + void ioStart_(); // 打开端口/绑定socket/建立转发 + void ioStop_(); // 停止转发/关闭设备 + +private: + // 仅在主线程访问这些裸指针的“拥有关系”；实际方法调用都在IO线程中 + QThread ioThread_; + SerialEndpoint* serial_ = nullptr; + UdpHubEndpoint* udpHub_ = nullptr; + MavlinkTap* tap_ = nullptr; + ProxyConnection* proxy_ = nullptr; + Injector* injector_ = nullptr; + + // 构造参数 + const QString serialPort_; + const qint32 baudRate_; + const quint16 gcsPort_; + + // 配置参数（通过Queued修改） + int peerIdleMs_ = 30'000; + bool running_ = false; +}; diff --git a/src/core/SerialEndpoint.cpp b/src/core/SerialEndpoint.cpp new file mode 100644 index 0000000..a201820 --- /dev/null +++ b/src/core/SerialEndpoint.cpp @@ -0,0 +1,36 @@ +#include "SerialEndpoint.hpp" +#include + +SerialEndpoint::SerialEndpoint(const QString& portName, qint32 baudRate, QObject* parent) + : Endpoint(parent) +{ + port_.setPortName(portName); + port_.setBaudRate(baudRate); + port_.setDataBits(QSerialPort::Data8); + port_.setParity(QSerialPort::NoParity); + port_.setStopBits(QSerialPort::OneStop); + port_.setFlowControl(QSerialPort::NoFlowControl); + + QObject::connect(&port_, &QSerialPort::readyRead, this, [this](){ + QByteArray data = port_.readAll(); + emitRx(data); + }); +} + +bool SerialEndpoint::open() { + if (port_.isOpen()) return true; + bool ok = port_.open(QIODevice::ReadWrite); + if (!ok) qWarning() << "SerialEndpoint: failed to open" << port_.portName(); + return ok; +} + +void SerialEndpoint::close() { + if (port_.isOpen()) port_.close(); +} + +qint64 SerialEndpoint::writeBytes(const QByteArray& data) { + if (!port_.isOpen()) return -1; + qint64 n = port_.write(data); + port_.waitForBytesWritten(200); + return n; +} diff --git a/src/core/SerialEndpoint.hpp b/src/core/SerialEndpoint.hpp new file mode 100644 index 0000000..4721c4a --- /dev/null +++ b/src/core/SerialEndpoint.hpp @@ -0,0 +1,18 @@ +#pragma once +#include "Endpoint.hpp" +#include + +class SerialEndpoint : public Endpoint { + Q_OBJECT +public: + SerialEndpoint(const QString& portName, qint32 baudRate, QObject* parent = nullptr); + ~SerialEndpoint() override = default; + + bool open() override; + void close() override; + bool isOpen() const override { return port_.isOpen(); } + qint64 writeBytes(const QByteArray& data) override; + +private: + QSerialPort port_; +}; diff --git a/src/core/UdpHubEndpoint.cpp b/src/core/UdpHubEndpoint.cpp new file mode 100644 index 0000000..7192eb9 --- /dev/null +++ b/src/core/UdpHubEndpoint.cpp @@ -0,0 +1,86 @@ +#include "UdpHubEndpoint.hpp" +#include +#include + +UdpHubEndpoint::UdpHubEndpoint(quint16 bindPort, QObject* parent) + : Endpoint(parent), bindPort_(bindPort) +{ + QObject::connect(&gcTimer_, &QTimer::timeout, this, &UdpHubEndpoint::gcPeers_); + gcTimer_.setInterval(2000); +} + +bool UdpHubEndpoint::open() { + if (sock_.state() == QAbstractSocket::BoundState) return true; + bool ok = sock_.bind(QHostAddress::AnyIPv4, bindPort_, QUdpSocket::ShareAddress); + if (!ok) { + qWarning() << "UdpHubEndpoint: bind failed on port" << bindPort_; + return false; + } + QObject::connect(&sock_, &QUdpSocket::readyRead, this, &UdpHubEndpoint::onReadyRead_); + gcTimer_.start(); + return true; +} + +void UdpHubEndpoint::close() { + gcTimer_.stop(); + sock_.close(); + peers_.clear(); + lastSeenMs_.clear(); +} + +bool UdpHubEndpoint::isOpen() const { + return sock_.state() == QAbstractSocket::BoundState; +} + +qint64 UdpHubEndpoint::writeBytes(const QByteArray& data) { + qint64 total = 0; + for (const auto& p : peers_) { + qint64 n = sock_.writeDatagram(data, p.addr, p.port); + if (n > 0) total += n; + } + return total; +} + +void UdpHubEndpoint::addStaticPeer(const QHostAddress& addr, quint16 port) { + upsertPeer_(addr, port); +} + +void UdpHubEndpoint::onReadyRead_() { + while (sock_.hasPendingDatagrams()) { + QByteArray buf; + buf.resize(int(sock_.pendingDatagramSize())); + QHostAddress sender; quint16 senderPort = 0; + sock_.readDatagram(buf.data(), buf.size(), &sender, &senderPort); + + upsertPeer_(sender, senderPort); // 记录/刷新该 GCS + emitRx(buf); // 把字节给上层转发到串口 + } +} + +void UdpHubEndpoint::gcPeers_() { + const qint64 now = QDateTime::currentMSecsSinceEpoch(); + QList expired; + for (auto it = lastSeenMs_.cbegin(); it != lastSeenMs_.cend(); ++it) { + if (now - it.value() > idleMs_) expired.push_back(it.key()); + } + for (const auto& p : expired) { + peers_.remove(p); + lastSeenMs_.remove(p); + emit peerRemoved(p.addr, p.port); + } +} + +void UdpHubEndpoint::upsertPeer_(const QHostAddress& a, quint16 p) { + UdpPeer peer{a, p}; + + bool inserted = false; + if (!peers_.contains(peer)) { + peers_.insert(peer); // 返回迭代器，但我们不用 + inserted = true; + } + + lastSeenMs_[peer] = QDateTime::currentMSecsSinceEpoch(); + if (inserted) { + emit peerAdded(a, p); + } +} diff --git a/src/core/UdpHubEndpoint.hpp b/src/core/UdpHubEndpoint.hpp new file mode 100644 index 0000000..a681ad4 --- /dev/null +++ b/src/core/UdpHubEndpoint.hpp @@ -0,0 +1,51 @@ +#pragma once +#include "Endpoint.hpp" +#include +#include +#include +#include +#include + +struct UdpPeer { + QHostAddress addr; + quint16 port; + bool operator==(const UdpPeer& o) const { return addr == o.addr && port == o.port; } +}; +inline uint qHash(const UdpPeer& p, uint seed = 0) { + return qHash(p.addr.toString(), seed) ^ qHash(p.port, seed); +} + +class UdpHubEndpoint : public Endpoint { + Q_OBJECT +public: + explicit UdpHubEndpoint(quint16 bindPort, QObject* parent = nullptr); + + bool open() override; + void close() override; + bool isOpen() const override; + + // 广播：把 data 下发给所有已知 GCS peers + qint64 writeBytes(const QByteArray& data) override; + + // 可选：预置白名单 + void addStaticPeer(const QHostAddress& addr, quint16 port); + void setPeerIdleTimeoutMs(int ms) { idleMs_ = ms; } + +signals: + void peerAdded(const QHostAddress& addr, quint16 port); + void peerRemoved(const QHostAddress& addr, quint16 port); + +private slots: + void onReadyRead_(); + void gcPeers_(); + +private: + void upsertPeer_(const QHostAddress& a, quint16 p); + + QUdpSocket sock_; + quint16 bindPort_; + QSet peers_; + QMap lastSeenMs_; + QTimer gcTimer_; + int idleMs_ = 30'000; // 30s 无流量剔除 +}; diff --git a/src/main.cpp b/src/main.cpp new file mode 100644 index 0000000..8349220 --- /dev/null +++ b/src/main.cpp @@ -0,0 +1,11 @@ +#include "mainwindow.h" + +#include + +int main(int argc, char *argv[]) +{ + QApplication a(argc, argv); + MainWindow w; + w.show(); + return a.exec(); +} diff --git a/src/ui/mainwindow.cpp b/src/ui/mainwindow.cpp new file mode 100644 index 0000000..37e1b05 --- /dev/null +++ b/src/ui/mainwindow.cpp @@ -0,0 +1,14 @@ +#include "mainwindow.h" +#include "ui_mainwindow.h" + +MainWindow::MainWindow(QWidget *parent) + : QMainWindow(parent) + , ui(new Ui::MainWindow) +{ + ui->setupUi(this); +} + +MainWindow::~MainWindow() +{ + delete ui; +} diff --git a/src/ui/mainwindow.h b/src/ui/mainwindow.h new file mode 100644 index 0000000..ca592df --- /dev/null +++ b/src/ui/mainwindow.h @@ -0,0 +1,23 @@ +#ifndef MAINWINDOW_H +#define MAINWINDOW_H + +#include + +QT_BEGIN_NAMESPACE +namespace Ui { +class MainWindow; +} +QT_END_NAMESPACE + +class MainWindow : public QMainWindow +{ + Q_OBJECT + +public: + MainWindow(QWidget *parent = nullptr); + ~MainWindow(); + +private: + Ui::MainWindow *ui; +}; +#endif // MAINWINDOW_H diff --git a/src/ui/mainwindow.ui b/src/ui/mainwindow.ui new file mode 100644 index 0000000..7bff7e1 --- /dev/null +++ b/src/ui/mainwindow.ui @@ -0,0 +1,31 @@ + + + MainWindow + + + + 0 + 0 + 800 + 600 + + + + MainWindow + + + + + + 0 + 0 + 800 + 21 + + + + + + + +