137 lines
4.5 KiB
C++
137 lines
4.5 KiB
C++
#include "WeldSeamPresenter.h"
|
|
|
|
#include "VrError.h"
|
|
#include "VrLog.h"
|
|
|
|
#include <QApplication>
|
|
#include <algorithm>
|
|
#include <array>
|
|
#include <cstring>
|
|
#include <vector>
|
|
|
|
namespace {
|
|
|
|
constexpr uint16_t kCommandAddress = 0;
|
|
constexpr uint16_t kStatusAddress = 1;
|
|
constexpr uint16_t kPointCountAddress = 2;
|
|
constexpr uint16_t kPointDataAddress = 3;
|
|
constexpr size_t kMaxModbusPoints = 40;
|
|
constexpr size_t kRegistersPerPoint = 6; // X/Y/Z: 3 float32
|
|
|
|
void FloatToRegisters(float value, uint16_t& high, uint16_t& low)
|
|
{
|
|
uint32_t raw = 0;
|
|
std::memcpy(&raw, &value, sizeof(value));
|
|
high = static_cast<uint16_t>(raw >> 16);
|
|
low = static_cast<uint16_t>(raw & 0xffff);
|
|
}
|
|
|
|
} // namespace
|
|
|
|
void WeldSeamPresenter::SendTCPResult(const DetectionResult& result)
|
|
{
|
|
if (!m_pTCPServer || !m_pTCPServer->IsConnected()) return;
|
|
if (!result.success || result.positions.empty()) {
|
|
m_pTCPServer->SendMessage(QStringLiteral("RESULT_NG"));
|
|
return;
|
|
}
|
|
|
|
QString packet = QStringLiteral("RESULT_OK_%1")
|
|
.arg(static_cast<int>(result.positions.size()));
|
|
for (const auto& point : result.positions) {
|
|
packet += QStringLiteral("_%1_%2_%3")
|
|
.arg(point.x, 0, 'f', 3)
|
|
.arg(point.y, 0, 'f', 3)
|
|
.arg(point.z, 0, 'f', 3);
|
|
}
|
|
m_pTCPServer->SendMessage(packet);
|
|
}
|
|
|
|
void WeldSeamPresenter::PublishModbusResult(const DetectionResult& result)
|
|
{
|
|
if (!IsModbusServerRunning()) return;
|
|
const size_t count = result.success
|
|
? std::min(result.positions.size(), kMaxModbusPoints) : 0;
|
|
const uint16_t countRegister = static_cast<uint16_t>(count);
|
|
WriteModbusRegisters(kPointCountAddress, &countRegister, 1);
|
|
|
|
std::vector<uint16_t> registers(kMaxModbusPoints * kRegistersPerPoint, 0);
|
|
for (size_t i = 0; i < count; ++i) {
|
|
const float values[3] = {
|
|
static_cast<float>(result.positions[i].x),
|
|
static_cast<float>(result.positions[i].y),
|
|
static_cast<float>(result.positions[i].z)
|
|
};
|
|
for (size_t axis = 0; axis < 3; ++axis) {
|
|
const size_t offset = i * kRegistersPerPoint + axis * 2;
|
|
FloatToRegisters(values[axis], registers[offset], registers[offset + 1]);
|
|
}
|
|
}
|
|
WriteModbusRegisters(kPointDataAddress, registers.data(),
|
|
static_cast<uint16_t>(registers.size()));
|
|
UpdateModbusStatus(result.success && count > 0 ? 3 : 4);
|
|
}
|
|
|
|
void WeldSeamPresenter::ResetModbusResults()
|
|
{
|
|
if (!IsModbusServerRunning()) return;
|
|
std::vector<uint16_t> zeros(1 + kMaxModbusPoints * kRegistersPerPoint, 0);
|
|
WriteModbusRegisters(kPointCountAddress, zeros.data(), static_cast<uint16_t>(zeros.size()));
|
|
}
|
|
|
|
void WeldSeamPresenter::UpdateModbusStatus(uint16_t status)
|
|
{
|
|
if (IsModbusServerRunning()) WriteModbusRegisters(kStatusAddress, &status, 1);
|
|
}
|
|
|
|
int WeldSeamPresenter::InitTCPServer()
|
|
{
|
|
if (m_pTCPServer) return SUCCESS;
|
|
m_pTCPServer = new WeldSeamTCPProtocol();
|
|
if (QApplication::instance()) {
|
|
m_pTCPServer->moveToThread(QApplication::instance()->thread());
|
|
}
|
|
|
|
connect(m_pTCPServer, &WeldSeamTCPProtocol::ConnectionChanged,
|
|
this, &WeldSeamPresenter::OnTCPConnectionChanged);
|
|
connect(m_pTCPServer, &WeldSeamTCPProtocol::StartScanRequested, this, [this]() {
|
|
const QString response = StartDynamicScan(-1);
|
|
if (m_pTCPServer) m_pTCPServer->SendMessage(response);
|
|
});
|
|
connect(m_pTCPServer, &WeldSeamTCPProtocol::StopScanRequested, this, [this]() {
|
|
const QString response = StopDynamicScanAndDetect();
|
|
if (m_pTCPServer) m_pTCPServer->SendMessage(response);
|
|
});
|
|
|
|
const uint16_t port = m_pConfigManager
|
|
? m_pConfigManager->GetConfigResult().tcpPort : 7800;
|
|
if (!m_pTCPServer->StartServer(port)) {
|
|
delete m_pTCPServer;
|
|
m_pTCPServer = nullptr;
|
|
return ERR_CODE(DEV_OPEN_ERR);
|
|
}
|
|
m_tcpPort = port;
|
|
return SUCCESS;
|
|
}
|
|
|
|
void WeldSeamPresenter::StopTCPServer()
|
|
{
|
|
if (!m_pTCPServer) return;
|
|
m_pTCPServer->StopServer();
|
|
delete m_pTCPServer;
|
|
m_pTCPServer = nullptr;
|
|
m_bTCPConnected = false;
|
|
}
|
|
|
|
void WeldSeamPresenter::OnTCPConnectionChanged(bool connected)
|
|
{
|
|
m_bTCPConnected = connected;
|
|
if (!connected && m_scanState.load() == ScanState::Scanning) {
|
|
CancelDynamicScan();
|
|
}
|
|
if (auto* status = GetStatusCallback<IYWeldSeamStatus>()) {
|
|
status->OnRobotConnectionChanged(connected);
|
|
status->OnStatusUpdate(connected ? "TCP客户端已连接" : "TCP客户端已断开");
|
|
}
|
|
}
|