509 lines
16 KiB
C++
509 lines
16 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);
|
|
LOG_INFO("Attempting to reconnect PLC (attempt %d)...\n", 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->1 变化时触发拍照
|
|
bool currentState = (requestValue == 1);
|
|
if (currentState && !m_lastPhotoRequestState) {
|
|
LOG_INFO("Photo request detected (D1000=1)\n");
|
|
notifyPhotoRequested(1);
|
|
}
|
|
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));
|
|
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.pitch, registers[6], registers[7]);
|
|
floatToRegisters(coord.roll, 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, P=%.3f, R=%.3f, Y=%.3f\n",
|
|
pointIndex, startAddr, coord.x, coord.y, coord.z, coord.pitch, coord.roll, coord.yaw);
|
|
return true;
|
|
}
|
|
|
|
bool PLCModbusClient::SendCoordinatesToPLC(const std::vector<CoordinateData>& coords)
|
|
{
|
|
std::lock_guard<std::mutex> locker(m_mutex);
|
|
|
|
if (!m_plcClient || !m_plcClient->IsConnected()) {
|
|
LOG_ERROR("PLC not connected, cannot send coordinates\n");
|
|
return false;
|
|
}
|
|
|
|
int pointCount = std::min(static_cast<int>(coords.size()), MAX_POINTS);
|
|
if (pointCount == 0) {
|
|
LOG_WARNING("No coordinates to send\n");
|
|
return true;
|
|
}
|
|
|
|
std::vector<uint16_t> registers(pointCount * REGS_PER_POINT);
|
|
|
|
for (int i = 0; i < pointCount; i++) {
|
|
const CoordinateData& coord = coords[i];
|
|
int offset = i * REGS_PER_POINT;
|
|
floatToRegisters(coord.x, registers[offset + 0], registers[offset + 1]);
|
|
floatToRegisters(coord.y, registers[offset + 2], registers[offset + 3]);
|
|
floatToRegisters(coord.z, registers[offset + 4], registers[offset + 5]);
|
|
floatToRegisters(coord.roll, registers[offset + 6], registers[offset + 7]);
|
|
floatToRegisters(coord.pitch, registers[offset + 8], registers[offset + 9]);
|
|
floatToRegisters(coord.yaw, registers[offset + 10], registers[offset + 11]);
|
|
}
|
|
|
|
auto result = m_plcClient->WriteMultipleRegisters(m_registerConfig.addrCoordDataStart, registers);
|
|
if (result != IYModbusTCPClient::SUCCESS) {
|
|
LOG_ERROR("Failed to write coordinates to PLC (addr %d): %s\n",
|
|
m_registerConfig.addrCoordDataStart, m_plcClient->GetLastError().c_str());
|
|
return false;
|
|
}
|
|
|
|
LOG_INFO("Sent %d points to PLC (addr %d-%d)\n",
|
|
pointCount, m_registerConfig.addrCoordDataStart,
|
|
m_registerConfig.addrCoordDataStart + pointCount * REGS_PER_POINT - 1);
|
|
return true;
|
|
}
|
|
|
|
bool PLCModbusClient::NotifyDataComplete()
|
|
{
|
|
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, 1);
|
|
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 = 1)\n", m_registerConfig.addrDataComplete);
|
|
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);
|
|
}
|
|
}
|