1132 lines
49 KiB
C++
1132 lines
49 KiB
C++
#include "car.h"
|
||
#include <QApplication>
|
||
#include <QDebug>
|
||
#include <QHostAddress>
|
||
#include <QNetworkDatagram>
|
||
#include <QSettings>
|
||
#include <QUdpSocket>
|
||
#include <QtGlobal>
|
||
#include <cmath>
|
||
#include <cstring>
|
||
|
||
namespace {
|
||
|
||
// ===================== 线格式 =====================
|
||
// 一个 UDP 数据报承载 = 1 字节帧头 + 32 位 CAN ID + DLC + 8 字节数据 + 8 位校验和 + 1 字节帧尾，
|
||
// 共 16 字节。多字节字段按主机小端排列，与车载端网关一致；帧头/帧尾与校验和用于确认数据报内容合法。
|
||
#pragma pack(push, 1)
|
||
struct CanFrame {
|
||
quint8 header; // 固定帧头 0xAA
|
||
quint32 canId; // 29 位扩展帧 ID，直接用于分流
|
||
quint8 dlc; // 数据长度码，固定 8
|
||
quint8 data[8]; // 8 字节 CAN 原生数据段
|
||
quint8 checksum; // 帧头 + canId 4 字节 + dlc + data[8] + 帧尾，共 15 字节求和取低 8 位
|
||
quint8 tail; // 固定帧尾 0xBB
|
||
};
|
||
#pragma pack(pop)
|
||
|
||
const quint8 kFrameHeader = 0xAA;
|
||
const quint8 kFrameTail = 0xBB;
|
||
const int kFrameLen = int(sizeof(CanFrame));
|
||
const int kDataLen = 8;
|
||
|
||
// 洗消作业期间机械臂末端的自动俯仰摆动：中位 ±1024（轴量程 0~4096）、4 秒一个往返。
|
||
// 摆动在 50ms 那一组里推进（Arm_ControlCmd2 就在这组），所以一拍 = 50ms、
|
||
// 一个往返 = 4s ÷ 50ms = 80 拍。kWashSwingAmp 别超过 kRcAxisMid，不然会顶到量程两端。
|
||
const int kWashSwingAmp = 1024;
|
||
const int kWashSwingTicks = 80;
|
||
const int kSwingRampTicks = 10; // 回中 / 还原的过渡时长：10 拍 = 0.5s
|
||
const double kPi = 3.14159265358979323846;
|
||
|
||
// 轴值线性插值并夹到量程 0~4096。回中与还原都走斜坡，别一步跳变把机械臂顿一下。
|
||
quint16 lerpAxis(int from, int to, double t)
|
||
{
|
||
return quint16(qBound(0, int(from + (to - from) * qBound(0.0, t, 1.0)), 4096));
|
||
}
|
||
|
||
// 车端 CAN 的 UDP 端口：本机 9000 收（车端反馈），上行命令发往车端 192.168.1.224:3001。
|
||
// 收端绑 AnyIPv4（各网卡都收），发端必须写明车端 IP —— "0.0.0.0" 不是合法目的地址。
|
||
const quint16 kCanRxPort = 9000;
|
||
const quint16 kCanTxPort = 3001;
|
||
const char* kCanTxHost = "192.168.1.224";
|
||
|
||
quint8 canChecksum(quint32 canId, quint8 dlc, const quint8* data)
|
||
{
|
||
quint8 sum = kFrameHeader; // 帧头/帧尾也算进去（帧尾虽在校验和之后，双方按定长解析无妨）
|
||
for (int i = 0; i < 4; ++i) sum += quint8((canId >> (8 * i)) & 0xFF);
|
||
sum += dlc;
|
||
for (int i = 0; i < kDataLen; ++i) sum += data[i];
|
||
sum += kFrameTail;
|
||
return sum;
|
||
}
|
||
|
||
// ===================== CAN ID（sheet RC） =====================
|
||
// 分类一 遥控器控制指令（上行，7 条）
|
||
const quint32 kIdChassisCmd = 0x18208081;
|
||
const quint32 kIdArmCmd1 = 0x18218081;
|
||
const quint32 kIdArmCmd2 = 0x18228081;
|
||
const quint32 kIdDecontamCmd = 0x18238081;
|
||
const quint32 kIdDecontamDbg = 0x18248081;
|
||
const quint32 kIdDetectorCmd = 0x18258081;
|
||
const quint32 kIdEmergency = 0x18268081;
|
||
// 分类二/三 VCU 状态反馈（下行，12 条）
|
||
const quint32 kIdVehicleFb1 = 0x18308180; // 底盘状态1 BMS
|
||
const quint32 kIdVehicleFb2 = 0x18318180; // 底盘状态2 BMS
|
||
const quint32 kIdArmFb1 = 0x18328180; // 机械臂状态1 末端位置
|
||
const quint32 kIdArmFb2 = 0x18338180; // 机械臂状态2 末端姿态/速度
|
||
const quint32 kIdArmFb3 = 0x18348180; // 机械臂状态3 关节角 J1~J4
|
||
const quint32 kIdArmFb4 = 0x18358180; // 机械臂状态4 关节角 J5~J6
|
||
const quint32 kIdDecontamFb1 = 0x18368180; // 洗消工作状态1
|
||
const quint32 kIdDecontamFb2 = 0x18378180; // 洗消工作状态2
|
||
const quint32 kIdDetectorFb1 = 0x18388180; // 检测仪结果状态1
|
||
const quint32 kIdDetectorFb2 = 0x18398180; // 检测仪结果状态2
|
||
const quint32 kIdVehicleFb3 = 0x183A8180; // 底盘状态3 EPS/制动故障位
|
||
const quint32 kIdVehicleFb4 = 0x183B8180; // 底盘状态4 挡位/驻车/方向盘/制动电机
|
||
|
||
// ===================== 位运算 =====================
|
||
// 分类一/二/三全部是扩展帧、信号一律 Intel（小端）：起始位即最低位序号，
|
||
// 位序由低到高、可跨字节（如 bit47 长度 2 就横跨 byte5/byte6）。
|
||
quint64 intelBits(const quint8* d, int start, int len)
|
||
{
|
||
quint64 v = 0;
|
||
for (int i = 0; i < len; ++i) {
|
||
const int bit = start + i;
|
||
if (d[bit >> 3] & (1u << (bit & 7)))
|
||
v |= quint64(1) << i;
|
||
}
|
||
return v;
|
||
}
|
||
|
||
qint64 intelSigned(const quint8* d, int start, int len)
|
||
{
|
||
const quint64 raw = intelBits(d, start, len);
|
||
const quint64 signBit = quint64(1) << (len - 1);
|
||
return qint64((raw ^ signBit) - signBit); // 符号扩展
|
||
}
|
||
|
||
void setIntelBits(quint8* d, int start, int len, quint64 v)
|
||
{
|
||
for (int i = 0; i < len; ++i) {
|
||
const int bit = start + i;
|
||
const quint8 mask = quint8(1u << (bit & 7));
|
||
if (v & (quint64(1) << i)) d[bit >> 3] |= mask;
|
||
else d[bit >> 3] &= quint8(~mask);
|
||
}
|
||
}
|
||
|
||
// 物理值 = 总线原始整数值 × 精度 + 偏移量
|
||
double phys(quint64 raw, double scale, double offset = 0.0)
|
||
{
|
||
return double(raw) * scale + offset;
|
||
}
|
||
|
||
// ===================== 协议配置文件 =====================
|
||
// 与 AppContext 用同一份 config.ini（程序目录）。
|
||
QString cfgPath()
|
||
{
|
||
return QApplication::applicationDirPath() + "/config.ini";
|
||
}
|
||
|
||
// ===================== 故障码 → 中文 =====================
|
||
struct CodeText { quint8 code; const char* text; };
|
||
|
||
const char* lookup(const CodeText* tbl, int n, quint8 code)
|
||
{
|
||
for (int i = 0; i < n; ++i)
|
||
if (tbl[i].code == code) return tbl[i].text;
|
||
return nullptr;
|
||
}
|
||
|
||
const CodeText kMcuFaults[] = {
|
||
{6, "电机缺相预警"}, {17, "控制器过温警告"}, {18, "电机过温警告"},
|
||
{19, "控制器温度传感器警告"}, {20, "电机温度传感器警告"}, {21, "控制器过载警告"},
|
||
{22, "电机超速警告"}, {23, "直流母线电压过压警告"}, {24, "直流母线电压欠压警告"},
|
||
{35, "电流霍尔传感U相器故障"}, {36, "电流霍尔传感W相器故障"}, {37, "VCU报文丢失故障"},
|
||
{38, "VCU循环计数错误"}, {39, "VCU校验和错误"}, {40, "CAN BussOff故障"},
|
||
{49, "母线硬件过压故障"}, {50, "相电流硬件过流故障"}, {51, "IGBT短路故障"},
|
||
{52, "驱动欠压故障"}, {53, "旋变故障"}, {54, "电机超速故障"},
|
||
{55, "低压电源过压故障"}, {56, "低压电源欠压故障"}, {57, "直流母线电压过压故障"},
|
||
{58, "直流母线电压欠压故障"}, {59, "控制器过温故障"}, {60, "电机过温故障"},
|
||
{61, "电机缺相故障"}, {62, "驻坡失效故障"},
|
||
};
|
||
|
||
const CodeText kAirCompFaults[] = {
|
||
{2, "加速过流"}, {3, "减速过流"}, {4, "恒速过流"}, {5, "加速过压"},
|
||
{6, "减速过压"}, {7, "恒速过压"}, {8, "缓冲过载"}, {9, "欠压"},
|
||
{10, "控制器过载"}, {11, "电机过载"}, {12, "输入缺相"}, {13, "输出缺相"},
|
||
{14, "模块过热"}, {15, "外部故障"}, {16, "通信异常"}, {17, "接触器异常"},
|
||
{18, "电流检测故障"}, {19, "自学习故障"}, {20, "编码器故障"}, {21, "参数读写异常"},
|
||
{22, "变频器硬件故障"}, {23, "电机对地短路故障"}, {31, "PID反馈丢失"},
|
||
{40, "快速限流超时故障"}, {42, "速度偏差过大"}, {43, "电机超速"}, {45, "电机过温"},
|
||
{90, "编码器线数设定错误"}, {91, "未接编码器"}, {92, "初始位置错误"}, {94, "速度反馈错误"},
|
||
};
|
||
|
||
const CodeText kPlungerFaults[] = {
|
||
{1, "短路故障"}, {2, "欠压"}, {3, "过压"}, {4, "控制器过温"},
|
||
{5, "控制器温度传感器故障"}, {6, "电机过温"}, {7, "电机温度传感器故障"},
|
||
};
|
||
|
||
const CodeText kCommFaults[] = {
|
||
{1, "底盘电机通信异常"}, {2, "底盘BMS通信异常"}, {3, "机械臂通信异常"},
|
||
{4, "空压机通信异常"}, {5, "柱塞泵通信异常"}, {6, "叶片泵通信异常"},
|
||
{7, "气体检测仪通信异常"}, {8, "遥控器通信异常"},
|
||
};
|
||
|
||
const CodeText kDetectorWarns[] = {
|
||
{1, "进气嘴未接"}, {2, "氢气流量不足"}, {4, "仪器故障"}, {8, "浓度过高"},
|
||
};
|
||
|
||
// 底盘状态3（0x183A8180）的 46 个 1 bit 故障标志（bit0~bit45）
|
||
const char* const kVsb3Bits[46] = {
|
||
"EPS转子角未对中", "EPS转子角度断开", "EPS转子角度SPI故障", "EPSEEPROM故障",
|
||
"EPS方向盘中点未对中", "EPS方向盘角度超限", "EPS方向盘小齿信号断开", "EPS方向盘中齿信号断开",
|
||
"EPS过电流故障", "EPS电机A相电流异常", "EPS电机C相电流异常", "EPS电机B相电流异常",
|
||
"EPSVCU信号丢失", "EPS电机无助力", "EPS预驱配置失败", "EPS半堵转保护",
|
||
"EPS温度热保护", "EPS电压管理保护", "EPS转角响应不到位", "EPS转子角度PWM故障",
|
||
"EPS电压校验异常", "EPS热敏电阻开路或短路", "EPS热敏电阻温差过大",
|
||
"制动器过载故障", "制动器过温或过压故障", "制动器过流故障", "制动器电源欠压故障",
|
||
"制动器电源过压故障", "制动器压力不足", "制动器缺相故障", "制动器通讯故障",
|
||
"制动器电流采样故障", "制动器驱动故障", "制动器磁编故障", "制动器压力传感器故障",
|
||
"制动器踏板位置传感器故障", "制动器机械故障", "制动器堵转故障", "制动器急停按键闭合警告",
|
||
"制动器驻车时间过长", "制动器CAN1中断警告", "制动器CAN2中断警告", "制动器点火线断开警告",
|
||
"制动器电流异常警告", "制动器温度过高警告", "制动器输入电压过高警告",
|
||
};
|
||
|
||
// 各路 *_FaultLevel 统一为 0 正常 / 1~3 故障等级
|
||
const char* const kLevels[4] = {"", "一级故障", "二级故障", "三级故障"};
|
||
|
||
// 制动电机状态（Brake_*_Mot_fault）：0x00~0x07
|
||
const char* const kBrakeMotFaults[8] = {
|
||
"", "电机开路", "电机短路", "制动执行超时", "解除制动执行超时",
|
||
"驱动电路故障", "供电电压过低", "供电电压过高",
|
||
};
|
||
|
||
void addFault(QVector<CanFault>& out, const QString& system, const QString& group,
|
||
const char* codePrefix, quint8 code, const char* text, int level = 0)
|
||
{
|
||
if (!text) return;
|
||
CanFault f;
|
||
f.system = system;
|
||
f.group = group;
|
||
f.code = QString::fromLatin1(codePrefix) + QString::number(code);
|
||
f.codeNum = QString::number(code);
|
||
f.desc = QString::fromUtf8(text);
|
||
f.level = level;
|
||
out.append(f);
|
||
}
|
||
|
||
// 未在码表中登记的码也上报，避免“有故障但界面无提示”
|
||
void addFaultByCode(QVector<CanFault>& out, const QString& system, const QString& group,
|
||
const char* codePrefix, const CodeText* tbl, int n, quint8 code, int level = 0)
|
||
{
|
||
if (code == 0) return;
|
||
const char* t = lookup(tbl, n, code);
|
||
if (t) {
|
||
addFault(out, system, group, codePrefix, code, t, level);
|
||
return;
|
||
}
|
||
CanFault f;
|
||
f.system = system;
|
||
f.group = group;
|
||
f.code = QString::fromLatin1(codePrefix) + QString::number(code);
|
||
f.codeNum = QString::number(code);
|
||
f.desc = QString::fromLatin1(codePrefix) + QString::number(code);
|
||
f.level = level;
|
||
out.append(f);
|
||
}
|
||
|
||
// 故障等级：>0 才上报。等级型信号没有“码”，顶部显示成“<组名>：<描述>，N级”
|
||
void addLevelFault(QVector<CanFault>& out, const QString& system, const QString& group,
|
||
const char* codePrefix, const char* name, quint8 level)
|
||
{
|
||
if (level == 0 || level > 3) return;
|
||
CanFault f;
|
||
f.system = system;
|
||
f.group = group;
|
||
f.code = QString::fromLatin1(codePrefix) + QString::number(level); // 追溯用，顶部不显示
|
||
f.desc = QString::fromUtf8(name);
|
||
f.level = level;
|
||
out.append(f);
|
||
}
|
||
|
||
// 没有“故障码”的故障（单个故障位、方向盘对中状态这类无编码信号）：
|
||
// 顶部只显示描述，tracePrefix+traceSuffix 仅作故障历史追溯用。
|
||
void addNoCodeFault(QVector<CanFault>& out, const QString& system, const QString& group,
|
||
const char* tracePrefix, int traceSuffix, const char* text)
|
||
{
|
||
if (!text) return;
|
||
CanFault f;
|
||
f.system = system;
|
||
f.group = group;
|
||
f.code = QString::fromLatin1(tracePrefix) + QString::number(traceSuffix);
|
||
f.desc = QString::fromUtf8(text);
|
||
out.append(f);
|
||
}
|
||
|
||
const char* const kSysChassis = "机器人底盘";
|
||
const char* const kSysWash = "洗消系统";
|
||
const char* const kSysDetector = "气体检测仪";
|
||
|
||
// 顶部轮播的显示组名。前三个取自协议 CSV 的故障分组列（底盘状态 1~4 各自归属），
|
||
// 洗消/检测仪协议里没给分组列，按 system 命名。某组没有故障时顶部就不出现该组名。
|
||
const char* const kGroupChassisMotor = "底盘电机故障";
|
||
const char* const kGroupBms = "BMS系统故障";
|
||
const char* const kGroupSteerBrake = "转向制动系统故障";
|
||
const char* const kGroupWash = "洗消系统故障";
|
||
const char* const kGroupDetector = "气体检测仪故障";
|
||
// All_Comm_FaultCode 本身也是“一组”故障，顶部按“通信异常：3-机械臂通信异常”显示
|
||
const char* const kGroupComm = "通信异常";
|
||
|
||
// All_Comm_FaultCode → 顶部那行的文案。表里写的就是“机械臂通信异常”这种，
|
||
// 不再加“xx系统”前缀。码不认识时退回笼统的“通信异常”。
|
||
QString commFaultText(quint8 code)
|
||
{
|
||
for (const CodeText& c : kCommFaults)
|
||
if (c.code == code) return QString::fromUtf8(c.text);
|
||
return QString::fromUtf8(kGroupComm);
|
||
}
|
||
|
||
} // namespace
|
||
|
||
// 完整一行：<组名>：[<码>-]<描述>[，<N>级]；desc 为空则只显示组名（链路超时用）
|
||
QString CanFault::text() const
|
||
{
|
||
if (desc.isEmpty()) return group;
|
||
QString s = group + QString::fromUtf8("：");
|
||
if (!codeNum.isEmpty()) s += codeNum + "-";
|
||
s += desc;
|
||
if (level > 0) s += QString::fromUtf8("，%1级").arg(level);
|
||
return s;
|
||
}
|
||
|
||
// ===================== 构造与生命周期 =====================
|
||
CarCommunication::CarCommunication(Telemetry* tel, QObject* parent)
|
||
: QObject(parent), m_tel(tel)
|
||
{
|
||
m_sock = new QUdpSocket(this);
|
||
connect(m_sock, &QUdpSocket::readyRead, this, &CarCommunication::onDatagram);
|
||
|
||
// 端口固定为 kCanRxPort/kCanTxPort 常量，config.ini 只剩自检演示开关要读
|
||
QSettings s(cfgPath(), QSettings::IniFormat);
|
||
m_selfCheckFault = s.value("net/selfcheck_fault", true).toBool();
|
||
|
||
// 洗消四参数按存盘值初始化，否则开机帧发的是硬编码初值而非用户设定值。
|
||
// INI 读回的是文本（"6.5"），须用 toDouble 解析；协议字段都是整数，四舍五入取整。
|
||
m_plungerFlow = qRound(qBound(0.0, s.value("param/pump_flow", 20).toDouble(), 24.0));
|
||
m_foamRatio = qRound(qBound(6.0, s.value("param/foam_ratio", 6).toDouble(), 12.0));
|
||
m_foamPercent = qRound(qBound(0.0, s.value("param/foam_percent", 3).toDouble(), 10.0));
|
||
m_airFlow = qRound(qBound(0.0, s.value("param/air_flow", 100).toDouble(), 340.0));
|
||
|
||
// 通信超时判定：最慢的下行报文周期 1000ms，连续 2s 无任何合法帧 → 通信异常
|
||
m_hbTimer = new QTimer(this);
|
||
m_hbTimer->setInterval(2000);
|
||
connect(m_hbTimer, &QTimer::timeout, this, &CarCommunication::onHeartbeatTimeout);
|
||
|
||
// 上行周期定时器，周期取自协议「报文周期时间」列
|
||
m_fastTimer = new QTimer(this); // 50ms：底盘 / 机械臂 / 急停
|
||
m_fastTimer->setInterval(50);
|
||
connect(m_fastTimer, &QTimer::timeout, this, &CarCommunication::onSendFast);
|
||
|
||
m_midTimer = new QTimer(this); // 100ms：洗消指令 / 洗消调试
|
||
m_midTimer->setInterval(100);
|
||
connect(m_midTimer, &QTimer::timeout, this, &CarCommunication::onSendMid);
|
||
|
||
m_slowTimer = new QTimer(this); // 1000ms：气体检测仪
|
||
m_slowTimer->setInterval(1000);
|
||
connect(m_slowTimer, &QTimer::timeout, this, &CarCommunication::onSendSlow);
|
||
|
||
// 本端自检序列（见 onSelfCheckTick 的 TODO）
|
||
m_selfCheckTimer = new QTimer(this);
|
||
m_selfCheckTimer->setInterval(450);
|
||
connect(m_selfCheckTimer, &QTimer::timeout, this, &CarCommunication::onSelfCheckTick);
|
||
|
||
if (m_tel) m_tel->js.resize(6);
|
||
}
|
||
|
||
CarCommunication::~CarCommunication()
|
||
{
|
||
stop();
|
||
}
|
||
|
||
bool CarCommunication::start()
|
||
{
|
||
if (m_sock->state() == QAbstractSocket::BoundState)
|
||
return true;
|
||
|
||
m_sock->abort();
|
||
// AnyIPv4：回环与各本机网卡都能收到；若只允许本机回环，改成 QHostAddress::LocalHost
|
||
if (!m_sock->bind(QHostAddress::AnyIPv4, kCanRxPort)) {
|
||
// 端口被占用时走这里：不启动定时器，界面显示通信异常
|
||
qWarning() << "车端 UDP 端口绑定失败:" << kCanRxPort << m_sock->errorString();
|
||
setCommOk(false);
|
||
updateCommText(); // 收不到帧也不会有 cacheFaults，顶部得靠这里才显示“通信异常”
|
||
return false;
|
||
}
|
||
m_hbTimer->start();
|
||
m_fastTimer->start();
|
||
m_midTimer->start();
|
||
m_slowTimer->start();
|
||
return true;
|
||
}
|
||
|
||
void CarCommunication::stop()
|
||
{
|
||
m_hbTimer->stop();
|
||
m_fastTimer->stop();
|
||
m_midTimer->stop();
|
||
m_slowTimer->stop();
|
||
m_selfCheckTimer->stop();
|
||
m_sock->close();
|
||
}
|
||
|
||
void CarCommunication::setCommOk(bool ok)
|
||
{
|
||
if (ok == m_lastCommOk) return;
|
||
m_lastCommOk = ok;
|
||
if (m_tel) m_tel->commOk = ok;
|
||
}
|
||
|
||
// ===================== 收包与分流 =====================
|
||
// UDP 是数据报：单个数据报内含一条或多条完整帧，逐条校验即可，
|
||
// 不再需要串口时代那套跨读字节流的残帧缓冲与滑窗重同步。
|
||
void CarCommunication::onDatagram()
|
||
{
|
||
bool got = false;
|
||
while (m_sock->hasPendingDatagrams()) {
|
||
const QByteArray d = m_sock->receiveDatagram().data();
|
||
int off = 0;
|
||
while (off + kFrameLen <= d.size()) {
|
||
CanFrame f;
|
||
std::memcpy(&f, d.constData() + off, kFrameLen);
|
||
if (f.header == kFrameHeader && f.tail == kFrameTail && f.dlc == kDataLen
|
||
&& f.checksum == canChecksum(f.canId, f.dlc, f.data)) {
|
||
handleFrame(f.canId, f.data);
|
||
got = true;
|
||
off += kFrameLen;
|
||
} else {
|
||
++off; // 数据报内可能有杂字节，向后找下一个帧头
|
||
}
|
||
}
|
||
}
|
||
|
||
// 一批数据报**全部解完**再统一刷一次界面。
|
||
// 积压时 Telemetry 里留下的本来就是各字段的最新值，中间每帧都推一遍纯属浪费——
|
||
// 一批 100 帧会变成 100×(7 个页面 + 8 个底条) 次刷新，而界面只需要最后那一次。
|
||
// 只在"同一批"里合并，不额外等定时器，所以不引入延迟。
|
||
if (!got)
|
||
return;
|
||
m_hbTimer->start(); // 收到任意合法帧即刷新链路（CAN 无独立心跳报文）
|
||
setCommOk(true);
|
||
cacheFaults();
|
||
emit telemetryUpdated();
|
||
}
|
||
|
||
void CarCommunication::handleFrame(quint32 canId, const quint8* d)
|
||
{
|
||
if (!m_tel) return;
|
||
|
||
// 这里只把数据写进 Telemetry，不动链路状态、也不刷界面——那两件事统一在
|
||
// onDatagram() 排空一批之后做一次，这样一大批积压也只会重绘一次。
|
||
switch (canId) {
|
||
// ---------- 0x18308180 底盘状态1（BMS）：电池总况与故障等级 ----------
|
||
case kIdVehicleFb1: {
|
||
m_tel->volt = phys(intelBits(d, 0, 16), 0.1);
|
||
m_tel->soc = int(phys(intelBits(d, 16, 16), 0.1));
|
||
m_tel->hasBattery = true;
|
||
|
||
m_vsf1Faults.clear();
|
||
// 底盘电机：故障码(8位)与故障等级(2位)是配套下发的两个信号，协议注明“不会出现
|
||
// 等级为 0 却有故障码”的情况，故合并成一条“底盘电机故障：18-电机过温警告，1级”。
|
||
addFaultByCode(m_vsf1Faults, QString::fromUtf8(kSysChassis),
|
||
QString::fromUtf8(kGroupChassisMotor), "MCU",
|
||
kMcuFaults, int(sizeof(kMcuFaults) / sizeof(CodeText)),
|
||
quint8(intelBits(d, 32, 8)), int(intelBits(d, 40, 2)));
|
||
// 以下 *FaultLevel 都归 BMS 系统
|
||
const QString bms = QString::fromUtf8(kGroupBms);
|
||
addLevelFault(m_vsf1Faults, QString::fromUtf8(kSysChassis), bms,
|
||
"CELLL", "单体电压过低", quint8(intelBits(d, 45, 2)));
|
||
addLevelFault(m_vsf1Faults, QString::fromUtf8(kSysChassis), bms,
|
||
"CELLHL", "单体电压过高", quint8(intelBits(d, 47, 2)));
|
||
addLevelFault(m_vsf1Faults, QString::fromUtf8(kSysChassis), bms,
|
||
"VOLTL", "总电压过低", quint8(intelBits(d, 49, 2)));
|
||
addLevelFault(m_vsf1Faults, QString::fromUtf8(kSysChassis), bms,
|
||
"VOLTHL", "总电压过高", quint8(intelBits(d, 51, 2)));
|
||
addLevelFault(m_vsf1Faults, QString::fromUtf8(kSysChassis), bms,
|
||
"DLTPL", "放电低温", quint8(intelBits(d, 53, 2)));
|
||
addLevelFault(m_vsf1Faults, QString::fromUtf8(kSysChassis), bms,
|
||
"DHTPL", "放电高温", quint8(intelBits(d, 55, 2)));
|
||
addLevelFault(m_vsf1Faults, QString::fromUtf8(kSysChassis), bms,
|
||
"CLTPL", "充电低温", quint8(intelBits(d, 57, 2)));
|
||
addLevelFault(m_vsf1Faults, QString::fromUtf8(kSysChassis), bms,
|
||
"CHTPL", "充电高温", quint8(intelBits(d, 59, 2)));
|
||
addLevelFault(m_vsf1Faults, QString::fromUtf8(kSysChassis), bms,
|
||
"TIL", "温度不均衡", quint8(intelBits(d, 61, 2)));
|
||
break;
|
||
}
|
||
|
||
// ---------- 0x18318180 底盘状态2（BMS）：手刹 / 车速 / 继电器 / 通信 ----------
|
||
case kIdVehicleFb2: {
|
||
m_vsf2Faults.clear();
|
||
const QString bms = QString::fromUtf8(kGroupBms);
|
||
addLevelFault(m_vsf2Faults, QString::fromUtf8(kSysChassis), bms,
|
||
"VIL", "压差不均衡", quint8(intelBits(d, 0, 2)));
|
||
addLevelFault(m_vsf2Faults, QString::fromUtf8(kSysChassis), bms,
|
||
"DOCL", "放电过流", quint8(intelBits(d, 2, 2)));
|
||
addLevelFault(m_vsf2Faults, QString::fromUtf8(kSysChassis), bms,
|
||
"COCL", "充电过流", quint8(intelBits(d, 4, 2)));
|
||
addLevelFault(m_vsf2Faults, QString::fromUtf8(kSysChassis), bms,
|
||
"INCL", "内部通讯", quint8(intelBits(d, 6, 2)));
|
||
addLevelFault(m_vsf2Faults, QString::fromUtf8(kSysChassis), bms,
|
||
"EXCL", "外部通讯", quint8(intelBits(d, 8, 2)));
|
||
addLevelFault(m_vsf2Faults, QString::fromUtf8(kSysChassis), bms,
|
||
"SOCL", "SOC过低", quint8(intelBits(d, 10, 2)));
|
||
addLevelFault(m_vsf2Faults, QString::fromUtf8(kSysChassis), bms,
|
||
"INSL", "绝缘", quint8(intelBits(d, 12, 2)));
|
||
|
||
// 继电器粘连：0=正常 非 0=故障
|
||
struct { int bit; const char* name; const char* code; } relays[] = {
|
||
{14, "加热继电器粘连", "HEATW"},
|
||
{16, "充电继电器粘连", "CHGW"},
|
||
{18, "总负继电器粘连", "NEGW"},
|
||
{20, "总正继电器粘连", "POSW"},
|
||
};
|
||
for (const auto& r : relays) {
|
||
if (intelBits(d, r.bit, 2) != 0)
|
||
addFault(m_vsf2Faults, QString::fromUtf8(kSysChassis), bms, r.code, 1, r.name);
|
||
}
|
||
|
||
m_tel->handbrake = intelBits(d, 22, 2) != 0;
|
||
// Vehi_spd：协议表为 8 位、精度 0.01（0~2.55 km/h）。其后 Vehi_RdyStatus(32)、
|
||
// Reserve(34)、All_Comm_FaultCode(40) 的起始位与 8 位布局自洽，故按协议取值。
|
||
m_tel->speed = phys(intelBits(d, 24, 8), 0.01);
|
||
m_tel->rdyStatus = int(intelBits(d, 32, 2));
|
||
m_tel->hasSpeed = true;
|
||
|
||
const quint8 comm = quint8(intelBits(d, 40, 8)); // All_Comm_FaultCode
|
||
m_commFaultCode = comm;
|
||
m_commFaults.clear();
|
||
addFaultByCode(m_commFaults, QString::fromUtf8(kSysChassis),
|
||
QString::fromUtf8(kGroupComm), "COMM",
|
||
kCommFaults, int(sizeof(kCommFaults) / sizeof(CodeText)), comm);
|
||
break;
|
||
}
|
||
|
||
// ---------- 0x183A8180 底盘状态3：EPS / 制动故障位 ----------
|
||
case kIdVehicleFb3: {
|
||
m_vsf3Faults.clear();
|
||
const QString sys = QString::fromUtf8(kSysChassis);
|
||
const QString brake = QString::fromUtf8(kGroupSteerBrake);
|
||
for (int i = 0; i < 46; ++i) {
|
||
if (intelBits(d, i, 1) != 0)
|
||
addNoCodeFault(m_vsf3Faults, sys, brake, "EPS", i, kVsb3Bits[i]);
|
||
}
|
||
const quint8 eps = quint8(intelBits(d, 48, 8));
|
||
if (eps != 0) {
|
||
CanFault f;
|
||
f.system = sys;
|
||
f.group = brake;
|
||
f.code = QString("EPSC") + QString::number(eps);
|
||
f.codeNum = QString::number(eps);
|
||
f.desc = QString::fromUtf8("EPS故障码");
|
||
m_vsf3Faults.append(f);
|
||
}
|
||
break;
|
||
}
|
||
|
||
// ---------- 0x183B8180 底盘状态4：挡位 / 驻车 / 方向盘 / 制动电机 ----------
|
||
case kIdVehicleFb4: {
|
||
m_vsf4Faults.clear();
|
||
m_tel->gearPos = int(intelBits(d, 0, 2)); // 0=N 1=D 2=R
|
||
m_tel->parking = int(intelBits(d, 2, 2)); // 0=未驻车 1=左电机 2=右电机 3=完全驻车
|
||
m_tel->awdMode = int(intelBits(d, 4, 2)); // 0=二驱 1=四驱 2=四驱锁定
|
||
m_tel->steerAngle = double(intelBits(d, 8, 16));
|
||
m_tel->hasChassis4 = true;
|
||
|
||
// ALIGN 是状态字节（0x11=对中失败、0xEE/0x55=正常），不是故障码，顶部只显示描述
|
||
const QString sys = QString::fromUtf8(kSysChassis);
|
||
const QString brake = QString::fromUtf8(kGroupSteerBrake);
|
||
const quint8 align = quint8(intelBits(d, 24, 8));
|
||
m_tel->alignStatus = align; // 底盘页「角度对中状态」行
|
||
if (align == 0x11)
|
||
addNoCodeFault(m_vsf4Faults, sys, brake, "ALIGN", 1, "方向盘对中失败");
|
||
else if (align != 0xEE && align != 0x55)
|
||
addNoCodeFault(m_vsf4Faults, sys, brake, "ALIGN", 0, "方向盘未对中");
|
||
|
||
// 注：协议表把两个信号的“信号值描述”写反了（Brake_Right 描述成左电机、
|
||
// Brake_Left 描述成右电机），此处以信号名为准。
|
||
const quint8 br = quint8(intelBits(d, 32, 8));
|
||
if (br != 0) addFault(m_vsf4Faults, sys, brake, "BRKR", br,
|
||
br < 8 ? kBrakeMotFaults[br] : "制动电机故障");
|
||
const quint8 bl = quint8(intelBits(d, 40, 8));
|
||
if (bl != 0) addFault(m_vsf4Faults, sys, brake, "BRKL", bl,
|
||
bl < 8 ? kBrakeMotFaults[bl] : "制动电机故障");
|
||
break;
|
||
}
|
||
|
||
// ---------- 0x18328180 机械臂状态1：末端位置 ----------
|
||
case kIdArmFb1: {
|
||
m_tel->pos = QString("X:%1 Y:%2 Z:%3")
|
||
.arg(intelSigned(d, 0, 16))
|
||
.arg(intelSigned(d, 16, 16))
|
||
.arg(intelSigned(d, 32, 16));
|
||
break;
|
||
}
|
||
// ---------- 0x18338180 机械臂状态2：末端姿态与末端速度 ----------
|
||
case kIdArmFb2: {
|
||
m_tel->pose = QString("RX:%1 RY:%2 RZ:%3")
|
||
.arg(intelSigned(d, 0, 16))
|
||
.arg(intelSigned(d, 16, 16))
|
||
.arg(intelSigned(d, 32, 16));
|
||
m_tel->vel = double(intelSigned(d, 48, 8));
|
||
m_tel->hasArmPose = true;
|
||
break;
|
||
}
|
||
// ---------- 0x18348180 机械臂状态3：关节角 J1~J4 ----------
|
||
case kIdArmFb3: {
|
||
if (m_tel->js.size() < 6) m_tel->js.resize(6);
|
||
for (int i = 0; i < 4; ++i)
|
||
m_tel->js[i] = double(intelSigned(d, i * 16, 16));
|
||
m_tel->hasJoints = true;
|
||
break;
|
||
}
|
||
// ---------- 0x18358180 机械臂状态4：关节角 J5~J6 ----------
|
||
case kIdArmFb4: {
|
||
if (m_tel->js.size() < 6) m_tel->js.resize(6);
|
||
m_tel->js[4] = double(intelSigned(d, 0, 16));
|
||
m_tel->js[5] = double(intelSigned(d, 16, 16));
|
||
m_tel->hasJoints = true;
|
||
break;
|
||
}
|
||
|
||
// ---------- 0x18368180 洗消工作状态1：模式 / 泵 / 球阀 / 液位 / 流量 ----------
|
||
case kIdDecontamFb1: {
|
||
const quint8 mode = quint8(intelBits(d, 0, 2));
|
||
m_tel->washMode = (mode == 3) ? QString::fromUtf8("手动消毒")
|
||
: (mode == 2) ? QString::fromUtf8("手动消除")
|
||
: QString("--");
|
||
m_tel->washStart = intelBits(d, 2, 1) != 0;
|
||
m_tel->plunger = intelBits(d, 3, 1) != 0;
|
||
m_tel->air = intelBits(d, 4, 1) != 0;
|
||
m_tel->leaf = intelBits(d, 5, 1) != 0;
|
||
|
||
m_tel->d1open = intelBits(d, 8, 1) != 0;
|
||
m_tel->d1close = !m_tel->d1open;
|
||
m_tel->d2open = intelBits(d, 9, 1) != 0;
|
||
m_tel->d2close = !m_tel->d2open;
|
||
|
||
m_washFaults.clear();
|
||
const QString sys = QString::fromUtf8(kSysWash);
|
||
const QString grp = QString::fromUtf8(kGroupWash);
|
||
if (intelBits(d, 10, 1) != 0) // Sys_fault：无分路码，只报总故障
|
||
addNoCodeFault(m_washFaults, sys, grp, "SYS", 1, "洗消系统故障");
|
||
const bool lowA = intelBits(d, 6, 1) != 0;
|
||
const bool lowB = intelBits(d, 7, 1) != 0;
|
||
if (lowA)
|
||
addNoCodeFault(m_washFaults, sys, grp, "LVL", 1, "A组分/清水液位低");
|
||
if (lowB)
|
||
addNoCodeFault(m_washFaults, sys, grp, "LVL", 2, "B组分液位低");
|
||
m_tel->levelALow = lowA; // 首页/作业选择中间区域的液位数值据此红闪
|
||
m_tel->levelBLow = lowB;
|
||
|
||
m_tel->levelA = double(intelBits(d, 16, 8));
|
||
m_tel->levelFoam = double(intelBits(d, 24, 8));
|
||
m_tel->flowA = double(intelBits(d, 32, 8));
|
||
m_tel->flowB = double(intelBits(d, 40, 8));
|
||
m_tel->gasFlow = double(intelBits(d, 48, 8));
|
||
m_tel->foam = double(intelBits(d, 56, 8));
|
||
m_tel->hasWash = true;
|
||
break;
|
||
}
|
||
// ---------- 0x18378180 洗消工作状态2：压力 / 流量反馈 / 泵故障码 ----------
|
||
case kIdDecontamFb2: {
|
||
m_tel->press = phys(intelBits(d, 0, 8), 0.1);
|
||
m_tel->airPress = phys(intelBits(d, 8, 8), 0.1);
|
||
m_tel->flowPlungerFb = double(intelBits(d, 16, 8));
|
||
m_tel->flowAirFb = double(intelBits(d, 32, 8));
|
||
m_tel->flowVaneFb = double(intelBits(d, 40, 8));
|
||
m_tel->hasWashPress = true;
|
||
|
||
m_pumpFaults.clear();
|
||
const QString washGrp = QString::fromUtf8(kGroupWash);
|
||
addFaultByCode(m_pumpFaults, QString::fromUtf8(kSysWash), washGrp, "AIR",
|
||
kAirCompFaults, int(sizeof(kAirCompFaults) / sizeof(CodeText)),
|
||
quint8(intelBits(d, 48, 8)));
|
||
addFaultByCode(m_pumpFaults, QString::fromUtf8(kSysWash), washGrp, "PLG",
|
||
kPlungerFaults, int(sizeof(kPlungerFaults) / sizeof(CodeText)),
|
||
quint8(intelBits(d, 56, 8)));
|
||
break;
|
||
}
|
||
|
||
// ---------- 0x18388180 检测仪结果状态1：状态 / 毒物种类与等级 / 告警 ----------
|
||
case kIdDetectorFb1: {
|
||
const quint8 sta = quint8(intelBits(d, 0, 8));
|
||
const quint8 warn = quint8(intelBits(d, 24, 8));
|
||
// 2=计算目标基线 3=等待稳定 4=检测 5=等待点火 6=确定初始基线
|
||
m_tel->gasHost = (warn & 0x04) ? QString("fault")
|
||
: (sta == 4) ? QString("detecting")
|
||
: QString("standby");
|
||
m_tel->hasGasHost = true;
|
||
|
||
// Toxic_types 是位组合：1=P类 2=S类 4=HNO类 8=CH类 16=AS类
|
||
const quint8 types = quint8(intelBits(d, 8, 8));
|
||
static const struct { quint8 bit; const char* name; } kTypes[] = {
|
||
{1, "P类"}, {2, "S类"}, {4, "HNO类"}, {8, "CH类"}, {16, "AS类"},
|
||
};
|
||
QStringList names;
|
||
for (const auto& t : kTypes)
|
||
if (types & t.bit) names << QString::fromUtf8(t.name);
|
||
m_tel->gasType = names.isEmpty() ? QString("--") : names.join("/");
|
||
|
||
const quint8 lvl = quint8(intelBits(d, 16, 8));
|
||
m_tel->gasLevel = (lvl >= 1 && lvl <= 5) ? QString("%1级").arg(lvl) : QString("--");
|
||
|
||
m_detectorFaults.clear();
|
||
// 告警是位组合（1/2/4/8），没有故障码，顶部只显示描述
|
||
for (const CodeText& w : kDetectorWarns)
|
||
if (warn & w.code)
|
||
addNoCodeFault(m_detectorFaults, QString::fromUtf8(kSysDetector),
|
||
QString::fromUtf8(kGroupDetector), "W", w.code,
|
||
w.text);
|
||
break;
|
||
}
|
||
// ---------- 0x18398180 检测仪结果状态2：40 位浓度（ppb） ----------
|
||
case kIdDetectorFb2: {
|
||
m_tel->gasConc = double(intelBits(d, 0, 40));
|
||
m_tel->hasGasConc = true;
|
||
break;
|
||
}
|
||
|
||
default:
|
||
break; // 未知 canId（分类四本次未实现）：这一帧不写任何东西
|
||
}
|
||
}
|
||
|
||
// 把各帧的故障合并进 Telemetry，并把“新出现”的故障写入故障历史
|
||
void CarCommunication::cacheFaults()
|
||
{
|
||
if (!m_tel) return;
|
||
|
||
const QVector<QVector<CanFault>*> groups = {
|
||
&m_vsf1Faults, &m_vsf2Faults, &m_vsf3Faults, &m_vsf4Faults,
|
||
};
|
||
|
||
// 页面按 desc 显示：等级型故障补回“一级故障/二级故障/三级故障”后缀，
|
||
// 与顶部轮播的“，N级”写法不同，保持页面既有文案不变。
|
||
auto pageText = [](const CanFault& f) {
|
||
return f.level > 0 ? f.desc + QString::fromUtf8(kLevels[f.level]) : f.desc;
|
||
};
|
||
|
||
QStringList chassis;
|
||
for (const QVector<CanFault>* g : groups)
|
||
for (const CanFault& f : *g) chassis << pageText(f);
|
||
m_tel->chassisFaults = chassis;
|
||
|
||
QStringList wash;
|
||
for (const CanFault& f : m_washFaults) wash << pageText(f);
|
||
m_tel->washFaults = wash;
|
||
|
||
QStringList pump;
|
||
for (const CanFault& f : m_pumpFaults) pump << pageText(f);
|
||
m_tel->pumpFaults = pump;
|
||
|
||
// 气体检测仪单列一份：洗消页底栏的“系统故障”灯只认洗消与泵，
|
||
// 检测仪的告警归到它自己的卡片，不能顺带点亮洗消的故障灯。
|
||
QStringList det;
|
||
for (const CanFault& f : m_detectorFaults) det << pageText(f);
|
||
m_tel->detectorFaults = det;
|
||
|
||
updateCommText();
|
||
|
||
// 周期报文会反复报到同一故障，只在首次出现时写故障历史
|
||
const QVector<QVector<CanFault>*> all = {
|
||
&m_vsf1Faults, &m_vsf2Faults, &m_vsf3Faults, &m_vsf4Faults,
|
||
&m_washFaults, &m_pumpFaults, &m_detectorFaults, &m_commFaults,
|
||
};
|
||
for (const QVector<CanFault>* g : all) {
|
||
for (const CanFault& f : *g) {
|
||
const QString key = f.system + "|" + f.code;
|
||
if (m_reportedFaults.contains(key)) continue;
|
||
m_reportedFaults.insert(key);
|
||
emit faultAppended(f.system, f.code, pageText(f));
|
||
}
|
||
}
|
||
}
|
||
|
||
// 顶部左区固定显示的那一行：只反映通信状态，不再轮播各项故障明细
|
||
// （底盘/洗消/气体检测仪的明细各自进对应卡片的“故障”行）。
|
||
void CarCommunication::updateCommText()
|
||
{
|
||
if (!m_tel) return;
|
||
|
||
QString text;
|
||
if (!m_lastCommOk)
|
||
text = QString::fromUtf8(kGroupComm); // 链路断时没有码，只能笼统报“通信异常”
|
||
else if (m_commFaultCode != 0)
|
||
text = commFaultText(quint8(m_commFaultCode));
|
||
else
|
||
text = QString::fromUtf8("系统通信正常");
|
||
|
||
// 周期报文会反复刷同样的内容，只有真变了才通知顶部
|
||
if (text == m_tel->commText && m_commFaultCode == m_tel->commCode) return;
|
||
m_tel->commText = text;
|
||
m_tel->commCode = m_commFaultCode;
|
||
emit commTextChanged();
|
||
}
|
||
|
||
// ===================== 发包 =====================
|
||
void CarCommunication::sendFrame(quint32 canId, const quint8* data)
|
||
{
|
||
if (m_sock->state() != QAbstractSocket::BoundState) return;
|
||
|
||
CanFrame f;
|
||
f.header = kFrameHeader;
|
||
f.canId = canId;
|
||
f.dlc = kDataLen;
|
||
std::memcpy(f.data, data, kDataLen);
|
||
f.checksum = canChecksum(canId, f.dlc, f.data);
|
||
f.tail = kFrameTail;
|
||
m_sock->writeDatagram(reinterpret_cast<const char*>(&f), kFrameLen,
|
||
QHostAddress(kCanTxHost), kCanTxPort);
|
||
}
|
||
|
||
// 洗消作业期间的末端俯仰动作，每 50ms 一拍，共三段：
|
||
// 启动 → SwingCentering：先把俯仰轴斜坡放回中位，到位后转 SwingSine
|
||
// 作业 → SwingSine： 从中位起按正弦来回，一个往返 kWashSwingTicks 拍
|
||
// 停止 → SwingRestoring：从当前的摆动值斜坡还原成作业前的轴值，到位后收手
|
||
// 这三轴在协议里叫「末端姿态俯仰动作」，是像摇杆偏摆那样的量（中位 = 不动），不是绝对位置：
|
||
// 所以"回中心"就是把轴值放回 kRcAxisMid，"回到洗消前的位置"就是放回 m_pitchBeforeWash。
|
||
// 停的时候**必须**把轴值收回到一个不动值——Arm_ControlCmd2 是周期报文，
|
||
// 停在哪个值，车端就认为还在给哪个俯仰指令、末端会一直走下去。
|
||
void CarCommunication::updateWashSwing()
|
||
{
|
||
switch (m_swingState) {
|
||
case SwingOff:
|
||
return;
|
||
|
||
case SwingCentering:
|
||
m_pitch = lerpAxis(m_pitchBeforeWash, kRcAxisMid, double(++m_swingTick) / kSwingRampTicks);
|
||
if (m_swingTick >= kSwingRampTicks) {
|
||
m_swingState = SwingSine;
|
||
m_swingPhase = 0; // 起摆点固定是中位，作业过程可复现
|
||
}
|
||
return;
|
||
|
||
case SwingSine: {
|
||
const double ph = 2.0 * kPi * m_swingPhase / kWashSwingTicks;
|
||
// 夹一下：万一以后把 kWashSwingAmp 调过 kRcAxisMid，负数转 quint16 会翻成极值
|
||
m_pitch = quint16(qBound(0, int(kRcAxisMid + kWashSwingAmp * std::sin(ph)), 4096));
|
||
m_swingPhase = (m_swingPhase + 1) % kWashSwingTicks;
|
||
return;
|
||
}
|
||
|
||
case SwingRestoring:
|
||
m_pitch = lerpAxis(m_swingFrom, m_pitchBeforeWash, double(++m_swingTick) / kSwingRampTicks);
|
||
if (m_swingTick >= kSwingRampTicks) {
|
||
m_pitch = m_pitchBeforeWash; // 收尾对齐，别停在插值的截断值上
|
||
m_swingState = SwingOff;
|
||
}
|
||
return;
|
||
}
|
||
}
|
||
|
||
// 50ms 组：Chassis_ControlCmd / Arm_ControlCmd1 / Arm_ControlCmd2 / Emergency_Cmd
|
||
void CarCommunication::onSendFast()
|
||
{
|
||
{
|
||
quint8 d[8] = {0};
|
||
setIntelBits(d, 0, 1, 1); // Chassis_modeCmd 有效
|
||
setIntelBits(d, 1, 2, 0); // MCU_modeCmd 底盘预留
|
||
setIntelBits(d, 3, 1, 1); // MCU_enable 有效
|
||
setIntelBits(d, 4, 4, 0); // Reserve 置 0
|
||
setIntelBits(d, 8, 16, m_gear);
|
||
setIntelBits(d, 24, 16, m_targetSpd);
|
||
setIntelBits(d, 40, 16, m_turning);
|
||
sendFrame(kIdChassisCmd, d);
|
||
}
|
||
{
|
||
quint8 d[8] = {0};
|
||
setIntelBits(d, 0, 1, m_armMode ? 1 : 0); // Arm_modeCmd
|
||
setIntelBits(d, 1, 1, m_armMode ? 1 : 0); // Arm_enable
|
||
setIntelBits(d, 2, 6, 0); // Reserve
|
||
setIntelBits(d, 8, 16, m_updown);
|
||
setIntelBits(d, 24, 16, m_leftright);
|
||
setIntelBits(d, 40, 16, m_frontrear);
|
||
sendFrame(kIdArmCmd1, d);
|
||
}
|
||
// 洗消作业期间的末端俯仰摆动：这一拍该发什么轴值由它决定
|
||
updateWashSwing();
|
||
{
|
||
quint8 d[8] = {0};
|
||
setIntelBits(d, 0, 16, m_roll);
|
||
setIntelBits(d, 16, 16, m_pitch);
|
||
setIntelBits(d, 32, 16, m_yaw);
|
||
setIntelBits(d, 48, 8, m_homingPause);
|
||
sendFrame(kIdArmCmd2, d);
|
||
}
|
||
{
|
||
quint8 d[8] = {0};
|
||
setIntelBits(d, 0, 8, m_emergStop);
|
||
sendFrame(kIdEmergency, d);
|
||
}
|
||
}
|
||
|
||
// 100ms 组：Decontam_ControlCmd / Decontam_DebugCmd
|
||
void CarCommunication::onSendMid()
|
||
{
|
||
{
|
||
quint8 d[8] = {0};
|
||
setIntelBits(d, 0, 2, m_decontamMode);
|
||
setIntelBits(d, 2, 1, m_decontamStart ? 1 : 0);
|
||
setIntelBits(d, 3, 5, 0); // Reserve 置 0
|
||
|
||
// 发泡倍数 / 泡沫比例 / 空压机流量 / 柱塞泵流量：均由页面保存时经 sendSetParam
|
||
// 写入缓存（构造时先按 config.ini 初始化），这里直接读缓存组帧，不再每帧读文件。
|
||
// 取值范围按协议「信号值描述」列收口：发泡倍数 6-12 倍、泡沫比例 0-10%、
|
||
// 空压机流量 0-340 L/min；界面 ParamControl 的可调范围与之一致。
|
||
setIntelBits(d, 8, 8, quint64(qBound(6, m_foamRatio, 12)));
|
||
setIntelBits(d, 16, 8, quint64(qBound(0, m_foamPercent, 10)));
|
||
setIntelBits(d, 24, 8, quint64(qBound(0, m_airFlow, 340)));
|
||
setIntelBits(d, 32, 8, quint64(qBound(0, m_plungerFlow, 24)));
|
||
sendFrame(kIdDecontamCmd, d);
|
||
}
|
||
{
|
||
quint8 d[8] = {0};
|
||
setIntelBits(d, 0, 1, m_debugMode ? 1 : 0);
|
||
setIntelBits(d, 1, 1, m_plungerOn ? 1 : 0);
|
||
setIntelBits(d, 2, 1, m_airOn ? 1 : 0);
|
||
setIntelBits(d, 3, 1, m_vaneOn ? 1 : 0);
|
||
setIntelBits(d, 4, 1, m_d1Open ? 1 : 0);
|
||
setIntelBits(d, 5, 1, m_d2Open ? 1 : 0);
|
||
setIntelBits(d, 6, 2, 0); // Debug_Reserve1
|
||
setIntelBits(d, 8, 16, quint64(qBound(0, int(m_airRpm), 3000)));
|
||
setIntelBits(d, 24, 16, quint64(qBound(0, int(m_plungerRpm), 1450)));
|
||
setIntelBits(d, 40, 16, quint64(qBound(0, int(m_vaneRpm), 2000)));
|
||
// 界面每个泵只有一组“启动/停止”，上电与启动同源
|
||
setIntelBits(d, 56, 1, m_airOn ? 1 : 0);
|
||
setIntelBits(d, 57, 1, m_plungerOn ? 1 : 0);
|
||
setIntelBits(d, 58, 1, m_vaneOn ? 1 : 0);
|
||
sendFrame(kIdDecontamDbg, d);
|
||
}
|
||
}
|
||
|
||
// 1000ms 组：Detector_ControlCmd
|
||
void CarCommunication::onSendSlow()
|
||
{
|
||
quint8 d[8] = {0};
|
||
setIntelBits(d, 0, 8, m_detectorCmd);
|
||
sendFrame(kIdDetectorCmd, d);
|
||
}
|
||
|
||
// ===================== 上行命令 → 控制状态 =====================
|
||
// 方向名 → 轴量：档位 后 0 / 中 2048 / 前 4096；转向 左 0 / 中 2048 / 右 4096
|
||
static void chassisAxis(const QString& dir, const QString& turn, quint16& gear, quint16& turning)
|
||
{
|
||
gear = (dir == "fwd") ? 4096 : (dir == "back") ? 0 : kRcAxisMid;
|
||
turning = (turn == "left") ? 0 : (turn == "right") ? 4096 : kRcAxisMid;
|
||
}
|
||
|
||
void CarCommunication::sendChassisMove(const QString& dir, double speed, const QString& turn)
|
||
{
|
||
if (m_rcActive) return; // 遥控器正在操作：面板这一路让位（遥控器松手后页面会重发）
|
||
sendChassisMoveRc(dir, speed, turn);
|
||
}
|
||
|
||
void CarCommunication::sendChassisMoveRc(const QString& dir, double speed, const QString& turn)
|
||
{
|
||
chassisAxis(dir, turn, m_gear, m_turning);
|
||
|
||
// Vehi_TargetSpd 精度 0.01 km/h，量程 0~500（即 0~5.00 km/h）
|
||
m_targetSpd = quint16(qBound(0, int(qRound(speed * 100.0)), 500));
|
||
}
|
||
|
||
void CarCommunication::sendArmMove(const QString& axis, int dir)
|
||
{
|
||
if (m_rcActive) return; // 遥控器正在操作：面板这一路让位（遥控器松手后页面会重发）
|
||
sendArmMoveRc(axis, dir);
|
||
}
|
||
|
||
void CarCommunication::sendArmMoveRc(const QString& axis, int dir)
|
||
{
|
||
// 轴位推杆量：dir +1 / -1 / 0 → 4096 / 0 / 中位
|
||
const quint16 v = (dir > 0) ? 4096 : (dir < 0) ? 0 : kRcAxisMid;
|
||
|
||
if (axis == "z") m_updown = v;
|
||
else if (axis == "x") m_leftright = v;
|
||
else if (axis == "y") m_frontrear = v;
|
||
else if (axis == "roll") m_roll = v;
|
||
else if (axis == "pitch") m_pitch = v;
|
||
else if (axis == "yaw") m_yaw = v;
|
||
}
|
||
|
||
void CarCommunication::sendArmReturn()
|
||
{
|
||
m_homingPause = 0xAA; // 系统回位
|
||
}
|
||
|
||
void CarCommunication::sendArmPause()
|
||
{
|
||
m_homingPause = 0x55; // 暂停回位
|
||
}
|
||
|
||
void CarCommunication::sendGasStart()
|
||
{
|
||
m_detectorCmd = 0xAA; // 启动检测
|
||
}
|
||
|
||
void CarCommunication::sendGasStop()
|
||
{
|
||
m_detectorCmd = 0x55; // 停止检测
|
||
}
|
||
|
||
void CarCommunication::sendWashStart(const QString& mode)
|
||
{
|
||
m_decontamMode = (mode == "disinfect") ? 3 : 2; // 3=手动消毒 2=手动消除
|
||
m_decontamStart = true;
|
||
|
||
// 末端俯仰摆动：记下作业前的轴值，先回中心再起摆（见 updateWashSwing）
|
||
m_pitchBeforeWash = m_pitch;
|
||
m_swingTick = 0;
|
||
if (m_pitch == kRcAxisMid) {
|
||
m_swingState = SwingSine; // 本来就在中位，不用空走一段回中
|
||
m_swingPhase = 0;
|
||
} else {
|
||
m_swingState = SwingCentering;
|
||
}
|
||
}
|
||
|
||
void CarCommunication::sendWashStop()
|
||
{
|
||
m_decontamStart = false;
|
||
|
||
// 摆动收尾：从当前值斜坡还原成作业前的轴值。
|
||
// 不直接把状态清掉，是因为清掉等于把最后一拍的轴值一直重发下去——末端会接着走。
|
||
if (m_swingState != SwingOff) {
|
||
m_swingFrom = int(m_pitch);
|
||
m_swingTick = 0;
|
||
// 已经就在作业前的位置（常见：作业前本来就是中位），不必多走一段
|
||
m_swingState = (m_swingFrom == int(m_pitchBeforeWash)) ? SwingOff : SwingRestoring;
|
||
if (m_swingState == SwingOff)
|
||
m_pitch = m_pitchBeforeWash;
|
||
}
|
||
}
|
||
|
||
void CarCommunication::sendBallValve(int valve, bool open)
|
||
{
|
||
if (valve == 1) m_d1Open = open;
|
||
else m_d2Open = open;
|
||
m_debugMode = true; // 球阀属于调试指令
|
||
}
|
||
|
||
void CarCommunication::sendPumpPower(const QString& pump, bool on)
|
||
{
|
||
if (pump == "plunger") m_plungerOn = on;
|
||
else if (pump == "air") m_airOn = on;
|
||
else if (pump == "leaf") m_vaneOn = on; // 叶片泵 → VanePump
|
||
m_debugMode = true;
|
||
}
|
||
|
||
void CarCommunication::sendPumpRpm(const QString& pump, int rpm)
|
||
{
|
||
if (pump == "plunger") m_plungerRpm = quint16(qBound(0, rpm, 1450));
|
||
else if (pump == "air") m_airRpm = quint16(qBound(0, rpm, 3000));
|
||
else if (pump == "leaf") m_vaneRpm = quint16(qBound(0, rpm, 2000));
|
||
}
|
||
|
||
void CarCommunication::sendSetParam(const QString& key, double value)
|
||
{
|
||
// 参数缓存与 config.ini 同源：页面保存时先落盘再调这里，周期帧只读缓存。
|
||
if (key == "param/speed")
|
||
m_targetSpd = quint16(qBound(0, int(qRound(value * 100.0)), 500));
|
||
else if (key == "param/pump_flow")
|
||
// 字段是 0~24 的整数，界面刻度是 0.1：必须四舍五入而不是截断，
|
||
// 否则 0.1 累加出的尾差（面板 5.0 实际存 4.9999999999999982）会下发成 4
|
||
m_plungerFlow = qRound(qBound(0.0, value, 24.0));
|
||
else if (key == "param/foam_ratio")
|
||
m_foamRatio = qRound(qBound(6.0, value, 12.0));
|
||
else if (key == "param/foam_percent")
|
||
m_foamPercent = qRound(qBound(0.0, value, 10.0));
|
||
else if (key == "param/air_flow")
|
||
m_airFlow = qRound(qBound(0.0, value, 340.0));
|
||
}
|
||
|
||
void CarCommunication::sendSelfCheckStart()
|
||
{
|
||
m_selfCheckIndex = 0;
|
||
m_selfCheckWait = 0;
|
||
m_selfCheckAllOk = true;
|
||
m_debugModeBeforeCheck = m_debugMode;
|
||
m_selfCheckTimer->start();
|
||
}
|
||
|
||
void CarCommunication::sendEstop()
|
||
{
|
||
m_emergStop = 0x55;
|
||
}
|
||
|
||
void CarCommunication::sendEstopReset()
|
||
{
|
||
m_emergStop = 0;
|
||
}
|
||
|
||
// TODO：CAN 矩阵（sheet RC）没有自检报文，故 0~9 项暂由本端计时直接判过，仅为维持
|
||
// ④自检 → ⑤作业选择 的流程不中断（onSelfCheckDone 是唯一点亮“作业选择”的地方）。
|
||
// 拿到真实自检报文后，改为解析该报文并删除这段本地驱动。
|
||
// 14 项里只有球阀那 4 项（kSelfCheckValveFirst 起）是真检查：要下发指令并等反馈。
|
||
void CarCommunication::onSelfCheckTick()
|
||
{
|
||
// ---- 球阀 D1/D2 开启、关闭：下发 Decontam_DebugCmd，等 MBV_D1_Sta/MBV_D2_Sta 反馈到位 ----
|
||
if (m_selfCheckIndex >= kSelfCheckValveFirst) {
|
||
const int k = m_selfCheckIndex - kSelfCheckValveFirst; // 0..3
|
||
const bool isD1 = (k < 2);
|
||
const bool wantOpen = (k % 2 == 0);
|
||
|
||
if (m_selfCheckWait == 0) {
|
||
// 项首拍：置调试模式 + 写目标阀位。命令帧由 100ms 周期定时器持续重发，
|
||
// 所以只需在这里改状态，不必每拍重发。
|
||
m_debugMode = true;
|
||
if (isD1) m_d1Open = wantOpen;
|
||
else m_d2Open = wantOpen;
|
||
emit selfCheckItemBusy(m_selfCheckIndex);
|
||
}
|
||
|
||
// 车端完全没有反馈时不能算“回复正常”：否则 d1open/d2open 的缺省值 false
|
||
// 会让“关闭”两项在遥测全无的情况下被误判通过。
|
||
const bool fbOpen = m_tel && (isD1 ? m_tel->d1open : m_tel->d2open);
|
||
const bool pass = m_lastCommOk && (fbOpen == wantOpen);
|
||
|
||
if (!pass && m_selfCheckWait < kValveWaitTicks) {
|
||
++m_selfCheckWait; // 反馈还没到位：继续等，本轮不推进序列
|
||
return;
|
||
}
|
||
m_selfCheckWait = 0;
|
||
finishSelfCheckItem(pass);
|
||
return;
|
||
}
|
||
|
||
// ---- 0~9：无对应报文，本地计时判过。演示开关只在此段生效 ----
|
||
finishSelfCheckItem(!(m_selfCheckFault && m_selfCheckIndex == kSelfCheckDemoFaultIndex));
|
||
}
|
||
|
||
// 记录单项结果并推进序列；跑完最后一项时收尾（恢复调试模式、发 selfCheckDone）。
|
||
// allOk 由本端累计，不能再用演示开关推——球阀那 4 项真的可能失败。
|
||
void CarCommunication::finishSelfCheckItem(bool ok)
|
||
{
|
||
m_selfCheckAllOk = m_selfCheckAllOk && ok;
|
||
emit selfCheckItem(m_selfCheckIndex, ok);
|
||
|
||
if (++m_selfCheckIndex < kSelfCheckItemCount)
|
||
return;
|
||
|
||
m_selfCheckTimer->stop();
|
||
m_debugMode = m_debugModeBeforeCheck; // 退出调试模式，别把整车留在自检状态
|
||
emit selfCheckDone(m_selfCheckAllOk);
|
||
}
|
||
|
||
void CarCommunication::onHeartbeatTimeout()
|
||
{
|
||
setCommOk(false);
|
||
// 超时后不再有收帧，cacheFaults 不会跑：这里补刷一次顶部，
|
||
// 否则顶部会一直停在“系统通信正常”上、看不出链路已经断了。
|
||
updateCommText();
|
||
}
|