Remove address parameter

README.md

@@ -12,6 +12,9 @@ This is a non blocking Modbus client (master) for ESP32.
   -  read discrete inputs (02)
   -  read holding registers (03)
   -  read input registers (04)
+  -  write single holding register (06)
+  -  write multiple holding register (16)
+  -  "raw" requests for arbitrary function codes
 -  similar API as my [esp32ModbusTCP](https://github.com/bertmelis/esp32ModbusTCP) implementation
 
 ## Developement status
@@ -64,7 +67,8 @@ is connected via a 100 Ohms resistor to limit possible ground loop currents.
 
 The API is quite lightweight. It takes minimal 3 steps to get going.
 
-First create the ModbusRTU object. The constructor takes two arguments: HardwareSerial object and pin number of DE/RS.
+First create the ModbusRTU object. The constructor takes two arguments: HardwareSerial object and pin number of DE/RS (or -1, if 
+your module does auto half duplex).
 
 ```C++
 esp32ModbusRTU myModbus(&Serial, DE_PIN);
@@ -119,12 +123,23 @@ The request queue holds maximum 20 items. So a 21st request will fail until the
 #define QUEUE_SIZE 20
 ```
 
-The waiting time before a timeout error is returned can also be changed by a `#define` variable:
+The waiting time before a timeout error is returned can also be changed by a method call:
 
 ```C++
-#define TIMEOUT_MS 5000
+myModbus.setTimeOutValue(5000);
 ```
 
+## Caveat
+
+The ESP32 Arduino core implementation of the handling of Serial interfaces has a design decision built in that prevents real time
+Serial communications for data packets received larger than 112 bytes. the underlying FIFO buffer is only copied into the Serial's buffer 
+when 112 bytes have been received. The copy process then takes longer than the MODBUS timeout lasts, so the remainder of the packet is lost. 
+
+The library has a workaround built in that covers the issue by waiting a loooong time (16 milliseconds) until determining a bus timeout (= end
+of packet). This is possible since the library implements a MODBUS master device, thus controlling the bus timing.
+
+But note that this is not according to MODBUS standards.
+
 ## Issues
 
 Please file a Github issue ~~if~~ when you find a bug. You can also use the issue tracker for feature requests.

examples/SDM630/SDM630.ino

@@ -18,7 +18,7 @@ void setup() {
   Serial.begin(115200);  // Serial output
   Serial1.begin(9600, SERIAL_8N1, 17, 4, true);  // Modbus connection
 
-  modbus.onData([](uint8_t serverAddress, esp32Modbus::FunctionCode fc, uint16_t address, uint8_t* data, size_t length) {
+  modbus.onData([](uint8_t serverAddress, uint8_t fc, uint8_t* data, size_t length) {
     Serial.printf("id 0x%02x fc 0x%02x len %u: 0x", serverAddress, fc, length);
     for (size_t i = 0; i < length; ++i) {
       Serial.printf("%02x", data[i]);

src/ModbusMessage.cpp

@@ -103,7 +103,8 @@ uint16_t make_word(uint8_t high, uint8_t low) {
 ModbusMessage::ModbusMessage(uint8_t length) :
   _buffer(nullptr),
   _length(length),
-  _index(0) {
+  _index(0),
+  _token(0) {
   if (length < 5) _length = 5;  // minimum for Modbus Exception codes
   _buffer = new uint8_t[_length];
   for (uint8_t i = 0; i < _length; ++i) {
@@ -123,6 +124,10 @@ uint8_t ModbusMessage::getSize() {
   return _index;
 }
 
+uint32_t ModbusMessage::getToken() {
+  return _token;
+}
+
 void ModbusMessage::add(uint8_t value) {
   if (_index < _length) _buffer[_index++] = value;
 }
@@ -131,108 +136,94 @@ ModbusRequest::ModbusRequest(uint8_t length) :
   ModbusMessage(length),
   _slaveAddress(0),
   _functionCode(0),
-  _address(0),
   _byteCount(0) {}
 
-  uint16_t ModbusRequest::getAddress() {
-  return _address;
+uint8_t ModbusRequest::getSlaveAddress() {
+  return _slaveAddress;
 }
 
-ModbusRequest02::ModbusRequest02(uint8_t slaveAddress, uint16_t address, uint16_t numberCoils) :
+uint8_t ModbusRequest::getFunctionCode() {
+  return _functionCode;
+}
+
+ModbusRequest02::ModbusRequest02(uint8_t slaveAddress, uint16_t address, uint16_t numberCoils, uint32_t token) :
   ModbusRequest(8) {
   _slaveAddress = slaveAddress;
   _functionCode = esp32Modbus::READ_DISCR_INPUT;
-  _address = address;
   _byteCount = numberCoils / 8 + 1;
+  _token = token;
   add(_slaveAddress);
   add(_functionCode);
-  add(high(_address));
-  add(low(_address));
+  add(high(address));
+  add(low(address));
   add(high(numberCoils));
   add(low(numberCoils));
   uint16_t CRC = CRC16(_buffer, 6);
   add(low(CRC));
   add(high(CRC));
 }
 
-size_t ModbusRequest02::responseLength() {
-  return 5 + _byteCount;
-}
-
-ModbusRequest03::ModbusRequest03(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters) :
+ModbusRequest03::ModbusRequest03(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters, uint32_t token) :
   ModbusRequest(12) {
   _slaveAddress = slaveAddress;
   _functionCode = esp32Modbus::READ_HOLD_REGISTER;
-  _address = address;
   _byteCount = numberRegisters * 2;  // register is 2 bytes wide
+  _token = token;
   add(_slaveAddress);
   add(_functionCode);
-  add(high(_address));
-  add(low(_address));
+  add(high(address));
+  add(low(address));
   add(high(numberRegisters));
   add(low(numberRegisters));
   uint16_t CRC = CRC16(_buffer, 6);
   add(low(CRC));
   add(high(CRC));
 }
 
-size_t ModbusRequest03::responseLength() {
-  return 5 + _byteCount;
-}
-
-ModbusRequest04::ModbusRequest04(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters) :
+ModbusRequest04::ModbusRequest04(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters, uint32_t token) :
   ModbusRequest(8) {
   _slaveAddress = slaveAddress;
   _functionCode = esp32Modbus::READ_INPUT_REGISTER;
-  _address = address;
   _byteCount = numberRegisters * 2;  // register is 2 bytes wide
+  _token = token;
   add(_slaveAddress);
   add(_functionCode);
-  add(high(_address));
-  add(low(_address));
+  add(high(address));
+  add(low(address));
   add(high(numberRegisters));
   add(low(numberRegisters));
   uint16_t CRC = CRC16(_buffer, 6);
   add(low(CRC));
   add(high(CRC));
 }
 
-size_t ModbusRequest04::responseLength() {
-  // slaveAddress (1) + functionCode (1) + byteCount (1) + length x 2 + CRC (2)
-  return 5 + _byteCount;
-}
-
-ModbusRequest06::ModbusRequest06(uint8_t slaveAddress, uint16_t address, uint16_t data) :
+ModbusRequest06::ModbusRequest06(uint8_t slaveAddress, uint16_t address, uint16_t data, uint32_t token) :
   ModbusRequest(8) {
   _slaveAddress = slaveAddress;
   _functionCode = esp32Modbus::WRITE_HOLD_REGISTER;
-  _address = address;
   _byteCount = 2;  // 1 register is 2 bytes wide
+  _token = token;
   add(_slaveAddress);
   add(_functionCode);
-  add(high(_address));
-  add(low(_address));
+  add(high(address));
+  add(low(address));
   add(high(data));
   add(low(data));
   uint16_t CRC = CRC16(_buffer, 6);
   add(low(CRC));
   add(high(CRC));
 }
 
-size_t ModbusRequest06::responseLength() {
-  return 8;
-}
-
-ModbusRequest16::ModbusRequest16(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters, uint8_t* data) :
+ModbusRequest16::ModbusRequest16(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters, uint8_t* data, uint32_t token) :
   ModbusRequest(9 + (numberRegisters * 2)) {
   _slaveAddress = slaveAddress;
   _functionCode = esp32Modbus::WRITE_MULT_REGISTERS;
-  _address = address;
   _byteCount = numberRegisters * 2;  // register is 2 bytes wide
+  _token = token;
   add(_slaveAddress);
   add(_functionCode);
-  add(high(_address));
-  add(low(_address));
+  add(high(address));
+  add(low(address));
   add(high(numberRegisters));
   add(low(numberRegisters));
   add(_byteCount);
@@ -244,39 +235,40 @@ ModbusRequest16::ModbusRequest16(uint8_t slaveAddress, uint16_t address, uint16_
   add(high(CRC));
 }
 
-size_t ModbusRequest16::responseLength() {
-  return 8;
+ModbusRequestRaw::ModbusRequestRaw(uint8_t slaveAddress, uint8_t functionCode, uint16_t dataLength, uint8_t* data, uint32_t token) :
+  ModbusRequest(dataLength + 4) {
+  _slaveAddress = slaveAddress;
+  _functionCode = functionCode;
+  _byteCount = dataLength + 2;
+  _token = token;
+  add(_slaveAddress);
+  add(_functionCode);
+  for (int i = 0; i < dataLength; i++) {
+      add(data[i]);
+    }
+  uint16_t CRC = CRC16(_buffer, _byteCount);
+  add(low(CRC));
+  add(high(CRC));
 }
 
 ModbusResponse::ModbusResponse(uint8_t length, ModbusRequest* request) :
   ModbusMessage(length),
   _request(request),
-  _error(esp32Modbus::SUCCES) {}
-
-bool ModbusResponse::isComplete() {
-  if (_buffer[1] > 0x80 && _index == 5) {  // 5: slaveAddress(1), errorCode(1), CRC(2) + indexed
-    return true;
-  }
-  if (_index == _request->responseLength()) return true;
-  return false;
-}
+  _error(esp32Modbus::SUCCES) { _token = request->getToken(); }
 
 bool ModbusResponse::isSucces() {
-  if (!isComplete()) {
-    _error = esp32Modbus::TIMEOUT;
-  } else if (_buffer[1] > 0x80) {
+  if (_buffer[1] > 0x80) {
     _error = static_cast<esp32Modbus::Error>(_buffer[2]);
   } else if (!checkCRC()) {
     _error = esp32Modbus::CRC_ERROR;
+  } else if (_buffer[0] != _request->getSlaveAddress()) {
+    _error = esp32Modbus::INVALID_SLAVE;
   // TODO(bertmelis): add other checks
-  } else {
-    _error = esp32Modbus::SUCCES;
   }
   if (_error == esp32Modbus::SUCCES) {
     return true;
-  } else {
-    return false;
   }
+  return false;
 }
 
 bool ModbusResponse::checkCRC() {
@@ -296,14 +288,50 @@ uint8_t ModbusResponse::getSlaveAddress() {
   return _buffer[0];
 }
 
-esp32Modbus::FunctionCode ModbusResponse::getFunctionCode() {
-  return static_cast<esp32Modbus::FunctionCode>(_buffer[1]);
+uint8_t ModbusResponse::getFunctionCode() {
+  return _buffer[1];
 }
 
 uint8_t* ModbusResponse::getData() {
-  return &_buffer[3];
+  uint8_t fc = _request->getFunctionCode();
+  if (fc == 0x01 || fc == 0x02 || fc == 0x03 || fc == 0x04) {
+    return &_buffer[3];
+  } else {
+    return &_buffer[2];
+  }
 }
 
 uint8_t ModbusResponse::getByteCount() {
-  return _buffer[2];
+  uint8_t fc = _request->getFunctionCode();
+  if (fc == 0x01 || fc == 0x02 || fc == 0x03 || fc == 0x04) {
+    return _buffer[2];
+  } else {
+    return _index - 2;
+  }
+}
+
+void ModbusResponse::setErrorResponse(uint8_t errorCode) {
+  if (_length != 5) {
+    delete _buffer;
+    _buffer = new uint8_t[5];
+    _length = 5;
+  }
+  _index = 0;
+  _error = static_cast<esp32Modbus::Error>(errorCode);
+  add(_request->getSlaveAddress());
+  add(_request->getFunctionCode() | 0x80);
+  add(errorCode);
+  uint16_t CRC = CRC16(_buffer, 3);
+  add(low(CRC));
+  add(high(CRC));
+}
+
+void ModbusResponse::setData(uint16_t dataLength, uint8_t *data) {
+  if (_length != dataLength) {
+    delete _buffer;
+    _buffer = new uint8_t[dataLength];
+    _length = dataLength;
+  }
+  _index = dataLength;
+  memcpy(_buffer, data, dataLength);
 }

src/ModbusMessage.h

@@ -27,6 +27,7 @@ SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 
 #include <stdint.h>  // for uint*_t
 #include <stddef.h>  // for size_t
+#include <cstring>  // for memcpy
 
 #include "esp32ModbusTypeDefs.h"
 
@@ -37,77 +38,80 @@ class ModbusMessage {
   virtual ~ModbusMessage();
   uint8_t* getMessage();
   uint8_t getSize();
+  uint32_t getToken();
   void add(uint8_t value);
 
  protected:
   explicit ModbusMessage(uint8_t length);
   uint8_t* _buffer;
   uint8_t _length;
   uint8_t _index;
+  uint32_t _token;
 };
 
 class ModbusResponse;  // forward declare for use in ModbusRequest
 
 class ModbusRequest : public ModbusMessage {
  public:
-  virtual size_t responseLength() = 0;
-  uint16_t getAddress();
+  uint8_t getFunctionCode();
+  uint8_t getSlaveAddress();
 
  protected:
   explicit ModbusRequest(uint8_t length);
   uint8_t _slaveAddress;
   uint8_t _functionCode;
-  uint16_t _address;
   uint16_t _byteCount;
 };
 
 // read discrete coils
 class ModbusRequest02 : public ModbusRequest {
  public:
-  explicit ModbusRequest02(uint8_t slaveAddress, uint16_t address, uint16_t numberCoils);
-  size_t responseLength();
+  explicit ModbusRequest02(uint8_t slaveAddress, uint16_t address, uint16_t numberCoils, uint32_t token = 0);
 };
 
 // read holding registers
 class ModbusRequest03 : public ModbusRequest {
  public:
-  explicit ModbusRequest03(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters);
-  size_t responseLength();
+  explicit ModbusRequest03(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters, uint32_t token = 0);
 };
 
 // read input registers
 class ModbusRequest04 : public ModbusRequest {
  public:
-  explicit ModbusRequest04(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters);
-  size_t responseLength();
+  explicit ModbusRequest04(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters, uint32_t token = 0);
 };
 
 // write single holding registers
 class ModbusRequest06 : public ModbusRequest {
  public:
-  explicit ModbusRequest06(uint8_t slaveAddress, uint16_t address, uint16_t data);
-  size_t responseLength();
+  explicit ModbusRequest06(uint8_t slaveAddress, uint16_t address, uint16_t data, uint32_t token = 0);
 };
 
 // write multiple holding registers
 class ModbusRequest16 : public ModbusRequest {
  public:
-  explicit ModbusRequest16(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters, uint8_t* data);
-  size_t responseLength();
+  explicit ModbusRequest16(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters, uint8_t* data, uint32_t token = 0);
+};
+
+// "raw" request based on a request packet obtained as is.
+class ModbusRequestRaw : public ModbusRequest {
+ public:
+  explicit ModbusRequestRaw(uint8_t slaveAddress, uint8_t functionCode, uint16_t dataLength, uint8_t* data, uint32_t token = 0);
 };
 
 class ModbusResponse : public ModbusMessage {
  public:
   explicit ModbusResponse(uint8_t length, ModbusRequest* request);
-  bool isComplete();
   bool isSucces();
   bool checkCRC();
   esp32Modbus::Error getError() const;
 
   uint8_t getSlaveAddress();
-  esp32Modbus::FunctionCode getFunctionCode();
+  uint8_t getFunctionCode();
   uint8_t* getData();
   uint8_t getByteCount();
+  void setErrorResponse(uint8_t errorCode);
+  void setData(uint16_t dataLength, uint8_t *data);
 
  private:
   ModbusRequest* _request;

src/esp32ModbusRTU.cpp

@@ -31,11 +31,15 @@ using namespace esp32ModbusRTUInternals;  // NOLINT
 esp32ModbusRTU::esp32ModbusRTU(HardwareSerial* serial, int8_t rtsPin) :
   TimeOutValue(TIMEOUT_MS),
   _serial(serial),
-  _lastMillis(0),
+  _lastMicros(0),
   _interval(0),
   _rtsPin(rtsPin),
   _task(nullptr),
-  _queue(nullptr)  {
+  _queue(nullptr),
+  _onData(nullptr),
+  _onError(nullptr),
+  _onDataToken(nullptr),
+  _onErrorToken(nullptr)  {
     _queue = xQueueCreate(QUEUE_SIZE, sizeof(ModbusRequest*));
 }
 
@@ -51,31 +55,41 @@ void esp32ModbusRTU::begin(int coreID /* = -1 */) {
   }
   xTaskCreatePinnedToCore((TaskFunction_t)&_handleConnection, "esp32ModbusRTU", 4096, this, 5, &_task, coreID >= 0 ? coreID : NULL);
   // silent interval is at least 3.5x character time
-  _interval = 40000 / _serial->baudRate();  // 4 * 1000 * 10 / baud
-  if (_interval == 0) _interval = 1;  // minimum of 1msec interval
+  // _interval = 35000000UL / _serial->baudRate();  // 3.5 * 10 bits * 1000 µs * 1000 ms / baud
+  _interval = 40000000UL / _serial->baudRate();  // 4 * 10 bits * 1000 µs * 1000 ms / baud
+
+  // The following is okay for sending at any baud rate, but problematic at receiving with baud rates above 35000,
+  // since the calculated interval will be below 1000µs!
+  // f.i. 115200bd ==> interval=304µs
+  if (_interval < 1000) _interval = 1000;  // minimum of 1msec interval
 }
 
-bool esp32ModbusRTU::readDiscreteInputs(uint8_t slaveAddress, uint16_t address, uint16_t numberCoils) {
-  ModbusRequest* request = new ModbusRequest02(slaveAddress, address, numberCoils);
+bool esp32ModbusRTU::readDiscreteInputs(uint8_t slaveAddress, uint16_t address, uint16_t numberCoils, uint32_t token) {
+  ModbusRequest* request = new ModbusRequest02(slaveAddress, address, numberCoils, token);
+  return _addToQueue(request);
+}
+bool esp32ModbusRTU::readHoldingRegisters(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters, uint32_t token) {
+  ModbusRequest* request = new ModbusRequest03(slaveAddress, address, numberRegisters, token);
   return _addToQueue(request);
 }
-bool esp32ModbusRTU::readHoldingRegisters(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters) {
-  ModbusRequest* request = new ModbusRequest03(slaveAddress, address, numberRegisters);
+
+bool esp32ModbusRTU::readInputRegisters(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters, uint32_t token) {
+  ModbusRequest* request = new ModbusRequest04(slaveAddress, address, numberRegisters, token);
   return _addToQueue(request);
 }
 
-bool esp32ModbusRTU::readInputRegisters(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters) {
-  ModbusRequest* request = new ModbusRequest04(slaveAddress, address, numberRegisters);
+bool esp32ModbusRTU::writeSingleHoldingRegister(uint8_t slaveAddress, uint16_t address, uint16_t data, uint32_t token) {
+  ModbusRequest* request = new ModbusRequest06(slaveAddress, address, data, token);
   return _addToQueue(request);
 }
 
-bool esp32ModbusRTU::writeSingleHoldingRegister(uint8_t slaveAddress, uint16_t address, uint16_t data) {
-  ModbusRequest* request = new ModbusRequest06(slaveAddress, address, data);
+bool esp32ModbusRTU::writeMultHoldingRegisters(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters, uint8_t* data, uint32_t token) {
+  ModbusRequest* request = new ModbusRequest16(slaveAddress, address, numberRegisters, data, token);
   return _addToQueue(request);
 }
 
-bool esp32ModbusRTU::writeMultHoldingRegisters(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters, uint8_t* data) {
-  ModbusRequest* request = new ModbusRequest16(slaveAddress, address, numberRegisters, data);
+bool esp32ModbusRTU::rawRequest(uint8_t slaveAddress, uint8_t functionCode, uint16_t dataLength, uint8_t* data, uint32_t token) {
+  ModbusRequest* request = new ModbusRequestRaw(slaveAddress, functionCode, dataLength, data, token);
   return _addToQueue(request);
 }
 
@@ -87,6 +101,14 @@ void esp32ModbusRTU::onError(esp32Modbus::MBRTUOnError handler) {
   _onError = handler;
 }
 
+void esp32ModbusRTU::onDataToken(esp32Modbus::MBRTUOnDataToken handler) {
+  _onDataToken = handler;
+}
+
+void esp32ModbusRTU::onErrorToken(esp32Modbus::MBRTUOnErrorToken handler) {
+  _onErrorToken = handler;
+}
+
 bool esp32ModbusRTU::_addToQueue(ModbusRequest* request) {
   if (!request) {
     return false;
@@ -105,9 +127,26 @@ void esp32ModbusRTU::_handleConnection(esp32ModbusRTU* instance) {
       instance->_send(request->getMessage(), request->getSize());
       ModbusResponse* response = instance->_receive(request);
       if (response->isSucces()) {
-        if (instance->_onData) instance->_onData(response->getSlaveAddress(), response->getFunctionCode(), request->getAddress(), response->getData(), response->getByteCount());
+        // if the non-token onData handler is set, call it
+        if (instance->_onData)
+          instance->_onData(
+            response->getSlaveAddress(),
+            response->getFunctionCode(),
+            response->getData(),
+            response->getByteCount());
+        // else, if the token onData handler is set, call that
+        else if (instance->_onDataToken)
+          instance->_onDataToken(
+            response->getSlaveAddress(),
+            response->getFunctionCode(),
+            response->getData(),
+            response->getByteCount(),
+            response->getToken());
       } else {
+        // Same for error responses. non-token onError set?
         if (instance->_onError) instance->_onError(response->getError());
+        // No, but token onError instead?
+        else if (instance->_onErrorToken) instance->_onErrorToken(response->getError(), response->getToken());
       }
       delete request;  // object created in public methods
       delete response;  // object created in _receive()
@@ -116,14 +155,14 @@ void esp32ModbusRTU::_handleConnection(esp32ModbusRTU* instance) {
 }
 
 void esp32ModbusRTU::_send(uint8_t* data, uint8_t length) {
-  while (millis() - _lastMillis < _interval) delay(1);  // respect _interval
+  while (micros() - _lastMicros < _interval) delayMicroseconds(1);  // respect _interval
   // Toggle rtsPin, if necessary
   if (_rtsPin >= 0) digitalWrite(_rtsPin, HIGH);
   _serial->write(data, length);
   _serial->flush();
   // Toggle rtsPin, if necessary
   if (_rtsPin >= 0) digitalWrite(_rtsPin, LOW);
-  _lastMillis = millis();
+  _lastMicros = micros();
 }
 
 // Adjust timeout on MODBUS - some slaves require longer/allow for shorter times
@@ -132,20 +171,115 @@ void esp32ModbusRTU::setTimeOutValue(uint32_t tov) {
 }
 
 ModbusResponse* esp32ModbusRTU::_receive(ModbusRequest* request) {
-  ModbusResponse* response = new ModbusResponse(request->responseLength(), request);
-  while (true) {
-    while (_serial->available()) {
-      response->add(_serial->read());
-    }
-    if (response->isComplete()) {
-      _lastMillis = millis();
+  // Allocate initial buffer size
+  const uint16_t BUFBLOCKSIZE(128);
+  uint8_t *buffer = new uint8_t[BUFBLOCKSIZE];
+  uint8_t bufferBlocks = 1;
+
+  // Index into buffer
+  register uint16_t bufferPtr = 0;
+
+  // State machine states
+  enum STATES : uint8_t { WAIT_INTERVAL = 0, WAIT_DATA, IN_PACKET, DATA_READ, ERROR_EXIT, FINISHED };
+  register STATES state = WAIT_INTERVAL;
+
+  // Timeout tracker
+  uint32_t TimeOut = millis();
+
+  // Error code
+  esp32Modbus::Error errorCode = esp32Modbus::SUCCES;
+
+  // Return data object
+  ModbusResponse* response = nullptr;
+
+  while (state != FINISHED) {
+    switch (state) {
+    // WAIT_INTERVAL: spend the remainder of the bus quiet time waiting
+    case WAIT_INTERVAL:
+      // Time passed?
+      if (micros() - _lastMicros >= _interval) {
+        // Yes, proceed to reading data
+        state = WAIT_DATA;
+      } else {
+        // No, wait a little longer
+        delayMicroseconds(1);
+      }
       break;
-    }
-    if (millis() - _lastMillis > TimeOutValue) {
+    // WAIT_DATA: await first data byte, but watch timeout
+    case WAIT_DATA:
+      if (_serial->available()) {
+        state = IN_PACKET;
+        _lastMicros = micros();
+      } else if (millis() - TimeOut >= TimeOutValue) {
+        errorCode = esp32Modbus::TIMEOUT;
+        state = ERROR_EXIT;
+      }
+      break;
+    // IN_PACKET: read data until a gap of at least _interval time passed without another byte arriving
+    case IN_PACKET:
+      // Data waiting and space left in buffer?
+      while (_serial->available()) {
+        // Yes. Catch the byte
+        buffer[bufferPtr++] = _serial->read();
+        // Buffer full?
+        if (bufferPtr >= bufferBlocks * BUFBLOCKSIZE) {
+          // Yes. Extend it by another block
+          bufferBlocks++;
+          uint8_t *temp = new uint8_t[bufferBlocks * BUFBLOCKSIZE];
+          memcpy(temp, buffer, (bufferBlocks - 1) * BUFBLOCKSIZE);
+          // Use intermediate pointer temp2 to keep cppcheck happy
+          delete[] buffer;
+          buffer = temp;
+        }
+        // Rewind timer
+        _lastMicros = micros();
+      }
+      // Gap of at least _interval micro seconds passed without data?
+      // ***********************************************
+      // Important notice!
+      // Due to an implementation decision done in the ESP32 Arduino core code,
+      // the correct time to detect a gap of _interval µs is not effective, as
+      // the core FIFO handling takes much longer than that.
+      //
+      // Workaround: uncomment the following line to wait for 16ms(!) for the handling to finish:
+      if (micros() - _lastMicros >= 16000) {
+      //
+      // Alternate solution: is to modify the uartEnableInterrupt() function in
+      // the core implementation file 'esp32-hal-uart.c', to have the line
+      //    'uart->dev->conf1.rxfifo_full_thrhd = 1; // 112;'
+      // This will change the number of bytes received to trigger the copy interrupt
+      // from 112 (as is implemented in the core) to 1, effectively firing the interrupt
+      // for any single byte.
+      // Then you may uncomment the line below instead:
+      // if (micros() - _lastMicros >= _interval) {
+      //
+        state = DATA_READ;
+      }
+      break;
+    // DATA_READ: successfully gathered some data. Prepare return object.
+    case DATA_READ:
+      // Allocate response object
+      response = new ModbusResponse(bufferPtr, request);
+      // Move gathered data into it
+      response->setData(bufferPtr, buffer);
+      state = FINISHED;
+      break;
+    // ERROR_EXIT: We had a timeout. Prepare error return object
+    case ERROR_EXIT:
+      response = new ModbusResponse(5, request);
+      response->setErrorResponse(errorCode);
+      state = FINISHED;
+      break;
+    // FINISHED: we are done, keep the compiler happy by pseudo-treating it.
+    case FINISHED:
       break;
     }
-    delay(1);  // take care of watchdog
   }
+
+  // Deallocate buffer
+  delete[] buffer;
+  _lastMicros = micros();
+
   return response;
 }
 

src/esp32ModbusRTU.h

@@ -52,13 +52,16 @@ class esp32ModbusRTU {
   explicit esp32ModbusRTU(HardwareSerial* serial, int8_t rtsPin = -1);
   ~esp32ModbusRTU();
   void begin(int coreID = -1);
-  bool readDiscreteInputs(uint8_t slaveAddress, uint16_t address, uint16_t numberCoils);
-  bool readHoldingRegisters(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters);
-  bool readInputRegisters(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters);
-  bool writeSingleHoldingRegister(uint8_t slaveAddress, uint16_t address, uint16_t data);
-  bool writeMultHoldingRegisters(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters, uint8_t* data);
+  bool readDiscreteInputs(uint8_t slaveAddress, uint16_t address, uint16_t numberCoils, uint32_t token = 0);
+  bool readHoldingRegisters(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters, uint32_t token = 0);
+  bool readInputRegisters(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters, uint32_t token = 0);
+  bool writeSingleHoldingRegister(uint8_t slaveAddress, uint16_t address, uint16_t data, uint32_t token = 0);
+  bool writeMultHoldingRegisters(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters, uint8_t* data, uint32_t token = 0);
+  bool rawRequest(uint8_t slaveAddress, uint8_t functionCode, uint16_t dataLength, uint8_t *data, uint32_t token = 0);
   void onData(esp32Modbus::MBRTUOnData handler);
   void onError(esp32Modbus::MBRTUOnError handler);
+  void onDataToken(esp32Modbus::MBRTUOnDataToken handler);
+  void onErrorToken(esp32Modbus::MBRTUOnErrorToken handler);
   void setTimeOutValue(uint32_t tov);
 
  private:
@@ -70,13 +73,15 @@ class esp32ModbusRTU {
  private:
   uint32_t TimeOutValue;
   HardwareSerial* _serial;
-  uint32_t _lastMillis;
+  uint32_t _lastMicros;
   uint32_t _interval;
   int8_t _rtsPin;
   TaskHandle_t _task;
   QueueHandle_t _queue;
   esp32Modbus::MBRTUOnData _onData;
   esp32Modbus::MBRTUOnError _onError;
+  esp32Modbus::MBRTUOnDataToken _onDataToken;
+  esp32Modbus::MBRTUOnErrorToken _onErrorToken;
 };
 
 #endif

src/esp32ModbusTypeDefs.h

@@ -60,10 +60,10 @@ enum Error : uint8_t {
   COMM_ERROR            = 0xE4  // general communication error
 };
 
-typedef std::function<void(uint16_t, uint8_t, esp32Modbus::FunctionCode, uint8_t*, uint16_t)> MBTCPOnData;
-typedef std::function<void(uint8_t, esp32Modbus::FunctionCode, uint16_t, uint8_t*, uint16_t)> MBRTUOnData;
-typedef std::function<void(uint16_t, esp32Modbus::Error)> MBTCPOnError;
+typedef std::function<void(uint8_t, uint8_t, uint8_t*, uint16_t)> MBRTUOnData;
 typedef std::function<void(esp32Modbus::Error)> MBRTUOnError;
+typedef std::function<void(uint8_t, uint8_t, uint8_t*, uint16_t, uint32_t)> MBRTUOnDataToken;
+typedef std::function<void(esp32Modbus::Error, uint32_t)> MBRTUOnErrorToken;
 
 }  // namespace esp32Modbus
 

tests/Test_ModbusMessage.cpp

@@ -17,7 +17,7 @@ TEST_CASE("Read input registers", "[FC04]") {
   REQUIRE(request->getSize() == sizeof(stdMessage));
   REQUIRE_THAT(request->getMessage(), ByteArrayEqual(stdMessage, sizeof(stdMessage)));
 
-  esp32ModbusRTUInternals::ModbusResponse* response = new esp32ModbusRTUInternals::ModbusResponse(request->responseLength(), request);
+  esp32ModbusRTUInternals::ModbusResponse* response = new esp32ModbusRTUInternals::ModbusResponse(7, request);
 
   SECTION("normal response") {
     for (uint8_t i = 0; i < sizeof(stdResponse); ++i) {
@@ -48,7 +48,7 @@ TEST_CASE("Write multiple holding registers", "[FC16]") {
   REQUIRE(request->getSize() == sizeof(stdMessage));
   REQUIRE_THAT(request->getMessage(), ByteArrayEqual(stdMessage, sizeof(stdMessage)));
 
-  esp32ModbusRTUInternals::ModbusResponse* response = new esp32ModbusRTUInternals::ModbusResponse(request->responseLength(), request);
+  esp32ModbusRTUInternals::ModbusResponse* response = new esp32ModbusRTUInternals::ModbusResponse(8, request);
 
   SECTION("normal response") {
     for (uint8_t i = 0; i < sizeof(stdResponse); ++i) {