477 lines
14 KiB
C++
477 lines
14 KiB
C++
#include "PLCModbusClient.h"
|
||
#include "VrLog.h"
|
||
#include <cstring>
|
||
#include <chrono>
|
||
|
||
PLCModbusClient::PLCModbusClient()
|
||
: m_plcClient(nullptr)
|
||
, m_plcPort(502)
|
||
, m_lastPhotoRequestState(false)
|
||
, m_lastConnectedState(false)
|
||
, m_pollRunning(false)
|
||
, m_photoRequestPaused(false)
|
||
, m_pollIntervalMs(1000)
|
||
, m_shutdownRequested(false)
|
||
, m_autoReconnect(true)
|
||
, m_reconnectInterval(2000)
|
||
, m_plcReconnectAttempts(0)
|
||
{
|
||
}
|
||
|
||
PLCModbusClient::~PLCModbusClient()
|
||
{
|
||
Shutdown();
|
||
}
|
||
|
||
bool PLCModbusClient::Initialize(const std::string& plcIP, int plcPort)
|
||
{
|
||
RegisterConfig regConfig;
|
||
return Initialize(plcIP, plcPort, regConfig);
|
||
}
|
||
|
||
bool PLCModbusClient::Initialize(const std::string& plcIP, int plcPort, const RegisterConfig& regConfig)
|
||
{
|
||
// 如果已经在运行，先停止
|
||
if (m_pollRunning) {
|
||
StopPolling();
|
||
}
|
||
|
||
m_plcIP = plcIP;
|
||
m_plcPort = plcPort;
|
||
m_registerConfig = regConfig;
|
||
m_shutdownRequested = false;
|
||
m_plcReconnectAttempts = 0;
|
||
m_lastPhotoRequestState = false;
|
||
m_lastConnectedState = false;
|
||
|
||
LOG_INFO("Link PLC ip : %s, port : %d\n", plcIP.c_str(), plcPort);
|
||
LOG_INFO("PLCModbusClient Initialize: PhotoRequest=%d, DataComplete=%d, CoordStart=%d\n",
|
||
m_registerConfig.addrPhotoRequest, m_registerConfig.addrDataComplete,
|
||
m_registerConfig.addrCoordDataStart);
|
||
|
||
bool connected = false;
|
||
bool createFailed = false;
|
||
|
||
// 创建 PLC 客户端并连接
|
||
if (!plcIP.empty()) {
|
||
std::lock_guard<std::mutex> locker(m_mutex);
|
||
|
||
// 清理旧的客户端
|
||
if (m_plcClient) {
|
||
m_plcClient->Disconnect();
|
||
delete m_plcClient;
|
||
m_plcClient = nullptr;
|
||
}
|
||
|
||
if (!IYModbusTCPClient::CreateInstance(&m_plcClient, plcIP, plcPort)) {
|
||
LOG_ERROR("Failed to create PLC Modbus client\n");
|
||
createFailed = true;
|
||
} else {
|
||
m_plcClient->SetTimeout(1000);
|
||
auto result = m_plcClient->Connect();
|
||
if (result != IYModbusTCPClient::SUCCESS) {
|
||
LOG_WARNING("Failed to connect to PLC at %s:%d, error: %s\n",
|
||
m_plcIP.c_str(), m_plcPort, m_plcClient->GetLastError().c_str());
|
||
} else {
|
||
LOG_INFO("Connected to PLC at %s:%d\n", m_plcIP.c_str(), m_plcPort);
|
||
connected = true;
|
||
}
|
||
}
|
||
}
|
||
|
||
// 在锁外调用回调，避免死锁
|
||
if (createFailed) {
|
||
notifyError("PLC客户端创建失败");
|
||
}
|
||
|
||
m_lastConnectedState = connected;
|
||
notifyConnectionStateChanged(connected);
|
||
|
||
return true;
|
||
}
|
||
|
||
void PLCModbusClient::Shutdown()
|
||
{
|
||
// 防止重复调用
|
||
bool expected = false;
|
||
if (!m_shutdownRequested.compare_exchange_strong(expected, true)) {
|
||
return;
|
||
}
|
||
|
||
LOG_INFO("PLCModbusClient shutdown requested\n");
|
||
|
||
// 停止轮询线程
|
||
StopPolling();
|
||
|
||
// 清理 PLC 客户端
|
||
{
|
||
std::lock_guard<std::mutex> locker(m_mutex);
|
||
if (m_plcClient) {
|
||
m_plcClient->Disconnect();
|
||
delete m_plcClient;
|
||
m_plcClient = nullptr;
|
||
}
|
||
}
|
||
|
||
LOG_INFO("PLCModbusClient shutdown complete\n");
|
||
}
|
||
|
||
void PLCModbusClient::StartPolling(int intervalMs)
|
||
{
|
||
if (m_shutdownRequested) {
|
||
LOG_WARNING("Cannot start polling: shutdown requested\n");
|
||
return;
|
||
}
|
||
|
||
if (m_pollRunning) {
|
||
LOG_WARNING("Polling already running\n");
|
||
return;
|
||
}
|
||
|
||
m_pollIntervalMs = intervalMs;
|
||
m_pollRunning = true;
|
||
m_pollThread = std::thread(&PLCModbusClient::pollThreadFunc, this);
|
||
LOG_INFO("PLC polling started, interval: %d ms\n", intervalMs);
|
||
}
|
||
|
||
void PLCModbusClient::StopPolling()
|
||
{
|
||
if (!m_pollRunning) {
|
||
return;
|
||
}
|
||
|
||
m_pollRunning = false;
|
||
if (m_pollThread.joinable()) {
|
||
m_pollThread.join();
|
||
}
|
||
LOG_INFO("PLC polling stopped\n");
|
||
}
|
||
|
||
void PLCModbusClient::SetPhotoTriggerCallback(PhotoTriggerCallback callback)
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_callbackMutex);
|
||
m_photoCallback = callback;
|
||
}
|
||
|
||
void PLCModbusClient::SetConnectionStateCallback(ConnectionStateCallback callback)
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_callbackMutex);
|
||
m_connectionStateCallback = callback;
|
||
}
|
||
|
||
void PLCModbusClient::SetErrorCallback(ErrorCallback callback)
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_callbackMutex);
|
||
m_errorCallback = callback;
|
||
}
|
||
|
||
void PLCModbusClient::SetReconnectingCallback(ReconnectingCallback callback)
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_callbackMutex);
|
||
m_reconnectingCallback = callback;
|
||
}
|
||
|
||
void PLCModbusClient::SetReconnectInterval(int intervalMs)
|
||
{
|
||
m_reconnectInterval = intervalMs;
|
||
}
|
||
|
||
void PLCModbusClient::SetAutoReconnect(bool enable)
|
||
{
|
||
m_autoReconnect = enable;
|
||
}
|
||
|
||
void PLCModbusClient::pollThreadFunc()
|
||
{
|
||
LOG_INFO("Poll thread started\n");
|
||
|
||
int reconnectCounter = 0;
|
||
|
||
while (m_pollRunning && !m_shutdownRequested) {
|
||
// 检查连接状态
|
||
bool currentConnected = checkConnection();
|
||
|
||
// 连接状态变化时通知
|
||
if (currentConnected != m_lastConnectedState) {
|
||
m_lastConnectedState = currentConnected;
|
||
notifyConnectionStateChanged(currentConnected);
|
||
|
||
// 重连成功后重置状态
|
||
if (currentConnected) {
|
||
m_lastPhotoRequestState = false;
|
||
m_plcReconnectAttempts = 0;
|
||
reconnectCounter = 0;
|
||
LOG_INFO("PLC reconnected successfully\n");
|
||
}
|
||
}
|
||
|
||
if (!currentConnected) {
|
||
// 未连接时，尝试重连
|
||
if (m_autoReconnect) {
|
||
reconnectCounter++;
|
||
int reconnectThreshold = (m_pollIntervalMs > 0) ? (m_reconnectInterval / m_pollIntervalMs) : 30;
|
||
if (reconnectCounter >= reconnectThreshold) {
|
||
reconnectCounter = 0;
|
||
m_plcReconnectAttempts++;
|
||
|
||
notifyReconnecting("PLC", m_plcReconnectAttempts);
|
||
tryConnectPLC();
|
||
}
|
||
}
|
||
std::this_thread::sleep_for(std::chrono::milliseconds(m_pollIntervalMs));
|
||
continue;
|
||
}
|
||
|
||
// 已连接，检查是否暂停了拍照请求轮询
|
||
if (m_photoRequestPaused) {
|
||
// 暂停期间不读取拍照请求，只维持连接
|
||
std::this_thread::sleep_for(std::chrono::milliseconds(m_pollIntervalMs));
|
||
continue;
|
||
}
|
||
|
||
// 读取拍照请求寄存器
|
||
int requestValue = readPhotoRequest();
|
||
if (requestValue >= 0) {
|
||
// 边沿检测：值从 0 变为非零时触发拍照
|
||
// D1001 值含义：1=第一个相机检测，2=第二个相机检测
|
||
bool currentState = (requestValue > 0);
|
||
if (currentState && !m_lastPhotoRequestState) {
|
||
LOG_INFO("Photo request detected (D1000=%d, camera %d)\n", requestValue, requestValue);
|
||
notifyPhotoRequested(requestValue);
|
||
}
|
||
m_lastPhotoRequestState = currentState;
|
||
} else {
|
||
// 读取失败，可能是连接断开，主动断开连接以触发重连
|
||
LOG_WARNING("Failed to read photo request, disconnecting to trigger reconnect\n");
|
||
disconnectPLC();
|
||
}
|
||
|
||
std::this_thread::sleep_for(std::chrono::milliseconds(m_pollIntervalMs));
|
||
}
|
||
|
||
LOG_INFO("Poll thread exited\n");
|
||
}
|
||
|
||
bool PLCModbusClient::checkConnection()
|
||
{
|
||
std::lock_guard<std::mutex> locker(m_mutex);
|
||
return m_plcClient && m_plcClient->IsConnected();
|
||
}
|
||
|
||
bool PLCModbusClient::tryConnectPLC()
|
||
{
|
||
std::lock_guard<std::mutex> locker(m_mutex);
|
||
|
||
if (!m_plcClient) {
|
||
if (!IYModbusTCPClient::CreateInstance(&m_plcClient, m_plcIP, m_plcPort)) {
|
||
LOG_ERROR("Failed to create PLC Modbus client\n");
|
||
return false;
|
||
}
|
||
m_plcClient->SetTimeout(1000);
|
||
}
|
||
|
||
auto result = m_plcClient->Connect();
|
||
if (result != IYModbusTCPClient::SUCCESS) {
|
||
LOG_WARNING("Failed to connect to PLC at %s:%d, error: %s\n",
|
||
m_plcIP.c_str(), m_plcPort, m_plcClient->GetLastError().c_str());
|
||
return false;
|
||
}
|
||
|
||
LOG_INFO("Connected to PLC at %s:%d\n", m_plcIP.c_str(), m_plcPort);
|
||
return true;
|
||
}
|
||
|
||
void PLCModbusClient::disconnectPLC()
|
||
{
|
||
std::lock_guard<std::mutex> locker(m_mutex);
|
||
|
||
if (m_plcClient) {
|
||
m_plcClient->Disconnect();
|
||
LOG_INFO("PLC disconnected for reconnection\n");
|
||
}
|
||
}
|
||
|
||
int PLCModbusClient::readPhotoRequest()
|
||
{
|
||
std::lock_guard<std::mutex> locker(m_mutex);
|
||
|
||
if (!m_plcClient || !m_plcClient->IsConnected()) {
|
||
return -1;
|
||
}
|
||
|
||
std::vector<uint16_t> values;
|
||
auto result = m_plcClient->ReadHoldingRegisters(m_registerConfig.addrPhotoRequest, 1, values);
|
||
|
||
if (result != IYModbusTCPClient::SUCCESS || values.empty()) {
|
||
return -1;
|
||
}
|
||
|
||
return static_cast<int>(values[0]);
|
||
}
|
||
|
||
bool PLCModbusClient::ClearPhotoRequest()
|
||
{
|
||
std::lock_guard<std::mutex> locker(m_mutex);
|
||
|
||
if (!m_plcClient || !m_plcClient->IsConnected()) {
|
||
LOG_ERROR("PLC not connected, cannot clear photo request\n");
|
||
return false;
|
||
}
|
||
|
||
auto result = m_plcClient->WriteSingleRegister(m_registerConfig.addrPhotoRequest, 0);
|
||
if (result != IYModbusTCPClient::SUCCESS) {
|
||
LOG_ERROR("Failed to clear photo request (addr %d): %s\n",
|
||
m_registerConfig.addrPhotoRequest, m_plcClient->GetLastError().c_str());
|
||
return false;
|
||
}
|
||
|
||
LOG_INFO("Cleared photo request (addr %d = 0)\n", m_registerConfig.addrPhotoRequest);
|
||
return true;
|
||
}
|
||
|
||
void PLCModbusClient::floatToRegisters(float value, uint16_t& high, uint16_t& low)
|
||
{
|
||
uint32_t bits;
|
||
std::memcpy(&bits, &value, sizeof(float));
|
||
|
||
// 根据字节序配置决定高低位寄存器的值
|
||
if (m_registerConfig.byteOrder == LITTLE_ENDIAN_ORDER) {
|
||
// 小端序 (DCBA) - 低字节在前
|
||
low = static_cast<uint16_t>((bits >> 16) & 0xFFFF);
|
||
high = static_cast<uint16_t>(bits & 0xFFFF);
|
||
} else {
|
||
// 大端序 (ABCD) - 高字节在前（默认）
|
||
high = static_cast<uint16_t>((bits >> 16) & 0xFFFF);
|
||
low = static_cast<uint16_t>(bits & 0xFFFF);
|
||
}
|
||
}
|
||
|
||
bool PLCModbusClient::SendCoordinateToPLC(const CoordinateData& coord, int pointIndex)
|
||
{
|
||
std::lock_guard<std::mutex> locker(m_mutex);
|
||
|
||
if (!m_plcClient || !m_plcClient->IsConnected()) {
|
||
LOG_ERROR("PLC not connected, cannot send coordinate\n");
|
||
return false;
|
||
}
|
||
|
||
if (pointIndex < 0 || pointIndex >= MAX_POINTS) {
|
||
LOG_ERROR("Invalid point index: %d (valid: 0-%d)\n", pointIndex, MAX_POINTS - 1);
|
||
return false;
|
||
}
|
||
|
||
int startAddr = m_registerConfig.addrCoordDataStart + pointIndex * REGS_PER_POINT;
|
||
|
||
std::vector<uint16_t> registers(REGS_PER_POINT);
|
||
floatToRegisters(coord.x, registers[0], registers[1]);
|
||
floatToRegisters(coord.y, registers[2], registers[3]);
|
||
floatToRegisters(coord.z, registers[4], registers[5]);
|
||
floatToRegisters(coord.roll, registers[6], registers[7]);
|
||
floatToRegisters(coord.pitch, registers[8], registers[9]);
|
||
floatToRegisters(coord.yaw, registers[10], registers[11]);
|
||
|
||
auto result = m_plcClient->WriteMultipleRegisters(startAddr, registers);
|
||
if (result != IYModbusTCPClient::SUCCESS) {
|
||
LOG_ERROR("Failed to write coordinate to PLC (addr %d): %s\n",
|
||
startAddr, m_plcClient->GetLastError().c_str());
|
||
return false;
|
||
}
|
||
|
||
LOG_INFO("Sent point[%d] (addr %d): X=%.3f, Y=%.3f, Z=%.3f, R=%.3f, P=%.3f, Y=%.3f\n",
|
||
pointIndex, startAddr, coord.x, coord.y, coord.z, coord.roll, coord.pitch, coord.yaw);
|
||
return true;
|
||
}
|
||
|
||
bool PLCModbusClient::NotifyDataComplete(int statusCode)
|
||
{
|
||
std::lock_guard<std::mutex> locker(m_mutex);
|
||
|
||
if (!m_plcClient || !m_plcClient->IsConnected()) {
|
||
LOG_ERROR("PLC not connected, cannot notify data complete\n");
|
||
return false;
|
||
}
|
||
|
||
auto result = m_plcClient->WriteSingleRegister(m_registerConfig.addrDataComplete, static_cast<uint16_t>(statusCode));
|
||
if (result != IYModbusTCPClient::SUCCESS) {
|
||
LOG_ERROR("Failed to notify data complete (addr %d): %s\n",
|
||
m_registerConfig.addrDataComplete, m_plcClient->GetLastError().c_str());
|
||
return false;
|
||
}
|
||
|
||
LOG_INFO("Notified data complete (addr %d = %d)\n", m_registerConfig.addrDataComplete, statusCode);
|
||
return true;
|
||
}
|
||
|
||
bool PLCModbusClient::IsPLCConnected() const
|
||
{
|
||
std::lock_guard<std::mutex> locker(m_mutex);
|
||
return m_plcClient && m_plcClient->IsConnected();
|
||
}
|
||
|
||
void PLCModbusClient::PausePhotoRequestPolling()
|
||
{
|
||
m_photoRequestPaused = true;
|
||
LOG_INFO("Photo request polling paused\n");
|
||
}
|
||
|
||
void PLCModbusClient::ResumePhotoRequestPolling()
|
||
{
|
||
m_photoRequestPaused = false;
|
||
LOG_INFO("Photo request polling resumed\n");
|
||
}
|
||
|
||
bool PLCModbusClient::IsPhotoRequestPollingPaused() const
|
||
{
|
||
return m_photoRequestPaused;
|
||
}
|
||
|
||
// ========== 安全的回调通知（先复制回调，释放锁后再调用）==========
|
||
|
||
void PLCModbusClient::notifyConnectionStateChanged(bool connected)
|
||
{
|
||
ConnectionStateCallback callback;
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_callbackMutex);
|
||
callback = m_connectionStateCallback;
|
||
}
|
||
if (callback) {
|
||
callback(connected);
|
||
}
|
||
}
|
||
|
||
void PLCModbusClient::notifyError(const std::string& errorMsg)
|
||
{
|
||
ErrorCallback callback;
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_callbackMutex);
|
||
callback = m_errorCallback;
|
||
}
|
||
if (callback) {
|
||
callback(errorMsg);
|
||
}
|
||
}
|
||
|
||
void PLCModbusClient::notifyPhotoRequested(int cameraIndex)
|
||
{
|
||
PhotoTriggerCallback callback;
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_callbackMutex);
|
||
callback = m_photoCallback;
|
||
}
|
||
if (callback) {
|
||
callback(cameraIndex);
|
||
}
|
||
}
|
||
|
||
void PLCModbusClient::notifyReconnecting(const std::string& device, int attempt)
|
||
{
|
||
ReconnectingCallback callback;
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_callbackMutex);
|
||
callback = m_reconnectingCallback;
|
||
}
|
||
if (callback) {
|
||
callback(device, attempt);
|
||
}
|
||
}
|