96 lines
2.6 KiB
C++
96 lines
2.6 KiB
C++
#include "ModbusServiceWorker.h"
|
|
#include "libdiff.h"
|
|
#include "globals.h"
|
|
|
|
#include <iostream>
|
|
|
|
ModbusServiceWorker::ModbusServiceWorker(int socket, LogFile *log)
|
|
: socket(socket), log(log)
|
|
{
|
|
s.Init(socket);
|
|
}
|
|
|
|
void ModbusServiceWorker::Run()
|
|
{
|
|
log->LogString("Started ModbusServiceWorker");
|
|
try {
|
|
while (true)
|
|
{
|
|
string header = s.Receive(7);
|
|
string transaction_id = header.substr(0, 2);
|
|
string protocol_id = header.substr(2, 2);
|
|
int length = ((int)(unsigned char)header[5]) | (((int)(unsigned char)header[4]) << 8);
|
|
string unit_identifier = header.substr(6, 1);
|
|
string data = s.Receive(length - 1);
|
|
string answer_data = "";
|
|
int function = data[0];
|
|
if (function == 3)
|
|
{
|
|
LockMutex(register_mutex);
|
|
int address = GetInt(data.substr(1, 2));
|
|
int amount = GetInt(data.substr(3, 2));
|
|
string result = "";
|
|
for (int a = address; a < address + amount; a++)
|
|
{
|
|
int val = 0;
|
|
if (a >= 0 && a <= 65535) val = registers[a];
|
|
result += (char)((val >> 8) & 0xFF);
|
|
result += (char)(val & 0xFF);
|
|
}
|
|
ReleaseMutex(register_mutex);
|
|
answer_data += (char)3;
|
|
answer_data += (char)result.length();
|
|
answer_data += result;
|
|
}
|
|
else if (function == 6)
|
|
{
|
|
LockMutex(register_mutex);
|
|
int address = GetInt(data.substr(1, 2));
|
|
int value = GetInt(data.substr(3, 2));
|
|
if (address >= 0 && address <= 65535)
|
|
{
|
|
registers[address] = value;
|
|
}
|
|
ReleaseMutex(register_mutex);
|
|
answer_data = data;
|
|
}
|
|
else throw (string)("Unknown function " + ToString(function));
|
|
answer_data = unit_identifier + answer_data;
|
|
string length_str = "";
|
|
length_str += (char)((answer_data.length() >> 8) & 0xFF);
|
|
length_str += (char)((answer_data.length()) & 0xFF);
|
|
s.Send(transaction_id + protocol_id + length_str + answer_data);
|
|
}
|
|
}
|
|
catch (string s)
|
|
{
|
|
log->LogString("Exception in ModbusServiceWorker: " + s);
|
|
}
|
|
catch (std::exception e)
|
|
{
|
|
log->LogString("Exception in ModbusServiceWorker: " + ToString(e.what()));
|
|
}
|
|
catch (...)
|
|
{
|
|
log->LogString("Unknown exception in ModbusServiceWorker");
|
|
// непонятно, что случилось, закрываем сокет и все дела
|
|
}
|
|
s.Disconnect();
|
|
}
|
|
|
|
int ModbusServiceWorker::GetInt(const string &s)
|
|
{
|
|
int res = 0;
|
|
for (int i = 0; i < (int)s.length(); i++)
|
|
{
|
|
res <<= 8;
|
|
res += (int)(unsigned char)s[i];
|
|
}
|
|
return res;
|
|
}
|
|
|
|
ModbusServiceWorker::~ModbusServiceWorker()
|
|
{
|
|
s.Disconnect();
|
|
}
|