243 lines
7.4 KiB
C++
243 lines
7.4 KiB
C++
#include "udpcommunication.h"
|
|
#include <QSettings>
|
|
#include <QJsonDocument>
|
|
#include <QJsonObject>
|
|
#include <QJsonArray>
|
|
#include <QHostAddress>
|
|
#include <QDateTime>
|
|
#include <QNetworkDatagram>
|
|
|
|
UdpCommunication::UdpCommunication(Telemetry* tel, QObject* parent)
|
|
: QObject(parent), m_tel(tel)
|
|
{
|
|
m_socket = new QUdpSocket(this);
|
|
connect(m_socket, &QUdpSocket::readyRead, this, &UdpCommunication::onReadyRead);
|
|
|
|
// 网络配置（默认回环：本地 9000 收，向 127.0.0.1:9001 发）
|
|
QSettings s;
|
|
m_localPort = s.value("net/local_port", 9000).toInt();
|
|
m_remoteHost = s.value("net/remote_host", "127.0.0.1").toString();
|
|
m_remotePort = s.value("net/remote_port", 9001).toInt();
|
|
m_mock = s.value("net/mock", true).toBool();
|
|
|
|
// 通信超时判定：连续 2s 无心跳 → 通信异常
|
|
m_hbTimer = new QTimer(this);
|
|
m_hbTimer->setInterval(2000);
|
|
connect(m_hbTimer, &QTimer::timeout, this, &UdpCommunication::onHeartbeatTimeout);
|
|
}
|
|
|
|
bool UdpCommunication::start()
|
|
{
|
|
if (m_socket->state() != QAbstractSocket::UnconnectedState)
|
|
return true;
|
|
bool ok = m_socket->bind(QHostAddress::AnyIPv4, m_localPort,
|
|
QUdpSocket::ShareAddress | QUdpSocket::ReuseAddressHint);
|
|
if (ok) m_hbTimer->start();
|
|
return ok;
|
|
}
|
|
|
|
void UdpCommunication::stop()
|
|
{
|
|
m_hbTimer->stop();
|
|
m_socket->close();
|
|
}
|
|
|
|
void UdpCommunication::sendJson(const QJsonObject& obj)
|
|
{
|
|
QByteArray data = QJsonDocument(obj).toJson(QJsonDocument::Compact);
|
|
m_socket->writeDatagram(data, QHostAddress(m_remoteHost), m_remotePort);
|
|
}
|
|
|
|
// ===================== 上行命令 =====================
|
|
void UdpCommunication::sendChassisMove(const QString& dir, double speed)
|
|
{
|
|
QJsonObject o; o["t"] = "chassis_move"; o["dir"] = dir; o["speed"] = speed;
|
|
sendJson(o);
|
|
}
|
|
|
|
void UdpCommunication::sendArmMove(const QString& axis, int dir)
|
|
{
|
|
QJsonObject o; o["t"] = "arm_move"; o["axis"] = axis; o["dir"] = dir;
|
|
sendJson(o);
|
|
}
|
|
|
|
void UdpCommunication::sendArmReturn()
|
|
{
|
|
QJsonObject o; o["t"] = "arm_return";
|
|
sendJson(o);
|
|
}
|
|
|
|
void UdpCommunication::sendArmPause()
|
|
{
|
|
QJsonObject o; o["t"] = "arm_pause";
|
|
sendJson(o);
|
|
}
|
|
|
|
void UdpCommunication::sendGasStart()
|
|
{
|
|
QJsonObject o; o["t"] = "gas_start";
|
|
sendJson(o);
|
|
}
|
|
|
|
void UdpCommunication::sendGasStop()
|
|
{
|
|
QJsonObject o; o["t"] = "gas_stop";
|
|
sendJson(o);
|
|
}
|
|
|
|
void UdpCommunication::sendWashStart(const QString& mode)
|
|
{
|
|
QJsonObject o; o["t"] = "wash_start"; o["mode"] = mode;
|
|
sendJson(o);
|
|
}
|
|
|
|
void UdpCommunication::sendWashStop()
|
|
{
|
|
QJsonObject o; o["t"] = "wash_stop";
|
|
sendJson(o);
|
|
}
|
|
|
|
void UdpCommunication::sendBallValve(int valve, bool open)
|
|
{
|
|
QJsonObject o; o["t"] = "ball_valve"; o["valve"] = valve; o["open"] = open;
|
|
sendJson(o);
|
|
}
|
|
|
|
void UdpCommunication::sendPumpPower(const QString& pump, bool on)
|
|
{
|
|
QJsonObject o; o["t"] = "pump_power"; o["pump"] = pump; o["on"] = on;
|
|
sendJson(o);
|
|
}
|
|
|
|
void UdpCommunication::sendPumpRpm(const QString& pump, int rpm)
|
|
{
|
|
QJsonObject o; o["t"] = "pump_rpm"; o["pump"] = pump; o["rpm"] = rpm;
|
|
sendJson(o);
|
|
}
|
|
|
|
void UdpCommunication::sendSetParam(const QString& key, double value)
|
|
{
|
|
QJsonObject o; o["t"] = "set_param"; o["key"] = key; o["value"] = value;
|
|
sendJson(o);
|
|
}
|
|
|
|
void UdpCommunication::sendSelfCheckStart()
|
|
{
|
|
QJsonObject o; o["t"] = "selfcheck_start";
|
|
sendJson(o);
|
|
}
|
|
|
|
void UdpCommunication::sendEstop()
|
|
{
|
|
QJsonObject o; o["t"] = "estop";
|
|
sendJson(o);
|
|
}
|
|
|
|
void UdpCommunication::sendEstopReset()
|
|
{
|
|
QJsonObject o; o["t"] = "estop_reset";
|
|
sendJson(o);
|
|
}
|
|
|
|
// ===================== 收包与解析 =====================
|
|
void UdpCommunication::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())
|
|
continue;
|
|
parse(doc.object());
|
|
}
|
|
}
|
|
|
|
void UdpCommunication::parse(const QJsonObject& o)
|
|
{
|
|
const QString t = o.value("t").toString();
|
|
|
|
if (t == "heartbeat") {
|
|
bool ok = o.value("ok").toBool();
|
|
m_hbTimer->start(); // 重置超时计时
|
|
if (ok != m_lastCommOk) {
|
|
m_lastCommOk = ok;
|
|
if (m_tel) m_tel->commOk = ok;
|
|
emit commOkChanged(ok);
|
|
}
|
|
return;
|
|
}
|
|
|
|
if (t == "chassis") {
|
|
m_tel->soc = o.value("soc").toInt();
|
|
m_tel->volt = o.value("volt").toDouble();
|
|
m_tel->speed = o.value("speed").toDouble();
|
|
m_tel->handbrake = o.value("handbrake").toBool();
|
|
m_tel->chassisFaults.clear();
|
|
for (const QJsonValue& v : o.value("faults").toArray())
|
|
m_tel->chassisFaults << v.toString();
|
|
} else if (t == "arm") {
|
|
const QJsonArray a = o.value("js").toArray();
|
|
m_tel->js.clear();
|
|
for (const QJsonValue& v : a) m_tel->js << v.toDouble();
|
|
m_tel->pos = o.value("pos").toString();
|
|
m_tel->pose = o.value("pose").toString();
|
|
m_tel->vel = o.value("vel").toDouble();
|
|
m_tel->returnState = o.value("return_state").toString("idle");
|
|
m_tel->armHomed = o.value("homed").toBool();
|
|
} else if (t == "wash") {
|
|
m_tel->washMode = o.value("mode").toString("--");
|
|
m_tel->flowA = o.value("flowA").toDouble();
|
|
m_tel->flowB = o.value("flowB").toDouble();
|
|
m_tel->gasFlow = o.value("gasFlow").toDouble();
|
|
m_tel->press = o.value("press").toDouble();
|
|
m_tel->airPress = o.value("airPress").toDouble();
|
|
m_tel->foam = o.value("foam").toDouble();
|
|
m_tel->washFaults.clear();
|
|
for (const QJsonValue& v : o.value("faults").toArray())
|
|
m_tel->washFaults << v.toString();
|
|
} else if (t == "tank") {
|
|
m_tel->levelA = o.value("a").toDouble();
|
|
m_tel->levelFoam = o.value("foam").toDouble();
|
|
} else if (t == "gas") {
|
|
m_tel->gasHost = o.value("host").toString("standby");
|
|
m_tel->gasType = o.value("type").toString("--");
|
|
m_tel->gasLevel = o.value("level").toString("--");
|
|
m_tel->gasConc = o.value("conc").toDouble();
|
|
} else if (t == "valve") {
|
|
m_tel->d1open = o.value("d1open").toBool();
|
|
m_tel->d1close = o.value("d1close").toBool();
|
|
m_tel->d2open = o.value("d2open").toBool();
|
|
m_tel->d2close = o.value("d2close").toBool();
|
|
m_tel->valveFault = o.value("fault").toString();
|
|
if (!m_tel->valveFault.isEmpty())
|
|
emit faultAppended("洗消系统", m_tel->valveFault, m_tel->valveFault);
|
|
} else if (t == "pump") {
|
|
m_tel->plunger = o.value("plunger").toBool();
|
|
m_tel->air = o.value("air").toBool();
|
|
m_tel->leaf = o.value("leaf").toBool();
|
|
m_tel->pumpFaults.clear();
|
|
for (const QJsonValue& v : o.value("faults").toArray())
|
|
m_tel->pumpFaults << v.toString();
|
|
} else if (t == "estop") {
|
|
m_tel->estop = o.value("triggered").toBool();
|
|
emit estopReceived(m_tel->estop);
|
|
} else if (t == "selfcheck") {
|
|
emit selfCheckItem(o.value("item").toInt(), o.value("ok").toBool());
|
|
} else if (t == "selfcheck_done") {
|
|
emit selfCheckDone(o.value("all_ok").toBool());
|
|
} else {
|
|
return; // 未知类型不触发刷新
|
|
}
|
|
|
|
emit telemetryUpdated();
|
|
}
|
|
|
|
void UdpCommunication::onHeartbeatTimeout()
|
|
{
|
|
if (m_lastCommOk) {
|
|
m_lastCommOk = false;
|
|
if (m_tel) m_tel->commOk = false;
|
|
emit commOkChanged(false);
|
|
}
|
|
}
|