224 lines
7.4 KiB
C++
224 lines
7.4 KiB
C++
#include "mockvehicle.h"
|
|
#include <QSettings>
|
|
#include <QJsonDocument>
|
|
#include <QJsonObject>
|
|
#include <QJsonArray>
|
|
#include <QHostAddress>
|
|
#include <QNetworkDatagram>
|
|
#include <QtMath>
|
|
|
|
MockVehicle::MockVehicle(QObject* parent)
|
|
: QObject(parent)
|
|
{
|
|
QSettings s;
|
|
m_localPort = s.value("net/local_port", 9000).toInt(); // 发送到遥控端
|
|
m_remotePort = s.value("net/remote_port", 9001).toInt(); // 本端监听
|
|
m_enabled = s.value("net/mock", true).toBool();
|
|
m_plantSelfCheckFault = s.value("net/selfcheck_fault", false).toBool();
|
|
|
|
if (!m_enabled)
|
|
return;
|
|
|
|
m_socket = new QUdpSocket(this);
|
|
m_socket->bind(QHostAddress::AnyIPv4, m_remotePort,
|
|
QUdpSocket::ShareAddress | QUdpSocket::ReuseAddressHint);
|
|
connect(m_socket, &QUdpSocket::readyRead, this, &MockVehicle::onReadyRead);
|
|
|
|
m_sendTimer = new QTimer(this);
|
|
connect(m_sendTimer, &QTimer::timeout, this, &MockVehicle::onSendTick);
|
|
m_sendTimer->start(500);
|
|
|
|
m_selfCheckTimer = new QTimer(this);
|
|
connect(m_selfCheckTimer, &QTimer::timeout, this, &MockVehicle::onSelfCheckTick);
|
|
}
|
|
|
|
void MockVehicle::sendJson(const QJsonObject& obj)
|
|
{
|
|
QByteArray data = QJsonDocument(obj).toJson(QJsonDocument::Compact);
|
|
m_socket->writeDatagram(data, QHostAddress("127.0.0.1"), m_localPort);
|
|
}
|
|
|
|
void MockVehicle::onReadyRead()
|
|
{
|
|
while (m_socket->hasPendingDatagrams()) {
|
|
QNetworkDatagram dg = m_socket->receiveDatagram();
|
|
QJsonParseError err;
|
|
QJsonDocument doc = QJsonDocument::fromJson(dg.data(), &err);
|
|
if (err.error == QJsonParseError::NoError && doc.isObject())
|
|
handleCommand(doc.object());
|
|
}
|
|
}
|
|
|
|
void MockVehicle::handleCommand(const QJsonObject& o)
|
|
{
|
|
const QString t = o.value("t").toString();
|
|
|
|
if (t == "chassis_move") {
|
|
const QString dir = o.value("dir").toString();
|
|
m_speed = o.value("speed").toDouble();
|
|
if (dir == "stop") m_speed = 0;
|
|
} else if (t == "arm_move") {
|
|
int d = o.value("dir").toInt();
|
|
if (d == 0) return;
|
|
if (m_js.size() >= 6)
|
|
m_js[1] = qBound(-90.0, m_js[1] + d * 1.0, 90.0);
|
|
} else if (t == "arm_return") {
|
|
m_armReturn = "returning";
|
|
m_armHomed = false;
|
|
} else if (t == "arm_pause") {
|
|
m_armReturn = "idle";
|
|
} else if (t == "gas_start") {
|
|
m_gasRunning = true;
|
|
} else if (t == "gas_stop") {
|
|
m_gasRunning = false;
|
|
} else if (t == "wash_start") {
|
|
m_washRunning = true;
|
|
m_washMode = (o.value("mode").toString() == "disinfect") ? "手动消毒" : "手动消除";
|
|
m_pPlunger = true; m_pAir = true; m_pLeaf = true;
|
|
} else if (t == "wash_stop") {
|
|
m_washRunning = false;
|
|
m_pPlunger = false; m_pAir = false; m_pLeaf = false;
|
|
} else if (t == "ball_valve") {
|
|
int v = o.value("valve").toInt();
|
|
bool open = o.value("open").toBool();
|
|
if (v == 1) m_valve1Open = open;
|
|
else m_valve2Open = open;
|
|
} else if (t == "pump_power") {
|
|
const QString p = o.value("pump").toString();
|
|
bool on = o.value("on").toBool();
|
|
if (p == "plunger") m_pPlunger = on;
|
|
else if (p == "air") m_pAir = on;
|
|
else if (p == "leaf") m_pLeaf = on;
|
|
} else if (t == "pump_rpm") {
|
|
const QString p = o.value("pump").toString();
|
|
int rpm = o.value("rpm").toInt();
|
|
if (p == "plunger") m_plungerRpm = rpm;
|
|
else if (p == "air") m_airRpm = rpm;
|
|
else if (p == "leaf") m_leafRpm = rpm;
|
|
} else if (t == "selfcheck_start") {
|
|
m_selfCheckActive = true;
|
|
m_selfCheckIndex = 0;
|
|
m_selfCheckTimer->start(450);
|
|
} else if (t == "estop") {
|
|
m_estop = true;
|
|
m_washRunning = false; m_gasRunning = false;
|
|
m_pPlunger = false; m_pAir = false; m_pLeaf = false;
|
|
QJsonObject o2; o2["t"] = "estop"; o2["triggered"] = true;
|
|
sendJson(o2);
|
|
} else if (t == "estop_reset") {
|
|
m_estop = false;
|
|
QJsonObject o2; o2["t"] = "estop"; o2["triggered"] = false;
|
|
sendJson(o2);
|
|
}
|
|
// set_param 仅用于演示，无需额外处理
|
|
}
|
|
|
|
void MockVehicle::onSendTick()
|
|
{
|
|
sendHeartbeat();
|
|
sendTelemetry();
|
|
}
|
|
|
|
void MockVehicle::sendHeartbeat()
|
|
{
|
|
QJsonObject o; o["t"] = "heartbeat"; o["ok"] = true;
|
|
sendJson(o);
|
|
}
|
|
|
|
void MockVehicle::sendTelemetry()
|
|
{
|
|
// 底盘
|
|
{
|
|
QJsonObject o;
|
|
QJsonArray faults;
|
|
if (m_estop) faults << QStringLiteral("急停触发");
|
|
o["t"] = "chassis";
|
|
o["soc"] = 85; o["volt"] = 360.0; o["speed"] = m_speed;
|
|
o["handbrake"] = true; o["faults"] = faults;
|
|
sendJson(o);
|
|
}
|
|
// 机械臂
|
|
{
|
|
if (m_armReturn == "returning") {
|
|
bool allZero = true;
|
|
for (double& a : m_js) { if (qAbs(a) > 1.0) a *= 0.7; else a = 0; if (qAbs(a) > 0.1) allZero = false; }
|
|
if (allZero) { m_armReturn = "done"; m_armHomed = true; }
|
|
}
|
|
QJsonObject o;
|
|
QJsonArray js;
|
|
for (double a : m_js) js << a;
|
|
o["t"] = "arm";
|
|
o["js"] = js;
|
|
o["pos"] = QString("X:%1 Y:%2 Z:%3").arg(1200).arg(300).arg(800);
|
|
o["pose"] = "RX:0 RY:0 RZ:0";
|
|
o["vel"] = (m_armReturn == "returning") ? 0.5 : 0.0;
|
|
o["return_state"] = m_armReturn;
|
|
o["homed"] = m_armHomed;
|
|
sendJson(o);
|
|
}
|
|
// 洗消
|
|
{
|
|
QJsonObject o;
|
|
o["t"] = "wash";
|
|
o["mode"] = m_washRunning ? m_washMode : "--";
|
|
o["flowA"] = m_washRunning ? 20.0 : 0.0;
|
|
o["flowB"] = m_washRunning ? 0.5 : 0.0;
|
|
o["gasFlow"] = m_washRunning ? 100.0 : 0.0;
|
|
o["press"] = m_washRunning ? 0.3 : 0.0;
|
|
o["airPress"] = m_washRunning ? 0.35 : 0.0;
|
|
o["foam"] = (m_washMode == "手动消毒") ? 6.0 : 8.0;
|
|
o["faults"] = QJsonArray();
|
|
sendJson(o);
|
|
}
|
|
// 液位
|
|
{
|
|
QJsonObject o; o["t"] = "tank"; o["a"] = 100.0; o["foam"] = 30.0;
|
|
sendJson(o);
|
|
}
|
|
// 气体检测仪
|
|
{
|
|
static int step = 0; step++;
|
|
double conc = m_gasRunning ? 250.0 + 50.0 * qSin(step * 0.4) : 0.0;
|
|
QJsonObject o;
|
|
o["t"] = "gas";
|
|
o["host"] = m_gasRunning ? "detecting" : "standby";
|
|
o["type"] = m_gasRunning ? "S类" : "--";
|
|
o["level"] = m_gasRunning ? "2级" : "--";
|
|
o["conc"] = qMax(0.0, conc);
|
|
sendJson(o);
|
|
}
|
|
// 球阀
|
|
{
|
|
QJsonObject o;
|
|
o["t"] = "valve";
|
|
o["d1open"] = m_valve1Open; o["d1close"] = !m_valve1Open;
|
|
o["d2open"] = m_valve2Open; o["d2close"] = !m_valve2Open;
|
|
o["fault"] = "";
|
|
sendJson(o);
|
|
}
|
|
// 泵
|
|
{
|
|
QJsonObject o;
|
|
o["t"] = "pump";
|
|
o["plunger"] = m_pPlunger; o["air"] = m_pAir; o["leaf"] = m_pLeaf;
|
|
o["faults"] = QJsonArray();
|
|
sendJson(o);
|
|
}
|
|
}
|
|
|
|
void MockVehicle::onSelfCheckTick()
|
|
{
|
|
// 自检 10 项：通信系统/底盘通信/机械臂通信/洗消通信/传感器信号/清水液位/球阀D1开/球阀D1关/球阀D2开/球阀D2关
|
|
bool ok = true;
|
|
if (m_plantSelfCheckFault && m_selfCheckIndex == 6) ok = false; // 球阀D1关闭故障
|
|
QJsonObject o; o["t"] = "selfcheck"; o["item"] = m_selfCheckIndex; o["ok"] = ok;
|
|
sendJson(o);
|
|
m_selfCheckIndex++;
|
|
if (m_selfCheckIndex >= 10) {
|
|
m_selfCheckTimer->stop();
|
|
m_selfCheckActive = false;
|
|
QJsonObject o2; o2["t"] = "selfcheck_done"; o2["all_ok"] = !m_plantSelfCheckFault;
|
|
sendJson(o2);
|
|
}
|
|
}
|