mirror of
https://github.com/dalathegreat/Battery-Emulator.git
synced 2025-10-03 17:59:27 +02:00
340 lines
10 KiB
C++
340 lines
10 KiB
C++
// =================================================================================================
|
|
// eModbus: Copyright 2020 by Michael Harwerth, Bert Melis and the contributors to eModbus
|
|
// MIT license - see license.md for details
|
|
// =================================================================================================
|
|
#include "ModbusClientRTU.h"
|
|
|
|
#if HAS_FREERTOS
|
|
|
|
#undef LOCAL_LOG_LEVEL
|
|
// #define LOCAL_LOG_LEVEL LOG_LEVEL_VERBOSE
|
|
#include "Logging.h"
|
|
|
|
// Constructor takes Serial reference and optional DE/RE pin
|
|
ModbusClientRTU::ModbusClientRTU(int8_t rtsPin, uint16_t queueLimit) :
|
|
ModbusClient(),
|
|
MR_serial(nullptr),
|
|
MR_lastMicros(micros()),
|
|
MR_interval(2000),
|
|
MR_rtsPin(rtsPin),
|
|
MR_qLimit(queueLimit),
|
|
MR_timeoutValue(DEFAULTTIMEOUT),
|
|
MR_useASCII(false),
|
|
MR_skipLeadingZeroByte(false) {
|
|
if (MR_rtsPin >= 0) {
|
|
pinMode(MR_rtsPin, OUTPUT);
|
|
MTRSrts = [this](bool level) {
|
|
digitalWrite(MR_rtsPin, level);
|
|
};
|
|
MTRSrts(LOW);
|
|
} else {
|
|
MTRSrts = RTUutils::RTSauto;
|
|
}
|
|
}
|
|
|
|
// Alternative constructor takes Serial reference and RTS callback function
|
|
ModbusClientRTU::ModbusClientRTU(RTScallback rts, uint16_t queueLimit) :
|
|
ModbusClient(),
|
|
MR_serial(nullptr),
|
|
MR_lastMicros(micros()),
|
|
MR_interval(2000),
|
|
MTRSrts(rts),
|
|
MR_qLimit(queueLimit),
|
|
MR_timeoutValue(DEFAULTTIMEOUT),
|
|
MR_useASCII(false),
|
|
MR_skipLeadingZeroByte(false) {
|
|
MR_rtsPin = -1;
|
|
MTRSrts(LOW);
|
|
}
|
|
|
|
// Destructor: clean up queue, task etc.
|
|
ModbusClientRTU::~ModbusClientRTU() {
|
|
// Kill worker task and clean up request queue
|
|
end();
|
|
}
|
|
|
|
// begin: start worker task - general version
|
|
void ModbusClientRTU::begin(Stream& serial, uint32_t baudRate, int coreID) {
|
|
MR_serial = &serial;
|
|
doBegin(baudRate, coreID);
|
|
}
|
|
|
|
// begin: start worker task - HardwareSerial version
|
|
void ModbusClientRTU::begin(HardwareSerial& serial, int coreID) {
|
|
MR_serial = &serial;
|
|
uint32_t baudRate = serial.baudRate();
|
|
serial.setRxFIFOFull(1);
|
|
doBegin(baudRate, coreID);
|
|
}
|
|
|
|
void ModbusClientRTU::doBegin(uint32_t baudRate, int coreID) {
|
|
// Task already running? End it in case
|
|
end();
|
|
|
|
// Pull down RTS toggle, if necessary
|
|
MTRSrts(LOW);
|
|
|
|
// Set minimum interval time
|
|
MR_interval = RTUutils::calculateInterval(baudRate);
|
|
|
|
// Create unique task name
|
|
char taskName[18];
|
|
snprintf(taskName, 18, "Modbus%02XRTU", instanceCounter);
|
|
// Start task to handle the queue
|
|
xTaskCreatePinnedToCore((TaskFunction_t)&handleConnection, taskName, 4096, this, 6, &worker, coreID >= 0 ? coreID : NULL);
|
|
|
|
LOG_D("Client task %d started. Interval=%d\n", (uint32_t)worker, MR_interval);
|
|
}
|
|
|
|
// end: stop worker task
|
|
void ModbusClientRTU::end() {
|
|
if (worker) {
|
|
// Clean up queue
|
|
{
|
|
// Safely lock access
|
|
LOCK_GUARD(lockGuard, qLock);
|
|
// Get all queue entries one by one
|
|
while (!requests.empty()) {
|
|
// Remove front entry
|
|
requests.pop();
|
|
}
|
|
}
|
|
// Kill task
|
|
vTaskDelete(worker);
|
|
LOG_D("Client task %d killed.\n", (uint32_t)worker);
|
|
}
|
|
}
|
|
|
|
// setTimeOut: set/change the default interface timeout
|
|
void ModbusClientRTU::setTimeout(uint32_t TOV) {
|
|
MR_timeoutValue = TOV;
|
|
LOG_D("Timeout set to %d\n", TOV);
|
|
}
|
|
|
|
// Toggle protocol to ModbusASCII
|
|
void ModbusClientRTU::useModbusASCII(unsigned long timeout) {
|
|
MR_useASCII = true;
|
|
MR_timeoutValue = timeout; // Switch timeout to ASCII's value
|
|
LOG_D("Protocol mode: ASCII\n");
|
|
}
|
|
|
|
// Toggle protocol to ModbusRTU
|
|
void ModbusClientRTU::useModbusRTU() {
|
|
MR_useASCII = false;
|
|
LOG_D("Protocol mode: RTU\n");
|
|
}
|
|
|
|
// Inquire protocol mode
|
|
bool ModbusClientRTU::isModbusASCII() {
|
|
return MR_useASCII;
|
|
}
|
|
|
|
// Toggle skipping of leading 0x00 byte
|
|
void ModbusClientRTU::skipLeading0x00(bool onOff) {
|
|
MR_skipLeadingZeroByte = onOff;
|
|
LOG_D("Skip leading 0x00 mode = %s\n", onOff ? "ON" : "OFF");
|
|
}
|
|
|
|
// Return number of unprocessed requests in queue
|
|
uint32_t ModbusClientRTU::pendingRequests() {
|
|
return requests.size();
|
|
}
|
|
|
|
// Base addRequest taking a preformatted data buffer and length as parameters
|
|
Error ModbusClientRTU::addRequestM(ModbusMessage msg, uint32_t token) {
|
|
Error rc = SUCCESS; // Return value
|
|
|
|
LOG_D("request for %02X/%02X\n", msg.getServerID(), msg.getFunctionCode());
|
|
|
|
// Add it to the queue, if valid
|
|
if (msg) {
|
|
// Queue add successful?
|
|
if (!addToQueue(token, msg)) {
|
|
// No. Return error after deleting the allocated request.
|
|
rc = REQUEST_QUEUE_FULL;
|
|
}
|
|
}
|
|
|
|
LOG_D("RC=%02X\n", rc);
|
|
return rc;
|
|
}
|
|
|
|
// Base syncRequest follows the same pattern
|
|
ModbusMessage ModbusClientRTU::syncRequestM(ModbusMessage msg, uint32_t token) {
|
|
ModbusMessage response;
|
|
|
|
if (msg) {
|
|
// Queue add successful?
|
|
if (!addToQueue(token, msg, true)) {
|
|
// No. Return error after deleting the allocated request.
|
|
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 {
|
|
response.setError(msg.getServerID(), msg.getFunctionCode(), EMPTY_MESSAGE);
|
|
}
|
|
return response;
|
|
}
|
|
|
|
// addBroadcastMessage: create a fire-and-forget message to all servers on the RTU bus
|
|
Error ModbusClientRTU::addBroadcastMessage(const uint8_t *data, uint8_t len) {
|
|
Error rc = SUCCESS; // Return value
|
|
|
|
LOG_D("Broadcast request of length %d\n", len);
|
|
|
|
// We do only accept requests with data, 0 byte, data and CRC must fit into 256 bytes.
|
|
if (len && len < 254) {
|
|
// Create a "broadcast token"
|
|
uint32_t token = (millis() & 0xFFFFFF) | 0xBC000000;
|
|
ModbusMessage msg;
|
|
|
|
// Server ID is 0x00 for broadcast
|
|
msg.add((uint8_t)0x00);
|
|
// Append data
|
|
msg.add(data, len);
|
|
|
|
// Queue add successful?
|
|
if (!addToQueue(token, msg)) {
|
|
// No. Return error after deleting the allocated request.
|
|
rc = REQUEST_QUEUE_FULL;
|
|
}
|
|
} else {
|
|
rc = BROADCAST_ERROR;
|
|
}
|
|
|
|
LOG_D("RC=%02X\n", rc);
|
|
return rc;
|
|
}
|
|
|
|
|
|
// addToQueue: send freshly created request to queue
|
|
bool ModbusClientRTU::addToQueue(uint32_t token, ModbusMessage request, bool syncReq) {
|
|
bool rc = false;
|
|
// Did we get one?
|
|
if (request) {
|
|
RequestEntry re(token, request, syncReq);
|
|
if (requests.size()<MR_qLimit) {
|
|
// Yes. Safely lock queue and push request to queue
|
|
rc = true;
|
|
LOCK_GUARD(lockGuard, qLock);
|
|
requests.push(re);
|
|
}
|
|
{
|
|
LOCK_GUARD(cntLock, countAccessM);
|
|
messageCount++;
|
|
}
|
|
}
|
|
|
|
LOG_D("RC=%02X\n", rc);
|
|
return rc;
|
|
}
|
|
|
|
// handleConnection: worker task
|
|
// This was created in begin() to handle the queue entries
|
|
void ModbusClientRTU::handleConnection(ModbusClientRTU *instance) {
|
|
// initially clean the serial buffer
|
|
while (instance->MR_serial->available()) instance->MR_serial->read();
|
|
delay(100);
|
|
|
|
// Loop forever - or until task is killed
|
|
while (1) {
|
|
// Do we have a reuest in queue?
|
|
if (!instance->requests.empty()) {
|
|
// Yes. pull it.
|
|
RequestEntry request = instance->requests.front();
|
|
|
|
LOG_D("Pulled request from queue\n");
|
|
|
|
// Send it via Serial
|
|
RTUutils::send(*(instance->MR_serial), instance->MR_lastMicros, instance->MR_interval, instance->MTRSrts, request.msg, instance->MR_useASCII);
|
|
|
|
LOG_D("Request sent.\n");
|
|
// HEXDUMP_V("Data", request.msg.data(), request.msg.size());
|
|
|
|
// For a broadcast, we will not wait for a response
|
|
if (request.msg.getServerID() != 0 || ((request.token & 0xFF000000) != 0xBC000000)) {
|
|
// This is a regular request, Get the response - if any
|
|
ModbusMessage response = RTUutils::receive(
|
|
'C',
|
|
*(instance->MR_serial),
|
|
instance->MR_timeoutValue,
|
|
instance->MR_lastMicros,
|
|
instance->MR_interval,
|
|
instance->MR_useASCII,
|
|
instance->MR_skipLeadingZeroByte);
|
|
|
|
LOG_D("%s response (%d bytes) received.\n", response.size()>1 ? "Data" : "Error", response.size());
|
|
HEXDUMP_V("Data", response.data(), response.size());
|
|
|
|
// No error in receive()?
|
|
if (response.size() > 1) {
|
|
// No. Check message contents
|
|
// Does the serverID match the requested?
|
|
if (request.msg.getServerID() != response.getServerID()) {
|
|
// No. Return error response
|
|
response.setError(request.msg.getServerID(), request.msg.getFunctionCode(), SERVER_ID_MISMATCH);
|
|
// ServerID ok, but does the FC match as well?
|
|
} else if (request.msg.getFunctionCode() != (response.getFunctionCode() & 0x7F)) {
|
|
// No. Return error response
|
|
response.setError(request.msg.getServerID(), request.msg.getFunctionCode(), FC_MISMATCH);
|
|
}
|
|
} else {
|
|
// No, we got an error code from receive()
|
|
// Return it as error response
|
|
response.setError(request.msg.getServerID(), request.msg.getFunctionCode(), static_cast<Error>(response[0]));
|
|
}
|
|
|
|
LOG_D("Response generated.\n");
|
|
HEXDUMP_V("Response packet", response.data(), response.size());
|
|
|
|
// If we got an error, count it
|
|
if (response.getError() != SUCCESS) {
|
|
instance->errorCount++;
|
|
}
|
|
|
|
// Was it a synchronous request?
|
|
if (request.isSyncRequest) {
|
|
// Yes. Put it into the response map
|
|
{
|
|
LOCK_GUARD(sL, instance->syncRespM);
|
|
instance->syncResponse[request.token] = response;
|
|
}
|
|
// No, an async request. Do we have an onResponse handler?
|
|
} else if (instance->onResponse) {
|
|
// Yes. Call it
|
|
instance->onResponse(response, request.token);
|
|
} else {
|
|
// No, but we may have onData or onError handlers
|
|
// Did we get a normal response?
|
|
if (response.getError()==SUCCESS) {
|
|
// Yes. Do we have an onData handler registered?
|
|
if (instance->onData) {
|
|
// Yes. call it
|
|
instance->onData(response, request.token);
|
|
}
|
|
} else {
|
|
// No, something went wrong. All we have is an error
|
|
// Do we have an onError handler?
|
|
if (instance->onError) {
|
|
// Yes. Forward the error code to it
|
|
instance->onError(response.getError(), request.token);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
// Clean-up time.
|
|
{
|
|
// Safely lock the queue
|
|
LOCK_GUARD(lockGuard, instance->qLock);
|
|
// Remove the front queue entry
|
|
instance->requests.pop();
|
|
}
|
|
} else {
|
|
delay(1);
|
|
}
|
|
}
|
|
}
|
|
|
|
#endif // HAS_FREERTOS
|