1196 lines
44 KiB
C++
1196 lines
44 KiB
C++
#include "BagThreadPositionPresenter.h"
|
||
#include "VrError.h"
|
||
#include "VrLog.h"
|
||
#include <QtCore/QCoreApplication>
|
||
#include <QtCore/QFileInfo>
|
||
#include <QtCore/QDir>
|
||
#include <QtCore/QString>
|
||
#include <QtCore/QStandardPaths>
|
||
#include <QtCore/QFile>
|
||
#include <QtCore/QDateTime>
|
||
#include <cmath>
|
||
#include <algorithm>
|
||
#include <QImage>
|
||
#include <QThread>
|
||
#include <atomic>
|
||
#include <QJsonObject>
|
||
#include <QJsonArray>
|
||
#include <chrono>
|
||
|
||
#include "Version.h"
|
||
#include "VrTimeUtils.h"
|
||
#include "VrDateUtils.h"
|
||
#include "SG_baseDataType.h"
|
||
#include "VrConvert.h"
|
||
#include "DetectPresenter.h"
|
||
#include "PathManager.h"
|
||
#include "IGlLineLaserDevice.h" // GlLineLaserDevice接口
|
||
|
||
BagThreadPositionPresenter::BagThreadPositionPresenter(QObject *parent)
|
||
: BasePresenter(parent)
|
||
, m_pConfigManager(nullptr)
|
||
, m_pDetectPresenter(nullptr)
|
||
{
|
||
// 基类已经创建了相机重连定时器和检测数据缓存
|
||
}
|
||
|
||
BagThreadPositionPresenter::~BagThreadPositionPresenter()
|
||
{
|
||
// 关闭 ModbusTCP 客户端（必须在基类析构之前）
|
||
ShutdownModbusClient();
|
||
|
||
// 基类会自动处理：相机重连定时器、算法检测线程、检测数据缓存、相机设备资源、ModbusTCP服务器
|
||
|
||
// 释放ConfigManager（析构函数会自动调用Shutdown）
|
||
if (m_pConfigManager) {
|
||
delete m_pConfigManager;
|
||
m_pConfigManager = nullptr;
|
||
}
|
||
|
||
// 释放检测处理器
|
||
if(m_pDetectPresenter)
|
||
{
|
||
delete m_pDetectPresenter;
|
||
m_pDetectPresenter = nullptr;
|
||
}
|
||
}
|
||
|
||
int BagThreadPositionPresenter::InitApp()
|
||
{
|
||
LOG_DEBUG("Start APP Version: %s\n", BAGTHREADPOSITION_FULL_VERSION_STRING);
|
||
|
||
// 初始化连接状态
|
||
SetWorkStatus(WorkStatus::InitIng);
|
||
|
||
m_pDetectPresenter = new DetectPresenter();
|
||
|
||
int nRet = SUCCESS;
|
||
|
||
// 创建 ConfigManager 实例
|
||
m_pConfigManager = new ConfigManager();
|
||
if (!m_pConfigManager) {
|
||
LOG_ERROR("Failed to create ConfigManager instance\n");
|
||
if (auto pStatus = GetStatusCallback<IYBagThreadPositionStatus>()) pStatus->OnStatusUpdate("配置管理器创建失败");
|
||
return ERR_CODE(DEV_CONFIG_ERR);
|
||
}
|
||
|
||
// 初始化 ConfigManager
|
||
if (!m_pConfigManager->Initialize()) {
|
||
LOG_ERROR("Failed to initialize ConfigManager\n");
|
||
if (auto pStatus = GetStatusCallback<IYBagThreadPositionStatus>()) pStatus->OnStatusUpdate("配置管理器初始化失败");
|
||
return ERR_CODE(DEV_CONFIG_ERR);
|
||
}
|
||
|
||
LOG_INFO("Configuration loaded successfully\n");
|
||
|
||
// 获取配置结果
|
||
ConfigResult configResult = m_pConfigManager->GetConfigResult();
|
||
|
||
// 保存 ModbusTCP 大小端配置
|
||
m_modbusBigEndian = configResult.modbusBigEndian;
|
||
LOG_INFO("ModbusTCP bigEndian: %s\n", m_modbusBigEndian ? "true (大端)" : "false (小端)");
|
||
|
||
// 调用基类InitCamera进行相机初始化（bRGB=false, bSwing=false）
|
||
// 注意：BagThreadPosition使用GlLineLaserDevice，通过重写CreateDevice实现
|
||
InitCamera(configResult.cameraList, false, false);
|
||
|
||
LOG_INFO("Camera initialization completed. Connected cameras: %zu, default camera index: %d\n",
|
||
m_vrEyeDeviceList.size(), m_currentCameraIndex);
|
||
|
||
// 初始化各相机的基准距离
|
||
for (size_t i = 0; i < configResult.cameraList.size(); i++) {
|
||
double baseDistance = configResult.cameraList[i].baseDistance;
|
||
if (baseDistance != 0.0) {
|
||
ApplyBaseDistance(configResult.cameraList[i].index, baseDistance);
|
||
}
|
||
}
|
||
|
||
// 初始化 ModbusTCP 客户端（主动连接远端PLC）
|
||
InitModbusClient(configResult.modbusIP, configResult.modbusPort, configResult.modbusPollingInterval);
|
||
|
||
if (auto pStatus = GetStatusCallback<IYBagThreadPositionStatus>()) pStatus->OnStatusUpdate("设备初始化完成");
|
||
|
||
CheckAndUpdateWorkStatus();
|
||
|
||
if (auto pStatus = GetStatusCallback<IYBagThreadPositionStatus>()) pStatus->OnStatusUpdate("配置初始化成功");
|
||
|
||
return SUCCESS;
|
||
}
|
||
|
||
// 初始化算法参数（实现BasePresenter纯虚函数）
|
||
int BagThreadPositionPresenter::InitAlgoParams()
|
||
{
|
||
LOG_DEBUG("initializing algorithm parameters\n");
|
||
|
||
// 清空现有的手眼标定矩阵列表
|
||
m_clibMatrixList.clear();
|
||
|
||
// 获取手眼标定文件路径并确保文件存在
|
||
QString clibPath = PathManager::GetInstance().GetCalibrationFilePath();
|
||
|
||
LOG_INFO("Loading hand-eye matrices from: %s\n", clibPath.toStdString().c_str());
|
||
|
||
// 读取存在的矩阵数量
|
||
int nExistMatrixNum = CVrConvert::GetClibMatrixCount(clibPath.toStdString().c_str());
|
||
LOG_INFO("Found %d hand-eye calibration matrices\n", nExistMatrixNum);
|
||
|
||
// 循环加载每个矩阵
|
||
for(int matrixIndex = 0; matrixIndex < nExistMatrixNum; matrixIndex++)
|
||
{
|
||
// 构造矩阵标识符
|
||
char matrixIdent[64];
|
||
#ifdef _WIN32
|
||
sprintf_s(matrixIdent, "CalibMatrixInfo_%d", matrixIndex);
|
||
#else
|
||
sprintf(matrixIdent, "CalibMatrixInfo_%d", matrixIndex);
|
||
#endif
|
||
|
||
// 创建新的标定矩阵结构
|
||
CalibMatrix calibMatrix;
|
||
|
||
// 初始化为单位矩阵
|
||
double initClibMatrix[16] = {
|
||
1.0, 0.0, 0.0, 0.0, // 第一行
|
||
0.0, 1.0, 0.0, 0.0, // 第二行
|
||
0.0, 0.0, 1.0, 0.0, // 第三行
|
||
0.0, 0.0, 0.0, 1.0 // 第四行
|
||
};
|
||
|
||
// 加载矩阵数据
|
||
bool loadSuccess = CVrConvert::LoadClibMatrix(clibPath.toStdString().c_str(), matrixIdent, "dCalibMatrix", calibMatrix.clibMatrix);
|
||
|
||
if(loadSuccess)
|
||
{
|
||
m_clibMatrixList.push_back(calibMatrix);
|
||
LOG_INFO("Successfully loaded matrix %d\n", matrixIndex);
|
||
|
||
// 输出矩阵内容
|
||
QString clibMatrixStr;
|
||
LOG_INFO("Matrix %d content:\n", matrixIndex);
|
||
for (int i = 0; i < 4; ++i) {
|
||
clibMatrixStr.clear();
|
||
for (int j = 0; j < 4; ++j) {
|
||
clibMatrixStr += QString::asprintf("%8.4f ", calibMatrix.clibMatrix[i * 4 + j]);
|
||
}
|
||
LOG_INFO(" %s\n", clibMatrixStr.toStdString().c_str());
|
||
}
|
||
}
|
||
else
|
||
{
|
||
LOG_WARNING("Failed to load matrix %d, using identity matrix\n", matrixIndex);
|
||
// 如果加载失败，使用单位矩阵
|
||
memcpy(calibMatrix.clibMatrix, initClibMatrix, sizeof(initClibMatrix));
|
||
m_clibMatrixList.push_back(calibMatrix);
|
||
}
|
||
|
||
}
|
||
|
||
LOG_INFO("Total loaded %zu hand-eye calibration matrices\n", m_clibMatrixList.size());
|
||
|
||
// 从 ConfigManager 获取配置结果
|
||
ConfigResult configResult = m_pConfigManager->GetConfigResult();
|
||
const VrAlgorithmParams& xmlParams = configResult.algorithmParams;
|
||
|
||
LOG_INFO("Loaded XML params - Thread: isHorizonScan=%s\n",
|
||
xmlParams.threadParam.isHorizonScan ? "true" : "false");
|
||
LOG_INFO("Loaded XML params - Filter: continuityTh=%.1f, outlierTh=%.1f\n",
|
||
xmlParams.filterParam.continuityTh, xmlParams.filterParam.outlierTh);
|
||
|
||
LOG_INFO("Algorithm parameters initialized successfully\n");
|
||
|
||
return SUCCESS;
|
||
}
|
||
|
||
// 手眼标定矩阵管理方法实现
|
||
CalibMatrix BagThreadPositionPresenter::GetClibMatrix(int index) const
|
||
{
|
||
CalibMatrix clibMatrix;
|
||
|
||
double initClibMatrix[16] = {
|
||
1.0, 0.0, 0.0, 0.0, // 第一行
|
||
0.0, 1.0, 0.0, 0.0, // 第二行
|
||
0.0, 0.0, 1.0, 0.0, // 第三行
|
||
0.0, 0.0, 0.0, 1.0 // 第四行
|
||
};
|
||
memcpy(clibMatrix.clibMatrix, initClibMatrix, sizeof(initClibMatrix));
|
||
|
||
if (index >= 0 && index < static_cast<int>(m_clibMatrixList.size())) {
|
||
clibMatrix = m_clibMatrixList[index];
|
||
memcpy(clibMatrix.clibMatrix, m_clibMatrixList[index].clibMatrix, sizeof(initClibMatrix));
|
||
} else {
|
||
LOG_WARNING("Invalid hand-eye calibration matrix\n");
|
||
}
|
||
|
||
return clibMatrix;
|
||
}
|
||
|
||
|
||
void BagThreadPositionPresenter::CheckAndUpdateWorkStatus()
|
||
{
|
||
// 使用基类的 m_bCameraConnected 标志
|
||
if (m_bCameraConnected) {
|
||
SetWorkStatus(WorkStatus::Ready);
|
||
} else {
|
||
SetWorkStatus(WorkStatus::Error);
|
||
}
|
||
}
|
||
|
||
// 实现BasePresenter纯虚函数：执行算法检测
|
||
int BagThreadPositionPresenter::ProcessAlgoDetection(std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& detectionDataCache)
|
||
{
|
||
LOG_INFO("[Algo Thread] Start real detection task using algorithm\n");
|
||
|
||
// 1. 获取缓存的点云数据（已由基类验证非空）
|
||
unsigned int lineNum = detectionDataCache.size();
|
||
if(GetStatusCallback<IYBagThreadPositionStatus>()){
|
||
if (auto pStatus = GetStatusCallback<IYBagThreadPositionStatus>()) pStatus->OnStatusUpdate("扫描线数:" + std::to_string(lineNum) + "，正在算法检测...");
|
||
}
|
||
|
||
// 检查检测处理器是否已初始化
|
||
if (!m_pDetectPresenter) {
|
||
LOG_ERROR("DetectPresenter is null, cannot proceed with detection\n");
|
||
if (GetStatusCallback<IYBagThreadPositionStatus>()) {
|
||
if (auto pStatus = GetStatusCallback<IYBagThreadPositionStatus>()) pStatus->OnStatusUpdate("检测处理器未初始化");
|
||
}
|
||
|
||
// 更新ModbusTCP寄存器：地址2 = 2（检测失败）
|
||
if (IsModbusClientConnected()) {
|
||
WriteRemoteSingleRegister(2, 2); // 失败
|
||
LOG_INFO("ModbusTCP: 检测失败（处理器未初始化），地址2写入2\n");
|
||
}
|
||
|
||
return ERR_CODE(DEV_NOT_FIND);
|
||
}
|
||
|
||
CVrTimeUtils oTimeUtils;
|
||
|
||
// 获取当前使用的手眼标定矩阵
|
||
const CalibMatrix currentClibMatrix = GetClibMatrix(m_currentCameraIndex - 1);
|
||
|
||
// 从 ConfigManager 获取算法参数和调试参数
|
||
VrAlgorithmParams algorithmParams = m_pConfigManager->GetAlgorithmParams();
|
||
ConfigResult configResult = m_pConfigManager->GetConfigResult();
|
||
VrDebugParam debugParam = configResult.debugParam;
|
||
|
||
DetectionResult detectionResult;
|
||
|
||
int nRet = m_pDetectPresenter->DetectScrew(m_currentCameraIndex, detectionDataCache,
|
||
algorithmParams, debugParam, m_dataLoader,
|
||
currentClibMatrix.clibMatrix, detectionResult);
|
||
// 根据项目类型选择处理方式
|
||
if (GetStatusCallback<IYBagThreadPositionStatus>()) {
|
||
QString err = QString("错误:%1").arg(nRet);
|
||
if (auto pStatus = GetStatusCallback<IYBagThreadPositionStatus>()) pStatus->OnStatusUpdate(QString("检测%1").arg(SUCCESS == nRet ? "成功": err).toStdString());
|
||
}
|
||
|
||
LOG_INFO("[Algo Thread] sx_bagThreadMeasure detected %zu objects time : %.2f ms\n", detectionResult.positions.size(), oTimeUtils.GetElapsedTimeInMilliSec());
|
||
|
||
// 如果检测失败，更新ModbusTCP状态并恢复轮询
|
||
if (nRet != SUCCESS) {
|
||
if (IsModbusClientConnected()) {
|
||
WriteRemoteSingleRegister(2, 2); // 失败
|
||
LOG_INFO("ModbusTCP: 检测失败，地址2写入2\n");
|
||
}
|
||
// 检测失败也要恢复轮询，继续读取扫描请求
|
||
m_modbusPollingPaused = false;
|
||
}
|
||
|
||
ERR_CODE_RETURN(nRet);
|
||
|
||
// 8. 通知UI检测结果
|
||
detectionResult.cameraIndex = m_currentCameraIndex;
|
||
if (auto pStatus = GetStatusCallback<IYBagThreadPositionStatus>()) {
|
||
pStatus->OnDetectionResult(detectionResult);
|
||
}
|
||
|
||
// 保存检测结果用于ModbusTCP输出
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_modbusResultMutex);
|
||
m_lastDetectionResult = detectionResult;
|
||
}
|
||
|
||
// 更新ModbusTCP寄存器：地址2 = 1（检测成功），并输出xyzu数据到地址4
|
||
if (IsModbusClientConnected()) {
|
||
WriteRemoteSingleRegister(2, 1); // 成功
|
||
LOG_INFO("ModbusTCP: 检测成功，地址2写入1\n");
|
||
|
||
// 直接输出xyzu数据到地址4
|
||
if (!detectionResult.threadInfoList.empty()) {
|
||
size_t threadCount = detectionResult.threadInfoList.size();
|
||
std::vector<uint16_t> outputData;
|
||
outputData.reserve(threadCount * 8);
|
||
|
||
LOG_INFO("ModbusTCP: 准备输出 %zu 条拆线的xyzu数据\n", threadCount);
|
||
|
||
// 将每个拆线的 x, y, z, u 转换为寄存器数据
|
||
for (const auto& thread : detectionResult.threadInfoList) {
|
||
uint16_t regs[2];
|
||
|
||
// x (centerX)
|
||
float x = static_cast<float>(thread.centerX);
|
||
_FloatToRegisters(x, regs, m_modbusBigEndian);
|
||
outputData.push_back(regs[0]);
|
||
outputData.push_back(regs[1]);
|
||
|
||
// y (centerY)
|
||
float y = static_cast<float>(thread.centerY);
|
||
_FloatToRegisters(y, regs, m_modbusBigEndian);
|
||
outputData.push_back(regs[0]);
|
||
outputData.push_back(regs[1]);
|
||
|
||
// z (centerZ)
|
||
float z = static_cast<float>(thread.centerZ);
|
||
_FloatToRegisters(z, regs, m_modbusBigEndian);
|
||
outputData.push_back(regs[0]);
|
||
outputData.push_back(regs[1]);
|
||
|
||
// u (rotateAngle)
|
||
float u = static_cast<float>(thread.rotateAngle);
|
||
_FloatToRegisters(u, regs, m_modbusBigEndian);
|
||
outputData.push_back(regs[0]);
|
||
outputData.push_back(regs[1]);
|
||
|
||
LOG_DEBUG("ModbusTCP: 拆线数据: x=%.3f, y=%.3f, z=%.3f, u=%.3f\n",
|
||
thread.centerX, thread.centerY, thread.centerZ, thread.rotateAngle);
|
||
}
|
||
|
||
// 从地址4开始写入数据
|
||
if (!outputData.empty()) {
|
||
bool ret = WriteRemoteRegisters(4, outputData);
|
||
if (ret) {
|
||
LOG_INFO("ModbusTCP: 成功输出 %zu 条拆线的xyzu数据到寄存器（地址4起，共%zu个寄存器）\n",
|
||
threadCount, outputData.size());
|
||
} else {
|
||
LOG_ERROR("ModbusTCP: 输出xyzu数据失败\n");
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
// 恢复轮询（检测完成后恢复读取扫描请求）
|
||
m_modbusPollingPaused = false;
|
||
|
||
// 更新状态
|
||
QString statusMsg = QString("检测完成，发现%1条拆线").arg(detectionResult.positions.size() / 2);
|
||
if (auto pStatus = GetStatusCallback<IYBagThreadPositionStatus>()) pStatus->OnStatusUpdate(statusMsg.toStdString());
|
||
|
||
// 9. 检测完成后，将工作状态更新为"完成"
|
||
SetWorkStatus(WorkStatus::Completed);
|
||
|
||
// 恢复到就绪状态
|
||
SetWorkStatus(WorkStatus::Ready);
|
||
|
||
return SUCCESS;
|
||
}
|
||
|
||
|
||
// 实现配置改变通知接口
|
||
void BagThreadPositionPresenter::OnConfigChanged(const ConfigResult& configResult)
|
||
{
|
||
LOG_INFO("Configuration changed notification received, reloading algorithm parameters\n");
|
||
|
||
// 重新初始化算法参数
|
||
int result = InitAlgoParams();
|
||
if (result == SUCCESS) {
|
||
LOG_INFO("Algorithm parameters reloaded successfully after config change\n");
|
||
if (GetStatusCallback<IYBagThreadPositionStatus>()) {
|
||
if (auto pStatus = GetStatusCallback<IYBagThreadPositionStatus>()) pStatus->OnStatusUpdate("配置已更新，算法参数重新加载成功");
|
||
}
|
||
} else {
|
||
LOG_ERROR("Failed to reload algorithm parameters after config change, error: %d\n", result);
|
||
if (GetStatusCallback<IYBagThreadPositionStatus>()) {
|
||
if (auto pStatus = GetStatusCallback<IYBagThreadPositionStatus>()) pStatus->OnStatusUpdate("配置更新后算法参数重新加载失败");
|
||
}
|
||
}
|
||
}
|
||
|
||
// 根据相机索引获取调平参数
|
||
SSG_planeCalibPara BagThreadPositionPresenter::_GetCameraCalibParam(int cameraIndex)
|
||
{
|
||
// 查找指定相机索引的调平参数
|
||
SSG_planeCalibPara calibParam;
|
||
|
||
// 使用单位矩阵（未校准状态）
|
||
double identityMatrix[9] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
|
||
for (int i = 0; i < 9; i++) {
|
||
calibParam.planeCalib[i] = identityMatrix[i];
|
||
calibParam.invRMatrix[i] = identityMatrix[i];
|
||
}
|
||
calibParam.planeHeight = -1.0; // 使用默认高度
|
||
|
||
// 从 ConfigManager 获取算法参数
|
||
VrAlgorithmParams algorithmParams = m_pConfigManager->GetAlgorithmParams();
|
||
|
||
for (const auto& cameraParam : algorithmParams.planeCalibParam.cameraCalibParams) {
|
||
if (cameraParam.cameraIndex == cameraIndex) {
|
||
|
||
// 根据isCalibrated标志决定使用标定矩阵还是单位矩阵
|
||
if (cameraParam.isCalibrated) {
|
||
// 使用实际的标定矩阵
|
||
for (int i = 0; i < 9; i++) {
|
||
calibParam.planeCalib[i] = cameraParam.planeCalib[i];
|
||
calibParam.invRMatrix[i] = cameraParam.invRMatrix[i];
|
||
}
|
||
calibParam.planeHeight = cameraParam.planeHeight;
|
||
}
|
||
}
|
||
}
|
||
return calibParam;
|
||
}
|
||
|
||
// 实现BasePresenter纯虚函数：相机状态变化通知
|
||
void BagThreadPositionPresenter::OnCameraStatusChanged(int cameraIndex, bool isConnected)
|
||
{
|
||
LOG_INFO("Camera %d status changed: %s\n", cameraIndex, isConnected ? "connected" : "disconnected");
|
||
|
||
// 通知UI更新相机状态
|
||
if (GetStatusCallback<IYBagThreadPositionStatus>()) {
|
||
if (cameraIndex == 1) {
|
||
if (auto pStatus = GetStatusCallback<IYBagThreadPositionStatus>()) pStatus->OnCamera1StatusChanged(isConnected);
|
||
} else if (cameraIndex == 2) {
|
||
if (auto pStatus = GetStatusCallback<IYBagThreadPositionStatus>()) pStatus->OnCamera2StatusChanged(isConnected);
|
||
}
|
||
|
||
// 获取相机名称用于状态消息
|
||
QString cameraName;
|
||
int arrayIndex = cameraIndex - 1;
|
||
if (arrayIndex >= 0 && arrayIndex < static_cast<int>(m_vrEyeDeviceList.size())) {
|
||
cameraName = QString::fromStdString(m_vrEyeDeviceList[arrayIndex].first);
|
||
} else {
|
||
cameraName = QString("相机%1").arg(cameraIndex);
|
||
}
|
||
|
||
QString statusMsg = QString("%1%2").arg(cameraName).arg(isConnected ? "已连接" : "已断开");
|
||
if (auto pStatus = GetStatusCallback<IYBagThreadPositionStatus>()) pStatus->OnStatusUpdate(statusMsg.toStdString());
|
||
}
|
||
|
||
// 检查并更新工作状态
|
||
CheckAndUpdateWorkStatus();
|
||
}
|
||
|
||
// 实现BasePresenter虚函数：工作状态变化通知
|
||
void BagThreadPositionPresenter::OnWorkStatusChanged(WorkStatus status)
|
||
{
|
||
auto pStatus = GetStatusCallback<IYBagThreadPositionStatus>();
|
||
if (pStatus) {
|
||
pStatus->OnWorkStatusChanged(status);
|
||
}
|
||
}
|
||
|
||
|
||
// 实现BasePresenter虚函数：相机数量变化通知
|
||
void BagThreadPositionPresenter::OnCameraCountChanged(int count)
|
||
{
|
||
auto pStatus = GetStatusCallback<IYBagThreadPositionStatus>();
|
||
if (pStatus) {
|
||
pStatus->OnCameraCountChanged(count);
|
||
}
|
||
}
|
||
|
||
// 实现BasePresenter虚函数：状态文字更新通知
|
||
void BagThreadPositionPresenter::OnStatusUpdate(const std::string& statusMessage)
|
||
{
|
||
auto pStatus = GetStatusCallback<IYBagThreadPositionStatus>();
|
||
if (pStatus) {
|
||
pStatus->OnStatusUpdate(statusMessage);
|
||
}
|
||
}
|
||
|
||
// 实现BasePresenter虚函数：ModbusTCP连接状态变化通知
|
||
void BagThreadPositionPresenter::OnModbusServerStatusChanged(bool isConnected)
|
||
{
|
||
LOG_INFO("ModbusTCP Client 连接状态: %s\n", isConnected ? "已连接" : "已断开");
|
||
|
||
auto pStatus = GetStatusCallback<IYBagThreadPositionStatus>();
|
||
if (pStatus) {
|
||
// ModbusTCP连接状态关联到机械臂连接状态
|
||
pStatus->OnRobotConnectionChanged(isConnected);
|
||
|
||
// 更新状态文字，显示远端IP
|
||
std::string statusMsg = isConnected ?
|
||
"ModbusTCP已连接到 " + m_modbusClientIP :
|
||
"ModbusTCP已断开（" + m_modbusClientIP + "）";
|
||
pStatus->OnStatusUpdate(statusMsg);
|
||
}
|
||
}
|
||
|
||
// ============ 实现 ICameraLevelCalculator 接口 ============
|
||
|
||
bool BagThreadPositionPresenter::CalculatePlaneCalibration(
|
||
const std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& scanData,
|
||
double planeCalib[9],
|
||
double& planeHeight,
|
||
double invRMatrix[9])
|
||
{
|
||
try {
|
||
// 检查是否有足够的扫描数据
|
||
if (scanData.empty()) {
|
||
LOG_ERROR("No scan data available for plane calibration\n");
|
||
return false;
|
||
}
|
||
|
||
LOG_INFO("Calculating plane calibration from %zu scan lines\n", scanData.size());
|
||
|
||
// 转换为算法需要的XYZ格式
|
||
LaserDataLoader dataLoader;
|
||
std::vector<std::vector<SVzNL3DPosition>> xyzData;
|
||
int convertResult = dataLoader.ConvertToSVzNL3DPosition(scanData, xyzData);
|
||
if (convertResult != SUCCESS || xyzData.empty()) {
|
||
LOG_WARNING("Failed to convert data to XYZ format or no XYZ data available\n");
|
||
return false;
|
||
}
|
||
|
||
// 拆线定位项目暂时使用简单的平面拟合
|
||
// 使用默认的单位矩阵
|
||
double identity[9] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
|
||
memcpy(planeCalib, identity, sizeof(double) * 9);
|
||
memcpy(invRMatrix, identity, sizeof(double) * 9);
|
||
planeHeight = -1.0;
|
||
|
||
LOG_INFO("Plane calibration calculated successfully: height=%.3f\n", planeHeight);
|
||
return true;
|
||
|
||
} catch (const std::exception& e) {
|
||
LOG_ERROR("Exception in CalculatePlaneCalibration: %s\n", e.what());
|
||
return false;
|
||
} catch (...) {
|
||
LOG_ERROR("Unknown exception in CalculatePlaneCalibration\n");
|
||
return false;
|
||
}
|
||
}
|
||
|
||
// ============ 实现 ICameraLevelResultSaver 接口 ============
|
||
|
||
bool BagThreadPositionPresenter::SaveLevelingResults(double planeCalib[9], double planeHeight, double invRMatrix[9],
|
||
int cameraIndex, const QString& cameraName)
|
||
{
|
||
try {
|
||
if (!m_pConfigManager) {
|
||
LOG_ERROR("ConfigManager is null, cannot save leveling results\n");
|
||
return false;
|
||
}
|
||
|
||
// 验证传入的相机参数
|
||
if (cameraIndex <= 0) {
|
||
LOG_ERROR("Invalid camera index: %d\n", cameraIndex);
|
||
return false;
|
||
}
|
||
|
||
if (cameraName.isEmpty()) {
|
||
LOG_ERROR("Camera name is empty\n");
|
||
return false;
|
||
}
|
||
|
||
// 获取当前配置
|
||
QString configPath = PathManager::GetInstance().GetConfigFilePath();
|
||
LOG_INFO("Config path: %s\n", configPath.toUtf8().constData());
|
||
|
||
SystemConfig systemConfig = m_pConfigManager->GetConfig();
|
||
|
||
// 创建或更新指定相机的调平参数
|
||
VrCameraPlaneCalibParam cameraParam;
|
||
cameraParam.cameraIndex = cameraIndex;
|
||
cameraParam.cameraName = cameraName.toStdString();
|
||
cameraParam.planeHeight = planeHeight;
|
||
cameraParam.isCalibrated = true;
|
||
|
||
// 复制校准矩阵
|
||
for (int i = 0; i < 9; i++) {
|
||
cameraParam.planeCalib[i] = planeCalib[i];
|
||
cameraParam.invRMatrix[i] = invRMatrix[i];
|
||
}
|
||
|
||
// 更新配置中的相机校准参数
|
||
systemConfig.configResult.algorithmParams.planeCalibParam.SetCameraCalibParam(cameraParam);
|
||
|
||
// 更新并保存配置
|
||
if (!m_pConfigManager->UpdateFullConfig(systemConfig)) {
|
||
LOG_ERROR("Failed to update config with leveling results\n");
|
||
return false;
|
||
}
|
||
|
||
if (!m_pConfigManager->SaveConfigToFile(configPath.toStdString())) {
|
||
LOG_ERROR("Failed to save config file with leveling results\n");
|
||
return false;
|
||
}
|
||
|
||
LOG_INFO("Leveling results saved successfully for camera %d (%s)\n", cameraIndex, cameraName.toUtf8().constData());
|
||
LOG_INFO("Plane height: %.3f\n", planeHeight);
|
||
LOG_INFO("Calibration marked as completed\n");
|
||
|
||
return true;
|
||
|
||
} catch (const std::exception& e) {
|
||
LOG_ERROR("Exception in SaveLevelingResults: %s\n", e.what());
|
||
return false;
|
||
}
|
||
}
|
||
|
||
bool BagThreadPositionPresenter::LoadLevelingResults(int cameraIndex, const QString& cameraName,
|
||
double planeCalib[9], double& planeHeight, double invRMatrix[9])
|
||
{
|
||
try {
|
||
if (!m_pConfigManager) {
|
||
LOG_ERROR("ConfigManager is null, cannot load calibration data\n");
|
||
return false;
|
||
}
|
||
|
||
// 从ConfigManager获取配置结果
|
||
ConfigResult configResult = m_pConfigManager->GetConfigResult();
|
||
|
||
// 获取指定相机的标定参数
|
||
VrCameraPlaneCalibParam cameraParamValue;
|
||
if (!configResult.algorithmParams.planeCalibParam.GetCameraCalibParam(cameraIndex, cameraParamValue) || !cameraParamValue.isCalibrated) {
|
||
LOG_INFO("No calibration data found for camera %d (%s)\n", cameraIndex, cameraName.toUtf8().constData());
|
||
return false;
|
||
}
|
||
|
||
// 复制标定数据
|
||
for (int i = 0; i < 9; i++) {
|
||
planeCalib[i] = cameraParamValue.planeCalib[i];
|
||
invRMatrix[i] = cameraParamValue.invRMatrix[i];
|
||
}
|
||
planeHeight = cameraParamValue.planeHeight;
|
||
|
||
LOG_INFO("Calibration data loaded successfully for camera %d (%s)\n", cameraIndex, cameraName.toUtf8().constData());
|
||
LOG_INFO("Plane height: %.3f\n", planeHeight);
|
||
|
||
return true;
|
||
|
||
} catch (const std::exception& e) {
|
||
LOG_ERROR("Exception in LoadLevelingResults: %s\n", e.what());
|
||
return false;
|
||
}
|
||
}
|
||
|
||
// 反初始化
|
||
void BagThreadPositionPresenter::DeinitApp()
|
||
{
|
||
LOG_DEBUG("Deinitializing BagThreadPositionPresenter\n");
|
||
|
||
// 关闭 ModbusTCP 客户端
|
||
ShutdownModbusClient();
|
||
|
||
// 停止检测
|
||
StopDetection();
|
||
|
||
// 释放ConfigManager（析构函数会自动调用Shutdown）
|
||
if (m_pConfigManager) {
|
||
delete m_pConfigManager;
|
||
m_pConfigManager = nullptr;
|
||
}
|
||
|
||
// 释放检测处理器
|
||
if (m_pDetectPresenter) {
|
||
delete m_pDetectPresenter;
|
||
m_pDetectPresenter = nullptr;
|
||
}
|
||
|
||
LOG_DEBUG("BagThreadPositionPresenter deinitialized\n");
|
||
}
|
||
|
||
// 触发检测
|
||
bool BagThreadPositionPresenter::TriggerDetection(int cameraIndex)
|
||
{
|
||
// 设置相机索引
|
||
if (cameraIndex > 0) {
|
||
SetDefaultCameraIndex(cameraIndex);
|
||
}
|
||
|
||
// 检查是否已连接相机
|
||
if (!m_bCameraConnected) {
|
||
LOG_WARNING("Camera not connected, cannot trigger detection\n");
|
||
return false;
|
||
}
|
||
|
||
// 触发检测
|
||
int ret = StartDetection(cameraIndex, false);
|
||
if (ret != SUCCESS) {
|
||
LOG_ERROR("Failed to trigger detection, error: %d\n", ret);
|
||
return false;
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
// 加载文件并检测
|
||
int BagThreadPositionPresenter::LoadAndDetect(const QString& fileName)
|
||
{
|
||
LOG_INFO("Loading data from file: %s\n", fileName.toStdString().c_str());
|
||
|
||
// 使用基类的方法加载调试数据并执行检测
|
||
return LoadDebugDataAndDetect(fileName.toStdString());
|
||
}
|
||
|
||
// 重连相机
|
||
void BagThreadPositionPresenter::ReconnectCamera()
|
||
{
|
||
LOG_INFO("Attempting to reconnect cameras\n");
|
||
TryReconnectCameras();
|
||
}
|
||
|
||
// 获取算法参数
|
||
BagThreadPositionPresenter::AlgoParams BagThreadPositionPresenter::GetAlgoParams() const
|
||
{
|
||
AlgoParams params;
|
||
|
||
if (m_pConfigManager) {
|
||
VrAlgorithmParams algorithmParams = m_pConfigManager->GetAlgorithmParams();
|
||
params.threadParam = algorithmParams.threadParam;
|
||
params.cornerParam = algorithmParams.cornerParam;
|
||
params.filterParam = algorithmParams.filterParam;
|
||
params.growParam = algorithmParams.growParam;
|
||
} else {
|
||
// 使用默认参数
|
||
params.threadParam.isHorizonScan = true;
|
||
}
|
||
|
||
return params;
|
||
}
|
||
|
||
// 设置算法参数
|
||
void BagThreadPositionPresenter::SetAlgoParams(const AlgoParams& params)
|
||
{
|
||
if (!m_pConfigManager) {
|
||
LOG_WARNING("ConfigManager not initialized, cannot set algorithm params\n");
|
||
return;
|
||
}
|
||
|
||
// 获取当前配置
|
||
VrAlgorithmParams algorithmParams = m_pConfigManager->GetAlgorithmParams();
|
||
|
||
// 更新参数
|
||
algorithmParams.threadParam = params.threadParam;
|
||
algorithmParams.cornerParam = params.cornerParam;
|
||
algorithmParams.filterParam = params.filterParam;
|
||
algorithmParams.growParam = params.growParam;
|
||
|
||
// 更新到ConfigManager
|
||
m_pConfigManager->UpdateAlgorithmParams(algorithmParams);
|
||
|
||
// 保存到配置文件
|
||
QString configPath = PathManager::GetInstance().GetConfigFilePath();
|
||
if (m_pConfigManager->SaveConfigToFile(configPath.toStdString())) {
|
||
LOG_INFO("Algorithm parameters updated and saved to config file\n");
|
||
} else {
|
||
LOG_ERROR("Failed to save algorithm parameters to config file\n");
|
||
}
|
||
}
|
||
|
||
// 重写BasePresenter虚函数：创建设备对象
|
||
// BagThreadPosition项目使用GlLineLaserDevice
|
||
int BagThreadPositionPresenter::CreateDevice(IVrEyeDevice** ppDevice)
|
||
{
|
||
if (!ppDevice) {
|
||
return ERR_CODE(DEV_ARG_INVAILD);
|
||
}
|
||
|
||
// 使用IGlLineLaserDevice工厂方法创建GlLineLaserDevice
|
||
IGlLineLaserDevice* pGlDevice = nullptr;
|
||
int nRet = IGlLineLaserDevice::CreateGlLineLaserObject(&pGlDevice);
|
||
if (nRet == SUCCESS && pGlDevice) {
|
||
*ppDevice = pGlDevice;
|
||
LOG_INFO("[BagThreadPositionPresenter] Created GlLineLaser device\n");
|
||
return SUCCESS;
|
||
}
|
||
|
||
LOG_ERROR("[BagThreadPositionPresenter] Failed to create GlLineLaser device, error: %d\n", nRet);
|
||
return ERR_CODE(DEV_OPEN_ERR);
|
||
}
|
||
|
||
// 应用基准距离到指定相机设备
|
||
void BagThreadPositionPresenter::ApplyBaseDistance(int cameraIndex, double baseDistance)
|
||
{
|
||
int arrayIndex = cameraIndex - 1;
|
||
if (arrayIndex < 0 || arrayIndex >= static_cast<int>(m_vrEyeDeviceList.size())) {
|
||
LOG_WARNING("[BagThreadPositionPresenter] ApplyBaseDistance: invalid cameraIndex %d\n", cameraIndex);
|
||
return;
|
||
}
|
||
|
||
IVrEyeDevice* pDevice = m_vrEyeDeviceList[arrayIndex].second;
|
||
IGlLineLaserDevice* pGlDevice = dynamic_cast<IGlLineLaserDevice*>(pDevice);
|
||
if (pGlDevice) {
|
||
int ret = pGlDevice->SetBaseDistance(baseDistance);
|
||
if (ret == SUCCESS) {
|
||
LOG_INFO("[BagThreadPositionPresenter] 相机%d基准距离已设置: %.1f mm\n", cameraIndex, baseDistance);
|
||
} else {
|
||
LOG_ERROR("[BagThreadPositionPresenter] 相机%d设置基准距离失败: %d\n", cameraIndex, ret);
|
||
}
|
||
} else {
|
||
LOG_WARNING("[BagThreadPositionPresenter] 相机%d不是GlLineLaser设备，无法设置基准距离\n", cameraIndex);
|
||
}
|
||
}
|
||
|
||
// 应用ModbusTCP配置（IP和大小端）
|
||
void BagThreadPositionPresenter::ApplyModbusConfig(const std::string& ip, bool bigEndian)
|
||
{
|
||
m_modbusBigEndian = bigEndian;
|
||
LOG_INFO("[BagThreadPositionPresenter] ModbusTCP大小端已更新: %s\n", bigEndian ? "大端" : "小端");
|
||
|
||
// 如果IP发生变化，重启ModbusTCP客户端
|
||
if (ip != m_modbusClientIP) {
|
||
LOG_INFO("[BagThreadPositionPresenter] ModbusTCP IP变更: %s -> %s，重启客户端\n",
|
||
m_modbusClientIP.c_str(), ip.c_str());
|
||
ShutdownModbusClient();
|
||
m_modbusShutdownRequested = false;
|
||
InitModbusClient(ip, m_modbusClientPort, m_modbusPollingInterval);
|
||
}
|
||
}
|
||
|
||
// ============ 面标定辅助函数 ============
|
||
|
||
SSG_planeCalibPara BagThreadPositionPresenter::GetBaseCalibPara(
|
||
const std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& scanData)
|
||
{
|
||
SSG_planeCalibPara calibPara;
|
||
|
||
// 初始化为单位矩阵
|
||
double identityMatrix[9] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
|
||
for (int i = 0; i < 9; i++) {
|
||
calibPara.planeCalib[i] = identityMatrix[i];
|
||
calibPara.invRMatrix[i] = identityMatrix[i];
|
||
}
|
||
calibPara.planeHeight = 2600.0;
|
||
|
||
// 转换为算法需要的XYZ格式
|
||
std::vector<std::vector<SVzNL3DPosition>> xyzData;
|
||
int convertResult = m_dataLoader.ConvertToSVzNL3DPosition(scanData, xyzData);
|
||
if (convertResult != SUCCESS || xyzData.empty()) {
|
||
LOG_WARNING("Failed to convert data to XYZ format for calibration\n");
|
||
return calibPara;
|
||
}
|
||
|
||
// 调用算法获取基础标定参数
|
||
calibPara = wd_bagThread_getBaseCalibPara(xyzData);
|
||
|
||
LOG_INFO("Base calibration parameters obtained: planeHeight=%.3f\n", calibPara.planeHeight);
|
||
|
||
return calibPara;
|
||
}
|
||
|
||
// ============ ModbusTCP Client 实现 ============
|
||
|
||
void BagThreadPositionPresenter::InitModbusClient(const std::string& ip, uint16_t port, int pollingInterval)
|
||
{
|
||
m_modbusClientIP = ip;
|
||
m_modbusClientPort = port;
|
||
m_modbusPollingInterval = pollingInterval;
|
||
m_modbusShutdownRequested = false;
|
||
m_modbusReconnectAttempts = 0;
|
||
m_lastScanRequestState = false;
|
||
m_lastModbusConnectedState = false;
|
||
|
||
LOG_INFO("[ModbusTCP Client] 初始化: IP=%s, 端口=%d, 轮询间隔=%dms\n",
|
||
ip.c_str(), port, pollingInterval);
|
||
|
||
// 创建客户端并尝试连接
|
||
bool connected = false;
|
||
if (!ip.empty()) {
|
||
std::lock_guard<std::mutex> lock(m_modbusClientMutex);
|
||
|
||
if (m_modbusClient) {
|
||
m_modbusClient->Disconnect();
|
||
delete m_modbusClient;
|
||
m_modbusClient = nullptr;
|
||
}
|
||
|
||
if (!IYModbusTCPClient::CreateInstance(&m_modbusClient, ip, port)) {
|
||
LOG_ERROR("[ModbusTCP Client] 创建客户端实例失败\n");
|
||
} else {
|
||
m_modbusClient->SetTimeout(1000);
|
||
auto result = m_modbusClient->Connect();
|
||
if (result != IYModbusTCPClient::SUCCESS) {
|
||
LOG_WARNING("[ModbusTCP Client] 连接 %s:%d 失败: %s\n",
|
||
ip.c_str(), port, m_modbusClient->GetLastError().c_str());
|
||
} else {
|
||
LOG_INFO("[ModbusTCP Client] 成功连接到 %s:%d\n", ip.c_str(), port);
|
||
connected = true;
|
||
}
|
||
}
|
||
}
|
||
|
||
m_lastModbusConnectedState = connected;
|
||
OnModbusServerStatusChanged(connected);
|
||
|
||
// 启动轮询线程
|
||
StartModbusPolling();
|
||
}
|
||
|
||
void BagThreadPositionPresenter::ShutdownModbusClient()
|
||
{
|
||
// 防止重复调用
|
||
bool expected = false;
|
||
if (!m_modbusShutdownRequested.compare_exchange_strong(expected, true)) {
|
||
return;
|
||
}
|
||
|
||
LOG_INFO("[ModbusTCP Client] 正在关闭...\n");
|
||
|
||
// 停止轮询线程
|
||
StopModbusPolling();
|
||
|
||
// 清理客户端
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_modbusClientMutex);
|
||
if (m_modbusClient) {
|
||
m_modbusClient->Disconnect();
|
||
delete m_modbusClient;
|
||
m_modbusClient = nullptr;
|
||
}
|
||
}
|
||
|
||
LOG_INFO("[ModbusTCP Client] 已关闭\n");
|
||
}
|
||
|
||
void BagThreadPositionPresenter::StartModbusPolling()
|
||
{
|
||
if (m_modbusShutdownRequested) {
|
||
LOG_WARNING("[ModbusTCP Client] 无法启动轮询：已请求关闭\n");
|
||
return;
|
||
}
|
||
|
||
if (m_modbusPollingRunning) {
|
||
LOG_WARNING("[ModbusTCP Client] 轮询已在运行\n");
|
||
return;
|
||
}
|
||
|
||
m_modbusPollingRunning = true;
|
||
m_modbusPollingThread = std::thread(&BagThreadPositionPresenter::ModbusPollingThreadFunc, this);
|
||
LOG_INFO("[ModbusTCP Client] 轮询线程已启动，间隔: %d ms\n", m_modbusPollingInterval);
|
||
}
|
||
|
||
void BagThreadPositionPresenter::StopModbusPolling()
|
||
{
|
||
if (!m_modbusPollingRunning) {
|
||
return;
|
||
}
|
||
|
||
m_modbusPollingRunning = false;
|
||
if (m_modbusPollingThread.joinable()) {
|
||
m_modbusPollingThread.join();
|
||
}
|
||
LOG_INFO("[ModbusTCP Client] 轮询线程已停止\n");
|
||
}
|
||
|
||
void BagThreadPositionPresenter::ModbusPollingThreadFunc()
|
||
{
|
||
LOG_INFO("[ModbusTCP Client] 轮询线程启动\n");
|
||
|
||
int reconnectCounter = 0;
|
||
const int reconnectInterval = 2000; // 重连间隔 2秒
|
||
|
||
while (m_modbusPollingRunning && !m_modbusShutdownRequested) {
|
||
// 检查连接状态
|
||
bool currentConnected = CheckModbusConnection();
|
||
|
||
// 连接状态变化时通知
|
||
if (currentConnected != m_lastModbusConnectedState) {
|
||
m_lastModbusConnectedState = currentConnected;
|
||
OnModbusServerStatusChanged(currentConnected);
|
||
|
||
// 重连成功后重置状态
|
||
if (currentConnected) {
|
||
m_lastScanRequestState = false;
|
||
m_modbusReconnectAttempts = 0;
|
||
m_modbusReadFailCount = 0;
|
||
reconnectCounter = 0;
|
||
LOG_INFO("[ModbusTCP Client] 重连成功\n");
|
||
}
|
||
}
|
||
|
||
if (!currentConnected) {
|
||
// 未连接时，尝试重连
|
||
reconnectCounter++;
|
||
int reconnectThreshold = (m_modbusPollingInterval > 0) ?
|
||
(reconnectInterval / m_modbusPollingInterval) : 10;
|
||
if (reconnectCounter >= reconnectThreshold) {
|
||
reconnectCounter = 0;
|
||
m_modbusReconnectAttempts++;
|
||
LOG_INFO("[ModbusTCP Client] 尝试重连（第%d次）...\n", m_modbusReconnectAttempts);
|
||
TryConnectModbus();
|
||
}
|
||
std::this_thread::sleep_for(std::chrono::milliseconds(m_modbusPollingInterval));
|
||
continue;
|
||
}
|
||
|
||
// 已连接但暂停轮询（检测期间）
|
||
if (m_modbusPollingPaused) {
|
||
std::this_thread::sleep_for(std::chrono::milliseconds(m_modbusPollingInterval));
|
||
continue;
|
||
}
|
||
|
||
// 读取扫描请求寄存器 reg[0]
|
||
int requestValue = ReadScanRequest();
|
||
if (requestValue >= 0) {
|
||
// 读取成功，重置失败计数
|
||
m_modbusReadFailCount = 0;
|
||
|
||
// 电平检测：reg[0]==1 即触发扫描
|
||
if (requestValue == 1) {
|
||
LOG_INFO("[ModbusTCP Client] 检测到扫描请求（reg[0]=1）\n");
|
||
|
||
// 暂停轮询（检测期间不再读取请求）
|
||
m_modbusPollingPaused = true;
|
||
|
||
// 清零 reg[0]
|
||
WriteRemoteSingleRegister(0, 0);
|
||
|
||
// 清零 reg[2]（检测状态）
|
||
WriteRemoteSingleRegister(2, 0);
|
||
|
||
// 触发检测
|
||
bool success = TriggerDetection(m_currentCameraIndex);
|
||
if (!success) {
|
||
LOG_ERROR("[ModbusTCP Client] 触发检测失败\n");
|
||
WriteRemoteSingleRegister(2, 2); // 写入失败状态
|
||
m_modbusPollingPaused = false; // 恢复轮询
|
||
}
|
||
}
|
||
} else {
|
||
// 读取失败，累计失败次数
|
||
m_modbusReadFailCount++;
|
||
LOG_WARNING("[ModbusTCP Client] 读取扫描请求失败（连续第%d次）\n", m_modbusReadFailCount);
|
||
|
||
if (m_modbusReadFailCount >= MAX_READ_FAIL_COUNT) {
|
||
// 连续多次失败，断开连接以触发重连
|
||
LOG_WARNING("[ModbusTCP Client] 连续%d次读取失败，断开连接以触发重连\n", m_modbusReadFailCount);
|
||
m_modbusReadFailCount = 0;
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_modbusClientMutex);
|
||
if (m_modbusClient) {
|
||
m_modbusClient->Disconnect();
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
std::this_thread::sleep_for(std::chrono::milliseconds(m_modbusPollingInterval));
|
||
}
|
||
|
||
LOG_INFO("[ModbusTCP Client] 轮询线程退出\n");
|
||
}
|
||
|
||
bool BagThreadPositionPresenter::CheckModbusConnection()
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_modbusClientMutex);
|
||
return m_modbusClient && m_modbusClient->IsConnected();
|
||
}
|
||
|
||
bool BagThreadPositionPresenter::TryConnectModbus()
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_modbusClientMutex);
|
||
|
||
// 销毁旧实例，重新创建以确保 libmodbus 底层 socket 状态干净
|
||
if (m_modbusClient) {
|
||
m_modbusClient->Disconnect();
|
||
delete m_modbusClient;
|
||
m_modbusClient = nullptr;
|
||
}
|
||
|
||
if (!IYModbusTCPClient::CreateInstance(&m_modbusClient, m_modbusClientIP, m_modbusClientPort)) {
|
||
LOG_ERROR("[ModbusTCP Client] 创建客户端实例失败\n");
|
||
return false;
|
||
}
|
||
m_modbusClient->SetTimeout(1000);
|
||
|
||
auto result = m_modbusClient->Connect();
|
||
if (result != IYModbusTCPClient::SUCCESS) {
|
||
LOG_WARNING("[ModbusTCP Client] 连接 %s:%d 失败: %s\n",
|
||
m_modbusClientIP.c_str(), m_modbusClientPort,
|
||
m_modbusClient->GetLastError().c_str());
|
||
return false;
|
||
}
|
||
|
||
LOG_INFO("[ModbusTCP Client] 已连接到 %s:%d\n", m_modbusClientIP.c_str(), m_modbusClientPort);
|
||
return true;
|
||
}
|
||
|
||
int BagThreadPositionPresenter::ReadScanRequest()
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_modbusClientMutex);
|
||
|
||
if (!m_modbusClient || !m_modbusClient->IsConnected()) {
|
||
return -1;
|
||
}
|
||
|
||
std::vector<uint16_t> values;
|
||
auto result = m_modbusClient->ReadHoldingRegisters(0, 1, values);
|
||
|
||
if (result != IYModbusTCPClient::SUCCESS || values.empty()) {
|
||
return -1;
|
||
}
|
||
|
||
return static_cast<int>(values[0]);
|
||
}
|
||
|
||
bool BagThreadPositionPresenter::WriteRemoteRegisters(uint16_t startAddress, const std::vector<uint16_t>& values)
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_modbusClientMutex);
|
||
|
||
if (!m_modbusClient || !m_modbusClient->IsConnected()) {
|
||
LOG_WARNING("[ModbusTCP Client] 未连接，无法写入寄存器\n");
|
||
return false;
|
||
}
|
||
|
||
auto result = m_modbusClient->WriteMultipleRegisters(startAddress, values);
|
||
if (result != IYModbusTCPClient::SUCCESS) {
|
||
LOG_ERROR("[ModbusTCP Client] 写入寄存器失败（地址=%d，数量=%zu）: %s\n",
|
||
startAddress, values.size(), m_modbusClient->GetLastError().c_str());
|
||
return false;
|
||
}
|
||
|
||
LOG_DEBUG("[ModbusTCP Client] 写入寄存器成功：地址=%d，数量=%zu\n", startAddress, values.size());
|
||
return true;
|
||
}
|
||
|
||
bool BagThreadPositionPresenter::WriteRemoteSingleRegister(uint16_t address, uint16_t value)
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_modbusClientMutex);
|
||
|
||
if (!m_modbusClient || !m_modbusClient->IsConnected()) {
|
||
LOG_WARNING("[ModbusTCP Client] 未连接，无法写入寄存器\n");
|
||
return false;
|
||
}
|
||
|
||
auto result = m_modbusClient->WriteSingleRegister(address, value);
|
||
if (result != IYModbusTCPClient::SUCCESS) {
|
||
LOG_ERROR("[ModbusTCP Client] 写入单寄存器失败（地址=%d）: %s\n",
|
||
address, m_modbusClient->GetLastError().c_str());
|
||
return false;
|
||
}
|
||
|
||
LOG_DEBUG("[ModbusTCP Client] 写入单寄存器成功：地址=%d，值=%d\n", address, value);
|
||
return true;
|
||
}
|
||
|
||
bool BagThreadPositionPresenter::IsModbusClientConnected() const
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_modbusClientMutex);
|
||
return m_modbusClient && m_modbusClient->IsConnected();
|
||
}
|
||
|
||
// ============ 辅助函数 ============
|
||
|
||
/**
|
||
* @brief 将 float 转换为寄存器数据（支持大小端切换）
|
||
* @param value float 值
|
||
* @param regs 输出的寄存器数组（2个uint16_t）
|
||
* @param bigEndian true=大端，false=小端
|
||
*/
|
||
void BagThreadPositionPresenter::_FloatToRegisters(float value, uint16_t* regs, bool bigEndian)
|
||
{
|
||
// 将 float 转换为 4 字节
|
||
uint32_t intValue;
|
||
memcpy(&intValue, &value, sizeof(float));
|
||
|
||
if (bigEndian) {
|
||
// 大端：高位在前
|
||
regs[0] = static_cast<uint16_t>((intValue >> 16) & 0xFFFF); // 高16位
|
||
regs[1] = static_cast<uint16_t>(intValue & 0xFFFF); // 低16位
|
||
} else {
|
||
// 小端：低位在前
|
||
regs[0] = static_cast<uint16_t>(intValue & 0xFFFF); // 低16位
|
||
regs[1] = static_cast<uint16_t>((intValue >> 16) & 0xFFFF); // 高16位
|
||
}
|
||
}
|