mirror of
https://github.com/emsesp/EMS-ESP32.git
synced 2025-12-06 07:49:52 +03:00
tidy up TODOs
This commit is contained in:
@@ -7,210 +7,213 @@
|
|||||||
// #undef LOCAL_LOG_LEVEL
|
// #undef LOCAL_LOG_LEVEL
|
||||||
#include "Logging.h"
|
#include "Logging.h"
|
||||||
|
|
||||||
ModbusClientTCPasync::ModbusClientTCPasync(IPAddress address, uint16_t port, uint16_t queueLimit) :
|
ModbusClientTCPasync::ModbusClientTCPasync(IPAddress address, uint16_t port, uint16_t queueLimit)
|
||||||
ModbusClient(),
|
: ModbusClient()
|
||||||
txQueue(),
|
, txQueue()
|
||||||
rxQueue(),
|
, rxQueue()
|
||||||
MTA_client(),
|
, MTA_client()
|
||||||
MTA_timeout(DEFAULTTIMEOUT),
|
, MTA_timeout(DEFAULTTIMEOUT)
|
||||||
MTA_idleTimeout(DEFAULTIDLETIME),
|
, MTA_idleTimeout(DEFAULTIDLETIME)
|
||||||
MTA_qLimit(queueLimit),
|
, MTA_qLimit(queueLimit)
|
||||||
MTA_maxInflightRequests(queueLimit),
|
, MTA_maxInflightRequests(queueLimit)
|
||||||
MTA_lastActivity(0),
|
, MTA_lastActivity(0)
|
||||||
MTA_state(DISCONNECTED),
|
, MTA_state(DISCONNECTED)
|
||||||
MTA_host(address),
|
, MTA_host(address)
|
||||||
MTA_port(port)
|
, MTA_port(port) {
|
||||||
{
|
// attach all handlers on async tcp events
|
||||||
// attach all handlers on async tcp events
|
MTA_client.onConnect([](void * i, AsyncClient * c) { (static_cast<ModbusClientTCPasync *>(i))->onConnected(); }, this);
|
||||||
MTA_client.onConnect([](void* i, AsyncClient* c) { (static_cast<ModbusClientTCPasync*>(i))->onConnected(); }, this);
|
MTA_client.onDisconnect([](void * i, AsyncClient * c) { (static_cast<ModbusClientTCPasync *>(i))->onDisconnected(); }, this);
|
||||||
MTA_client.onDisconnect([](void* i, AsyncClient* c) { (static_cast<ModbusClientTCPasync*>(i))->onDisconnected(); }, this);
|
MTA_client.onError([](void * i, AsyncClient * c, int8_t error) { (static_cast<ModbusClientTCPasync *>(i))->onACError(c, error); }, this);
|
||||||
MTA_client.onError([](void* i, AsyncClient* c, int8_t error) { (static_cast<ModbusClientTCPasync*>(i))->onACError(c, error); }, this);
|
// MTA_client.onTimeout([](void* i, AsyncClient* c, uint32_t time) { (static_cast<ModbusClientTCPasync*>(i))->onTimeout(time); }, this);
|
||||||
// MTA_client.onTimeout([](void* i, AsyncClient* c, uint32_t time) { (static_cast<ModbusClientTCPasync*>(i))->onTimeout(time); }, this);
|
// MTA_client.onAck([](void* i, AsyncClient* c, size_t len, uint32_t time) { (static_cast<ModbusClientTCPasync*>(i))->onAck(len, time); }, this);
|
||||||
// MTA_client.onAck([](void* i, AsyncClient* c, size_t len, uint32_t time) { (static_cast<ModbusClientTCPasync*>(i))->onAck(len, time); }, this);
|
MTA_client.onData([](void * i,
|
||||||
MTA_client.onData([](void* i, AsyncClient* c, void* data, size_t len) { (static_cast<ModbusClientTCPasync*>(i))->onPacket(static_cast<uint8_t*>(data), len); }, this);
|
AsyncClient * c,
|
||||||
MTA_client.onPoll([](void* i, AsyncClient* c) { (static_cast<ModbusClientTCPasync*>(i))->onPoll(); }, this);
|
void * data,
|
||||||
|
size_t len) { (static_cast<ModbusClientTCPasync *>(i))->onPacket(static_cast<uint8_t *>(data), len); },
|
||||||
|
this);
|
||||||
|
MTA_client.onPoll([](void * i, AsyncClient * c) { (static_cast<ModbusClientTCPasync *>(i))->onPoll(); }, this);
|
||||||
|
|
||||||
// disable nagle algorithm ref Modbus spec
|
// disable nagle algorithm ref Modbus spec
|
||||||
MTA_client.setNoDelay(true);
|
MTA_client.setNoDelay(true);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Destructor: clean up queue, task etc.
|
// Destructor: clean up queue, task etc.
|
||||||
ModbusClientTCPasync::~ModbusClientTCPasync() {
|
ModbusClientTCPasync::~ModbusClientTCPasync() {
|
||||||
// Clean up queue
|
// Clean up queue
|
||||||
{
|
{
|
||||||
// Safely lock access
|
// Safely lock access
|
||||||
LOCK_GUARD(lock1, qLock);
|
LOCK_GUARD(lock1, qLock);
|
||||||
LOCK_GUARD(lock2, sLock);
|
LOCK_GUARD(lock2, sLock);
|
||||||
// Delete all elements from queues
|
// Delete all elements from queues
|
||||||
while (!txQueue.empty()) {
|
while (!txQueue.empty()) {
|
||||||
delete txQueue.front();
|
delete txQueue.front();
|
||||||
txQueue.pop_front();
|
txQueue.pop_front();
|
||||||
|
}
|
||||||
|
for (auto it = rxQueue.cbegin(); it != rxQueue.cend(); /* no increment */) {
|
||||||
|
delete it->second;
|
||||||
|
it = rxQueue.erase(it);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
for (auto it = rxQueue.cbegin(); it != rxQueue.cend();/* no increment */) {
|
// force close client
|
||||||
delete it->second;
|
MTA_client.close(true);
|
||||||
it = rxQueue.erase(it);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
// force close client
|
|
||||||
MTA_client.close(true);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// optionally manually connect to modbus server. Otherwise connection will be made upon first request
|
// optionally manually connect to modbus server. Otherwise connection will be made upon first request
|
||||||
void ModbusClientTCPasync::connect() {
|
void ModbusClientTCPasync::connect() {
|
||||||
LOG_D("connecting\n");
|
LOG_D("connecting\n");
|
||||||
LOCK_GUARD(lock1, sLock);
|
LOCK_GUARD(lock1, sLock);
|
||||||
// only connect if disconnected
|
// only connect if disconnected
|
||||||
if (MTA_state == DISCONNECTED) {
|
if (MTA_state == DISCONNECTED) {
|
||||||
MTA_state = CONNECTING;
|
MTA_state = CONNECTING;
|
||||||
MTA_client.connect(MTA_host, MTA_port);
|
MTA_client.connect(MTA_host, MTA_port);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// connect to another modbus server.
|
// connect to another modbus server.
|
||||||
void ModbusClientTCPasync::connect(IPAddress host, uint16_t port) {
|
void ModbusClientTCPasync::connect(IPAddress host, uint16_t port) {
|
||||||
// First disconnect, if connected
|
// First disconnect, if connected
|
||||||
disconnect(true);
|
disconnect(true);
|
||||||
// Set new host and port
|
// Set new host and port
|
||||||
MTA_host = host;
|
MTA_host = host;
|
||||||
MTA_port = port;
|
MTA_port = port;
|
||||||
connect();
|
connect();
|
||||||
}
|
}
|
||||||
|
|
||||||
// manually disconnect from modbus server. Connection will also auto close after idle time
|
// manually disconnect from modbus server. Connection will also auto close after idle time
|
||||||
void ModbusClientTCPasync::disconnect(bool force) {
|
void ModbusClientTCPasync::disconnect(bool force) {
|
||||||
LOG_D("disconnecting\n");
|
LOG_D("disconnecting\n");
|
||||||
MTA_client.close(force);
|
MTA_client.close(force);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Set timeout value
|
// Set timeout value
|
||||||
void ModbusClientTCPasync::setTimeout(uint32_t timeout) {
|
void ModbusClientTCPasync::setTimeout(uint32_t timeout) {
|
||||||
MTA_timeout = timeout;
|
MTA_timeout = timeout;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Set idle timeout value (time before connection auto closes after being idle)
|
// Set idle timeout value (time before connection auto closes after being idle)
|
||||||
void ModbusClientTCPasync::setIdleTimeout(uint32_t timeout) {
|
void ModbusClientTCPasync::setIdleTimeout(uint32_t timeout) {
|
||||||
MTA_idleTimeout = timeout;
|
MTA_idleTimeout = timeout;
|
||||||
}
|
}
|
||||||
|
|
||||||
void ModbusClientTCPasync::setMaxInflightRequests(uint32_t maxInflightRequests) {
|
void ModbusClientTCPasync::setMaxInflightRequests(uint32_t maxInflightRequests) {
|
||||||
MTA_maxInflightRequests = maxInflightRequests;
|
MTA_maxInflightRequests = maxInflightRequests;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Remove all pending request from queue
|
// Remove all pending request from queue
|
||||||
void ModbusClientTCPasync::clearQueue()
|
void ModbusClientTCPasync::clearQueue() {
|
||||||
{
|
LOCK_GUARD(lock1, qLock);
|
||||||
LOCK_GUARD(lock1, qLock);
|
LOCK_GUARD(lock2, sLock);
|
||||||
LOCK_GUARD(lock2, sLock);
|
// Delete all elements from queues
|
||||||
// Delete all elements from queues
|
while (!txQueue.empty()) {
|
||||||
while (!txQueue.empty()) {
|
delete txQueue.front();
|
||||||
delete txQueue.front();
|
txQueue.pop_front();
|
||||||
txQueue.pop_front();
|
}
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Base addRequest for preformatted ModbusMessage and last set target
|
// Base addRequest for preformatted ModbusMessage and last set target
|
||||||
Error ModbusClientTCPasync::addRequestM(ModbusMessage msg, uint32_t token) {
|
Error ModbusClientTCPasync::addRequestM(ModbusMessage msg, uint32_t token) {
|
||||||
Error rc = SUCCESS; // Return value
|
Error rc = SUCCESS; // Return value
|
||||||
|
|
||||||
// Add it to the queue, if valid
|
// Add it to the queue, if valid
|
||||||
if (msg) {
|
if (msg) {
|
||||||
// Queue add successful?
|
// Queue add successful?
|
||||||
if (!addToQueue(token, msg)) {
|
if (!addToQueue(token, msg)) {
|
||||||
// No. Return error after deleting the allocated request.
|
// No. Return error after deleting the allocated request.
|
||||||
rc = REQUEST_QUEUE_FULL;
|
rc = REQUEST_QUEUE_FULL;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
|
||||||
|
|
||||||
LOG_D("Add TCP request result: %02X\n", rc);
|
LOG_D("Add TCP request result: %02X\n", rc);
|
||||||
return rc;
|
return rc;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Base syncRequest follows the same pattern
|
// Base syncRequest follows the same pattern
|
||||||
ModbusMessage ModbusClientTCPasync::syncRequestM(ModbusMessage msg, uint32_t token) {
|
ModbusMessage ModbusClientTCPasync::syncRequestM(ModbusMessage msg, uint32_t token) {
|
||||||
ModbusMessage response;
|
ModbusMessage response;
|
||||||
|
|
||||||
if (msg) {
|
if (msg) {
|
||||||
// Queue add successful?
|
// Queue add successful?
|
||||||
if (!addToQueue(token, msg, true)) {
|
if (!addToQueue(token, msg, true)) {
|
||||||
// No. Return error after deleting the allocated request.
|
// No. Return error after deleting the allocated request.
|
||||||
response.setError(msg.getServerID(), msg.getFunctionCode(), REQUEST_QUEUE_FULL);
|
response.setError(msg.getServerID(), msg.getFunctionCode(), REQUEST_QUEUE_FULL);
|
||||||
|
} else {
|
||||||
|
// Request is queued - wait for the result.
|
||||||
|
response = waitSync(msg.getServerID(), msg.getFunctionCode(), token);
|
||||||
|
}
|
||||||
} else {
|
} else {
|
||||||
// Request is queued - wait for the result.
|
response.setError(msg.getServerID(), msg.getFunctionCode(), EMPTY_MESSAGE);
|
||||||
response = waitSync(msg.getServerID(), msg.getFunctionCode(), token);
|
|
||||||
}
|
}
|
||||||
} else {
|
return response;
|
||||||
response.setError(msg.getServerID(), msg.getFunctionCode(), EMPTY_MESSAGE);
|
|
||||||
}
|
|
||||||
return response;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// addToQueue: send freshly created request to queue
|
// addToQueue: send freshly created request to queue
|
||||||
bool ModbusClientTCPasync::addToQueue(int32_t token, ModbusMessage request, bool syncReq) {
|
bool ModbusClientTCPasync::addToQueue(int32_t token, ModbusMessage request, bool syncReq) {
|
||||||
// Did we get one?
|
// Did we get one?
|
||||||
if (request) {
|
if (request) {
|
||||||
LOCK_GUARD(lock1, qLock);
|
LOCK_GUARD(lock1, qLock);
|
||||||
if (txQueue.size() + rxQueue.size() < MTA_qLimit) {
|
if (txQueue.size() + rxQueue.size() < MTA_qLimit) {
|
||||||
HEXDUMP_V("Enqueue", request.data(), request.size());
|
HEXDUMP_V("Enqueue", request.data(), request.size());
|
||||||
RequestEntry *re = new RequestEntry(token, request, syncReq);
|
RequestEntry * re = new RequestEntry(token, request, syncReq);
|
||||||
if (!re) return false; //TODO: proper error returning in case allocation fails
|
if (!re)
|
||||||
// inject proper transactionID
|
return false; // TODO: proper error returning in case allocation fails
|
||||||
re->head.transactionID = messageCount++;
|
// inject proper transactionID
|
||||||
re->head.len = request.size();
|
re->head.transactionID = messageCount++;
|
||||||
// if we're already connected, try to send and push to rxQueue
|
re->head.len = request.size();
|
||||||
// or else push to txQueue and (re)connect
|
// if we're already connected, try to send and push to rxQueue
|
||||||
if (MTA_state == CONNECTED && send(re)) {
|
// or else push to txQueue and (re)connect
|
||||||
re->sentTime = millis();
|
if (MTA_state == CONNECTED && send(re)) {
|
||||||
rxQueue[re->head.transactionID] = re;
|
re->sentTime = millis();
|
||||||
} else {
|
rxQueue[re->head.transactionID] = re;
|
||||||
txQueue.push_back(re);
|
} else {
|
||||||
if (MTA_state == DISCONNECTED) {
|
txQueue.push_back(re);
|
||||||
connect();
|
if (MTA_state == DISCONNECTED) {
|
||||||
|
connect();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return true;
|
||||||
}
|
}
|
||||||
}
|
LOG_E("queue is full\n");
|
||||||
return true;
|
|
||||||
}
|
}
|
||||||
LOG_E("queue is full\n");
|
return false;
|
||||||
}
|
|
||||||
return false;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void ModbusClientTCPasync::onConnected() {
|
void ModbusClientTCPasync::onConnected() {
|
||||||
LOG_D("connected\n");
|
LOG_D("connected\n");
|
||||||
LOCK_GUARD(lock1, sLock);
|
LOCK_GUARD(lock1, sLock);
|
||||||
MTA_state = CONNECTED;
|
MTA_state = CONNECTED;
|
||||||
MTA_lastActivity = millis();
|
MTA_lastActivity = millis();
|
||||||
// from now on onPoll will be called every 500 msec
|
// from now on onPoll will be called every 500 msec
|
||||||
}
|
}
|
||||||
|
|
||||||
void ModbusClientTCPasync::onDisconnected() {
|
void ModbusClientTCPasync::onDisconnected() {
|
||||||
LOG_D("disconnected\n");
|
LOG_D("disconnected\n");
|
||||||
LOCK_GUARD(lock1, sLock);
|
LOCK_GUARD(lock1, sLock);
|
||||||
MTA_state = DISCONNECTED;
|
MTA_state = DISCONNECTED;
|
||||||
|
|
||||||
// empty queue on disconnect, calling errorcode on every waiting request
|
// empty queue on disconnect, calling errorcode on every waiting request
|
||||||
LOCK_GUARD(lock2, qLock);
|
LOCK_GUARD(lock2, qLock);
|
||||||
while (!txQueue.empty()) {
|
while (!txQueue.empty()) {
|
||||||
RequestEntry* r = txQueue.front();
|
RequestEntry * r = txQueue.front();
|
||||||
if (onError) {
|
if (onError) {
|
||||||
onError(IP_CONNECTION_FAILED, r->token);
|
onError(IP_CONNECTION_FAILED, r->token);
|
||||||
|
}
|
||||||
|
delete r;
|
||||||
|
txQueue.pop_front();
|
||||||
}
|
}
|
||||||
delete r;
|
while (!rxQueue.empty()) {
|
||||||
txQueue.pop_front();
|
RequestEntry * r = rxQueue.begin()->second;
|
||||||
}
|
if (onError) {
|
||||||
while (!rxQueue.empty()) {
|
onError(IP_CONNECTION_FAILED, r->token);
|
||||||
RequestEntry *r = rxQueue.begin()->second;
|
}
|
||||||
if (onError) {
|
delete r;
|
||||||
onError(IP_CONNECTION_FAILED, r->token);
|
rxQueue.erase(rxQueue.begin());
|
||||||
}
|
}
|
||||||
delete r;
|
|
||||||
rxQueue.erase(rxQueue.begin());
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void ModbusClientTCPasync::onACError(AsyncClient* c, int8_t error) {
|
void ModbusClientTCPasync::onACError(AsyncClient * c, int8_t error) {
|
||||||
// onDisconnect will alse be called, so nothing to do here
|
// onDisconnect will alse be called, so nothing to do here
|
||||||
LOG_W("TCP error: %s\n", c->errorToString(error));
|
LOG_W("TCP error: %s\n", c->errorToString(error));
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
@@ -222,180 +225,178 @@ void onAck(size_t len, uint32_t time) {
|
|||||||
// assuming we don't need this
|
// assuming we don't need this
|
||||||
}
|
}
|
||||||
*/
|
*/
|
||||||
void ModbusClientTCPasync::onPacket(uint8_t* data, size_t length) {
|
void ModbusClientTCPasync::onPacket(uint8_t * data, size_t length) {
|
||||||
LOG_D("packet received (len:%d)\n", length);
|
LOG_D("packet received (len:%d)\n", length);
|
||||||
// reset idle timeout
|
// reset idle timeout
|
||||||
MTA_lastActivity = millis();
|
MTA_lastActivity = millis();
|
||||||
|
|
||||||
if (length) {
|
if (length) {
|
||||||
LOG_D("parsing (len:%d)\n", length + 1);
|
LOG_D("parsing (len:%d)\n", length + 1);
|
||||||
}
|
|
||||||
while (length > 0) {
|
|
||||||
RequestEntry* request = nullptr;
|
|
||||||
ModbusMessage* response = nullptr;
|
|
||||||
uint16_t transactionID = 0;
|
|
||||||
uint16_t protocolID = 0;
|
|
||||||
uint16_t messageLength = 0;
|
|
||||||
bool isOkay = false;
|
|
||||||
|
|
||||||
// 1. Check for valid modbus message
|
|
||||||
|
|
||||||
// MBAP header is 6 bytes, we can't do anything with less
|
|
||||||
// total message should fit MBAP plus remaining bytes (in data[4], data[5])
|
|
||||||
if (length > 6) {
|
|
||||||
transactionID = (data[0] << 8) | data[1];
|
|
||||||
protocolID = (data[2] << 8) | data[3];
|
|
||||||
messageLength = (data[4] << 8) | data[5];
|
|
||||||
if (protocolID == 0 &&
|
|
||||||
length >= (uint32_t)messageLength + 6 &&
|
|
||||||
messageLength < 256) {
|
|
||||||
response = new ModbusMessage(messageLength);
|
|
||||||
response->add(&data[6], messageLength);
|
|
||||||
LOG_D("packet validated (len:%d)\n", messageLength);
|
|
||||||
|
|
||||||
// on next iteration: adjust remaining length and pointer to data
|
|
||||||
length -= 6 + messageLength;
|
|
||||||
data += 6 + messageLength;
|
|
||||||
isOkay = true;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
while (length > 0) {
|
||||||
|
RequestEntry * request = nullptr;
|
||||||
|
ModbusMessage * response = nullptr;
|
||||||
|
uint16_t transactionID = 0;
|
||||||
|
uint16_t protocolID = 0;
|
||||||
|
uint16_t messageLength = 0;
|
||||||
|
bool isOkay = false;
|
||||||
|
|
||||||
if (!isOkay) {
|
// 1. Check for valid modbus message
|
||||||
// invalid packet, abort function
|
|
||||||
LOG_W("packet invalid\n");
|
|
||||||
return;
|
|
||||||
} else {
|
|
||||||
// 2. we got a valid response, match with a request
|
|
||||||
LOCK_GUARD(lock1, qLock);
|
|
||||||
auto i = rxQueue.find(transactionID);
|
|
||||||
if (i != rxQueue.end()) {
|
|
||||||
// found it, handle it and stop iterating
|
|
||||||
request = i->second;
|
|
||||||
i = rxQueue.erase(i);
|
|
||||||
LOG_D("matched request\n");
|
|
||||||
} else {
|
|
||||||
// TCP packet did not yield valid modbus response, abort function
|
|
||||||
LOG_W("no matching request found\n");
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// 3. we have a valid request and a valid response, call appropriate callback
|
// MBAP header is 6 bytes, we can't do anything with less
|
||||||
if (request) {
|
// total message should fit MBAP plus remaining bytes (in data[4], data[5])
|
||||||
// compare request with response
|
if (length > 6) {
|
||||||
Error error = SUCCESS;
|
transactionID = (data[0] << 8) | data[1];
|
||||||
if (request->msg.getFunctionCode() != (response->getFunctionCode() & 0x7F)) {
|
protocolID = (data[2] << 8) | data[3];
|
||||||
error = FC_MISMATCH;
|
messageLength = (data[4] << 8) | data[5];
|
||||||
} else if (request->msg.getServerID() != response->getServerID()) {
|
if (protocolID == 0 && length >= (uint32_t)messageLength + 6 && messageLength < 256) {
|
||||||
error = SERVER_ID_MISMATCH;
|
response = new ModbusMessage(messageLength);
|
||||||
} else {
|
response->add(&data[6], messageLength);
|
||||||
error = response->getError();
|
LOG_D("packet validated (len:%d)\n", messageLength);
|
||||||
}
|
|
||||||
|
|
||||||
if (error != SUCCESS) {
|
// on next iteration: adjust remaining length and pointer to data
|
||||||
LOCK_GUARD(errorCntLock, countAccessM);
|
length -= 6 + messageLength;
|
||||||
errorCount++;
|
data += 6 + messageLength;
|
||||||
}
|
isOkay = true;
|
||||||
|
}
|
||||||
if (request->isSyncRequest) {
|
|
||||||
{
|
|
||||||
LOCK_GUARD(sL ,syncRespM);
|
|
||||||
syncResponse[request->token] = *response;
|
|
||||||
}
|
}
|
||||||
} else if (onResponse) {
|
|
||||||
onResponse(*response, request->token);
|
if (!isOkay) {
|
||||||
} else {
|
// invalid packet, abort function
|
||||||
if (error == SUCCESS) {
|
LOG_W("packet invalid\n");
|
||||||
if (onData) {
|
return;
|
||||||
onData(*response, request->token);
|
|
||||||
}
|
|
||||||
} else {
|
} else {
|
||||||
if (onError) {
|
// 2. we got a valid response, match with a request
|
||||||
onError(response->getError(), request->token);
|
LOCK_GUARD(lock1, qLock);
|
||||||
}
|
auto i = rxQueue.find(transactionID);
|
||||||
|
if (i != rxQueue.end()) {
|
||||||
|
// found it, handle it and stop iterating
|
||||||
|
request = i->second;
|
||||||
|
i = rxQueue.erase(i);
|
||||||
|
LOG_D("matched request\n");
|
||||||
|
} else {
|
||||||
|
// TCP packet did not yield valid modbus response, abort function
|
||||||
|
LOG_W("no matching request found\n");
|
||||||
|
return;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
|
||||||
delete request;
|
|
||||||
}
|
|
||||||
delete response;
|
|
||||||
|
|
||||||
} // end processing of incoming data
|
// 3. we have a valid request and a valid response, call appropriate callback
|
||||||
|
if (request) {
|
||||||
|
// compare request with response
|
||||||
|
Error error = SUCCESS;
|
||||||
|
if (request->msg.getFunctionCode() != (response->getFunctionCode() & 0x7F)) {
|
||||||
|
error = FC_MISMATCH;
|
||||||
|
} else if (request->msg.getServerID() != response->getServerID()) {
|
||||||
|
error = SERVER_ID_MISMATCH;
|
||||||
|
} else {
|
||||||
|
error = response->getError();
|
||||||
|
}
|
||||||
|
|
||||||
// check if we have to send the next request
|
if (error != SUCCESS) {
|
||||||
LOCK_GUARD(lock1, qLock);
|
LOCK_GUARD(errorCntLock, countAccessM);
|
||||||
handleSendingQueue();
|
errorCount++;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (request->isSyncRequest) {
|
||||||
|
{
|
||||||
|
LOCK_GUARD(sL, syncRespM);
|
||||||
|
syncResponse[request->token] = *response;
|
||||||
|
}
|
||||||
|
} else if (onResponse) {
|
||||||
|
onResponse(*response, request->token);
|
||||||
|
} else {
|
||||||
|
if (error == SUCCESS) {
|
||||||
|
if (onData) {
|
||||||
|
onData(*response, request->token);
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
if (onError) {
|
||||||
|
onError(response->getError(), request->token);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
delete request;
|
||||||
|
}
|
||||||
|
delete response;
|
||||||
|
|
||||||
|
} // end processing of incoming data
|
||||||
|
|
||||||
|
// check if we have to send the next request
|
||||||
|
LOCK_GUARD(lock1, qLock);
|
||||||
|
handleSendingQueue();
|
||||||
}
|
}
|
||||||
|
|
||||||
void ModbusClientTCPasync::onPoll() {
|
void ModbusClientTCPasync::onPoll() {
|
||||||
{
|
{
|
||||||
LOCK_GUARD(lock1, qLock);
|
LOCK_GUARD(lock1, qLock);
|
||||||
|
|
||||||
// try to send whatever is waiting
|
// try to send whatever is waiting
|
||||||
handleSendingQueue();
|
handleSendingQueue();
|
||||||
|
|
||||||
// next check if timeout has struck for oldest request
|
// next check if timeout has struck for oldest request
|
||||||
if (!rxQueue.empty()) {
|
if (!rxQueue.empty()) {
|
||||||
RequestEntry* request = rxQueue.begin()->second;
|
RequestEntry * request = rxQueue.begin()->second;
|
||||||
if (millis() - request->sentTime > MTA_timeout) {
|
if (millis() - request->sentTime > MTA_timeout) {
|
||||||
LOG_D("request timeouts (now:%lu-sent:%u)\n", millis(), request->sentTime);
|
LOG_D("request timeouts (now:%lu-sent:%u)\n", millis(), request->sentTime);
|
||||||
// oldest element timeouts, call onError and clean up
|
// oldest element timeouts, call onError and clean up
|
||||||
if (onError) {
|
if (onError) {
|
||||||
// Handle timeout error
|
// Handle timeout error
|
||||||
onError(TIMEOUT, request->token);
|
onError(TIMEOUT, request->token);
|
||||||
}
|
}
|
||||||
delete request;
|
delete request;
|
||||||
rxQueue.erase(rxQueue.begin());
|
rxQueue.erase(rxQueue.begin());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
} // end lockguard scope
|
||||||
|
|
||||||
|
// if nothing happened during idle timeout, gracefully close connection
|
||||||
|
if (millis() - MTA_lastActivity > MTA_idleTimeout) {
|
||||||
|
disconnect();
|
||||||
}
|
}
|
||||||
}
|
|
||||||
|
|
||||||
} // end lockguard scope
|
|
||||||
|
|
||||||
// if nothing happened during idle timeout, gracefully close connection
|
|
||||||
if (millis() - MTA_lastActivity > MTA_idleTimeout) {
|
|
||||||
disconnect();
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void ModbusClientTCPasync::handleSendingQueue() {
|
void ModbusClientTCPasync::handleSendingQueue() {
|
||||||
// ATTENTION: This method does not have a lock guard.
|
// ATTENTION: This method does not have a lock guard.
|
||||||
// Calling sites must assure shared resources are protected
|
// Calling sites must assure shared resources are protected
|
||||||
// by mutex.
|
// by mutex.
|
||||||
|
|
||||||
// try to send everything we have waiting
|
// try to send everything we have waiting
|
||||||
std::list<RequestEntry*>::iterator it = txQueue.begin();
|
std::list<RequestEntry *>::iterator it = txQueue.begin();
|
||||||
while (it != txQueue.end()) {
|
while (it != txQueue.end()) {
|
||||||
// get the actual element
|
// get the actual element
|
||||||
if (send(*it)) {
|
if (send(*it)) {
|
||||||
// after sending, update timeout value, add to other queue and remove from this queue
|
// after sending, update timeout value, add to other queue and remove from this queue
|
||||||
(*it)->sentTime = millis();
|
(*it)->sentTime = millis();
|
||||||
rxQueue[(*it)->head.transactionID] = (*it); // push request to other queue
|
rxQueue[(*it)->head.transactionID] = (*it); // push request to other queue
|
||||||
it = txQueue.erase(it); // remove from toSend queue and point i to next request
|
it = txQueue.erase(it); // remove from toSend queue and point i to next request
|
||||||
} else {
|
} else {
|
||||||
// sending didn't succeed, try next request
|
// sending didn't succeed, try next request
|
||||||
++it;
|
++it;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
bool ModbusClientTCPasync::send(RequestEntry* re) {
|
bool ModbusClientTCPasync::send(RequestEntry * re) {
|
||||||
// ATTENTION: This method does not have a lock guard.
|
// ATTENTION: This method does not have a lock guard.
|
||||||
// Calling sites must assure shared resources are protected
|
// Calling sites must assure shared resources are protected
|
||||||
// by mutex.
|
// by mutex.
|
||||||
|
|
||||||
if (rxQueue.size() >= MTA_maxInflightRequests) {
|
if (rxQueue.size() >= MTA_maxInflightRequests) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// check if TCP client is able to send
|
||||||
|
if (MTA_client.space() > ((uint32_t)re->msg.size() + 6)) {
|
||||||
|
// Write TCP header first
|
||||||
|
MTA_client.add(reinterpret_cast<const char *>((const uint8_t *)(re->head)), 6, ASYNC_WRITE_FLAG_COPY);
|
||||||
|
// Request comes next
|
||||||
|
MTA_client.add(reinterpret_cast<const char *>(re->msg.data()), re->msg.size(), ASYNC_WRITE_FLAG_COPY);
|
||||||
|
// done
|
||||||
|
MTA_client.send();
|
||||||
|
LOG_D("request sent (msgid:%d)\n", re->head.transactionID);
|
||||||
|
return true;
|
||||||
|
}
|
||||||
return false;
|
return false;
|
||||||
}
|
|
||||||
|
|
||||||
// check if TCP client is able to send
|
|
||||||
if (MTA_client.space() > ((uint32_t)re->msg.size() + 6)) {
|
|
||||||
// Write TCP header first
|
|
||||||
MTA_client.add(reinterpret_cast<const char *>((const uint8_t *)(re->head)), 6, ASYNC_WRITE_FLAG_COPY);
|
|
||||||
// Request comes next
|
|
||||||
MTA_client.add(reinterpret_cast<const char*>(re->msg.data()), re->msg.size(), ASYNC_WRITE_FLAG_COPY);
|
|
||||||
// done
|
|
||||||
MTA_client.send();
|
|
||||||
LOG_D("request sent (msgid:%d)\n", re->head.transactionID);
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
return false;
|
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -1,6 +1,5 @@
|
|||||||
/**
|
/**
|
||||||
* TODO:
|
* TODO: verwendete libs in readme hinzufügen
|
||||||
* - verwendete libs in readme hinzufügen
|
|
||||||
*/
|
*/
|
||||||
#include "modbus.h"
|
#include "modbus.h"
|
||||||
#include "modbus_entity_parameters.hpp"
|
#include "modbus_entity_parameters.hpp"
|
||||||
|
|||||||
Reference in New Issue
Block a user