152 lines
4.0 KiB
C++
152 lines
4.0 KiB
C++
#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/ttyS7").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);
|
||
}
|
||
}
|