增加通讯协议

2026-10-14 09:59:42 +08:00
parent 32878a9a80
commit 950e9e6c9e
102 changed files with 5538 additions and 2679 deletions
@@ -0,0 +1,151 @@
+#include "uart.h"
+#include <QApplication>
+#include <QDebug>
+#include <QSerialPort>
+#include <QSettings>
+
+namespace {
+
+const int    kFrameLen = 24;
+const quint8 kHead = 0x11;
+const quint8 kTail = 0x22;
+
+// 收不到帧头时缓冲上限，超过则只保留尾部一帧长度，避免长期涨下去
+const int kBufMax = 1024;
+
+// 小端无符号 16 位
+inline int leU16(const char* p)
+{
+    return int(quint8(p[0]) | (quint8(p[1]) << 8));
+}
+
+// 摇杆量按协议范围 0~4096 收口（外部输入，越界值不采信）
+inline int axis(const char* p)
+{
+    return qBound(0, leU16(p), 4096);
+}
+
+// 校验帧头、帧尾与和校验（BYTE0~BYTE22 之和的低 8 位）
+bool parseFrame(const char* d, RcFrame& f)
+{
+    if (quint8(d[0]) != kHead || quint8(d[22]) != kTail)
+        return false;
+
+    quint8 sum = 0;
+    for (int i = 0; i < 23; ++i)
+        sum += quint8(d[i]);
+    if (sum != quint8(d[23]))
+        return false;
+
+    const quint8 flags = quint8(d[1]);
+    f.driveMode      = (flags & 0x01) != 0;
+    f.armMode        = (flags & 0x02) != 0;
+    f.armEnabled     = (flags & 0x04) != 0;
+    f.chassisEnabled = (flags & 0x08) != 0;
+    f.estop          = (flags & 0x10) != 0;
+
+    f.gear  = axis(d + 3);
+    f.steer = axis(d + 5);
+    f.posZ  = axis(d + 7);
+    f.posX  = axis(d + 9);
+    f.posY  = axis(d + 11);
+    f.roll  = axis(d + 13);
+    f.pitch = axis(d + 15);
+    f.yaw   = axis(d + 17);
+
+    f.battery = qBound(0, int(quint8(d[19])), 100);
+    return true;
+}
+
+} // namespace
+
+RcCommunication::RcCommunication(QObject* parent)
+    : QObject(parent)
+{
+    m_serial = new QSerialPort(this);
+    connect(m_serial, &QSerialPort::readyRead, this, &RcCommunication::onReadyRead);
+
+    // 与 AppContext 共用程序目录的 config.ini：默认构造读不到 rc/* 这些键
+    // （本工程没有 setOrganizationName/setApplicationName）
+    QSettings s(QApplication::applicationDirPath() + "/config.ini", QSettings::IniFormat);
+    m_device = s.value("rc/device", "/dev/ttyS5").toString();
+    m_baud   = s.value("rc/baud", 115200).toInt();
+
+    // 协议发送周期 100ms，连续 500ms 无合法帧判为链路断开
+    m_linkTimer = new QTimer(this);
+    m_linkTimer->setInterval(500);
+    connect(m_linkTimer, &QTimer::timeout, this, &RcCommunication::onLinkTimeout);
+}
+
+RcCommunication::~RcCommunication()
+{
+    stop();
+}
+
+bool RcCommunication::start()
+{
+    if (m_serial->isOpen())
+        return true;
+
+    m_serial->setPortName(m_device);
+    m_serial->setBaudRate(m_baud);
+    m_serial->setDataBits(QSerialPort::Data8);
+    m_serial->setStopBits(QSerialPort::OneStop);
+    m_serial->setParity(QSerialPort::NoParity);
+    m_serial->setFlowControl(QSerialPort::NoFlowControl);
+
+    if (!m_serial->open(QIODevice::ReadWrite)) {
+        qWarning() << "遥控器串口打开失败:" << m_device << m_serial->errorString();
+        return false;
+    }
+    m_buf.clear();
+    m_linkTimer->start();
+    return true;
+}
+
+void RcCommunication::stop()
+{
+    m_linkTimer->stop();
+    if (m_serial->isOpen()) m_serial->close();
+    m_buf.clear();
+}
+
+void RcCommunication::onReadyRead()
+{
+    m_buf.append(m_serial->readAll());
+
+    // 逐字节扫描合法帧：容忍前导垃圾与粘包，未对齐时向后找下一个帧头。
+    // 串口是字节流，一帧可能被切开，尾部不足一帧的残段留在 m_buf 等下次数据。
+    int off = 0;
+    while (off + kFrameLen <= m_buf.size()) {
+        RcFrame f;
+        if (parseFrame(m_buf.constData() + off, f)) {
+            onFrame(f);
+            off += kFrameLen;
+        } else {
+            ++off;
+        }
+    }
+    if (off > 0) m_buf.remove(0, off);
+
+    if (m_buf.size() > kBufMax) m_buf.remove(0, m_buf.size() - kFrameLen);
+}
+
+void RcCommunication::onFrame(const RcFrame& frame)
+{
+    m_linkTimer->start();   // 重置链路超时
+
+    if (!m_linkOk) {
+        m_linkOk = true;
+        emit linkChanged(true);
+    }
+    emit frameReceived(frame);
+}
+
+void RcCommunication::onLinkTimeout()
+{
+    if (m_linkOk) {
+        m_linkOk = false;
+        emit linkChanged(false);
+    }
+}