397 lines
12 KiB
C++
397 lines
12 KiB
C++
#include "ModbusRTUMaster.h"
|
|
|
|
ModbusRTUMaster::ModbusRTUMaster(HardwareSerial& serial, uint8_t dePin) {
|
|
_hardwareSerial = &serial;
|
|
/*
|
|
#ifdef __AVR__
|
|
_softwareSerial = 0;
|
|
#endif
|
|
*/
|
|
#ifdef HAVE_CDCSERIAL
|
|
_usbSerial = 0;
|
|
#endif
|
|
_serial = &serial;
|
|
_dePin = dePin;
|
|
}
|
|
|
|
/*
|
|
#ifdef __AVR__
|
|
ModbusRTUMaster::ModbusRTUMaster(SoftwareSerial& serial, uint8_t dePin) {
|
|
_hardwareSerial = 0;
|
|
_softwareSerial = &serial;
|
|
#ifdef HAVE_CDCSERIAL
|
|
_usbSerial = 0;
|
|
#endif
|
|
_serial = &serial;
|
|
_dePin = dePin;
|
|
}
|
|
#endif
|
|
*/
|
|
|
|
#ifdef HAVE_CDCSERIAL
|
|
ModbusRTUMaster::ModbusRTUMaster(Serial_& serial, uint8_t dePin) {
|
|
_hardwareSerial = 0;
|
|
/*
|
|
#ifdef __AVR__
|
|
_softwareSerial = 0;
|
|
#endif
|
|
*/
|
|
_usbSerial = &serial;
|
|
_serial = &serial;
|
|
_dePin = dePin;
|
|
}
|
|
#endif
|
|
|
|
void ModbusRTUMaster::setTimeout(unsigned long timeout) {
|
|
_responseTimeout = timeout;
|
|
}
|
|
|
|
#ifdef ESP32
|
|
void ModbusRTUMaster::begin(unsigned long baud, uint32_t config, int8_t rxPin, int8_t txPin, bool invert) {
|
|
if (_hardwareSerial) {
|
|
_calculateTimeouts(baud, config);
|
|
_hardwareSerial->begin(baud, config, rxPin, txPin, invert);
|
|
}
|
|
#ifdef HAVE_CDCSERIAL
|
|
else if (_usbSerial) {
|
|
_calculateTimeouts(baud, config);
|
|
_usbSerial->begin(baud, config);
|
|
while (!_usbSerial);
|
|
}
|
|
#endif
|
|
if (_dePin != NO_DE_PIN) {
|
|
pinMode(_dePin, OUTPUT);
|
|
digitalWrite(_dePin, LOW);
|
|
}
|
|
_clearRxBuffer();
|
|
}
|
|
#else
|
|
void ModbusRTUMaster::begin(unsigned long baud, uint32_t config) {
|
|
if (config != SERIAL_8N1 && config != SERIAL_8E1 && config != SERIAL_8O1 && config != SERIAL_8N2 && config != SERIAL_8E2 && config != SERIAL_8O2) config = SERIAL_8N1;
|
|
if (_hardwareSerial) {
|
|
_calculateTimeouts(baud, config);
|
|
_hardwareSerial->begin(baud, config);
|
|
}
|
|
/*
|
|
#ifdef __AVR__
|
|
else if (_softwareSerial) {
|
|
_calculateTimeouts(baud, SERIAL_8N1);
|
|
_softwareSerial->begin(baud);
|
|
}
|
|
#endif
|
|
*/
|
|
#ifdef HAVE_CDCSERIAL
|
|
else if (_usbSerial) {
|
|
_calculateTimeouts(baud, config);
|
|
_usbSerial->begin(baud, config);
|
|
while (!_usbSerial);
|
|
}
|
|
#endif
|
|
if (_dePin != NO_DE_PIN) {
|
|
pinMode(_dePin, OUTPUT);
|
|
digitalWrite(_dePin, LOW);
|
|
}
|
|
_clearRxBuffer();
|
|
}
|
|
#endif
|
|
|
|
|
|
bool ModbusRTUMaster::readCoils(uint8_t id, uint16_t startAddress, bool *buf, uint16_t quantity) {
|
|
const uint8_t functionCode = 1;
|
|
uint8_t byteCount = _div8RndUp(quantity);
|
|
if (id < 1 || id > 247 || !buf || quantity == 0 || quantity > 2000) return false;
|
|
_buf[0] = id;
|
|
_buf[1] = functionCode;
|
|
_buf[2] = highByte(startAddress);
|
|
_buf[3] = lowByte(startAddress);
|
|
_buf[4] = highByte(quantity);
|
|
_buf[5] = lowByte(quantity);
|
|
_writeRequest(6);
|
|
uint16_t responseLength = _readResponse(id, functionCode);
|
|
if (responseLength != (uint16_t)(3 + byteCount) || _buf[2] != byteCount) return false;
|
|
for (uint16_t i = 0; i < quantity; i++) {
|
|
buf[i] = bitRead(_buf[3 + (i >> 3)], i & 7);
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool ModbusRTUMaster::readDiscreteInputs(uint8_t id, uint16_t startAddress, bool *buf, uint16_t quantity) {
|
|
const uint8_t functionCode = 2;
|
|
uint8_t byteCount = _div8RndUp(quantity);
|
|
if (id < 1 || id > 247 || !buf || quantity == 0 || quantity > 2000) return false;
|
|
_buf[0] = id;
|
|
_buf[1] = functionCode;
|
|
_buf[2] = highByte(startAddress);
|
|
_buf[3] = lowByte(startAddress);
|
|
_buf[4] = highByte(quantity);
|
|
_buf[5] = lowByte(quantity);
|
|
_writeRequest(6);
|
|
uint16_t responseLength = _readResponse(id, functionCode);
|
|
if (responseLength != (uint16_t)(3 + byteCount) || _buf[2] != byteCount) return false;
|
|
for (uint16_t i = 0; i < quantity; i++) {
|
|
buf[i] = bitRead(_buf[3 + (i >> 3)], i & 7);
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool ModbusRTUMaster::readHoldingRegisters(uint8_t id, uint16_t startAddress, uint16_t *buf, uint16_t quantity) {
|
|
const uint8_t functionCode = 3;
|
|
uint8_t byteCount = quantity * 2;
|
|
if (id < 1 || id > 247 || !buf || quantity == 0 || quantity > 125) return false;
|
|
_buf[0] = id;
|
|
_buf[1] = functionCode;
|
|
_buf[2] = highByte(startAddress);
|
|
_buf[3] = lowByte(startAddress);
|
|
_buf[4] = highByte(quantity);
|
|
_buf[5] = lowByte(quantity);
|
|
_writeRequest(6);
|
|
uint16_t responseLength = _readResponse(id, functionCode);
|
|
if (responseLength != (uint16_t)(3 + byteCount) || _buf[2] != byteCount) return false;
|
|
for (uint16_t i = 0; i < quantity; i++) {
|
|
buf[i] = _bytesToWord(_buf[3 + (i * 2)], _buf[4 + (i * 2)]);
|
|
}
|
|
return true;
|
|
}
|
|
|
|
void ModbusRTUMaster::begin_readHoldingRegisters(uint8_t id, uint16_t startAddress, uint16_t quantity) {
|
|
const uint8_t functionCode = 3;
|
|
uint8_t byteCount = quantity * 2;
|
|
if (id < 1 || id > 247 || quantity == 0 || quantity > 125) return false;
|
|
_buf[0] = id;
|
|
_buf[1] = functionCode;
|
|
_buf[2] = highByte(startAddress);
|
|
_buf[3] = lowByte(startAddress);
|
|
_buf[4] = highByte(quantity);
|
|
_buf[5] = lowByte(quantity);
|
|
_writeRequest(6);
|
|
}
|
|
|
|
bool ModbusRTUMaster::readHoldingRegisters_end(uint8_t id, uint16_t *buf, uint16_t quantity) {
|
|
const uint8_t functionCode = 3;
|
|
uint8_t byteCount = quantity * 2;
|
|
uint16_t responseLength = _readResponse(id, functionCode);
|
|
if (responseLength != (uint16_t)(3 + byteCount) || _buf[2] != byteCount) return false;
|
|
for (uint16_t i = 0; i < quantity; i++) {
|
|
buf[i] = _bytesToWord(_buf[3 + (i * 2)], _buf[4 + (i * 2)]);
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool ModbusRTUMaster::readInputRegisters(uint8_t id, uint16_t startAddress, uint16_t *buf, uint16_t quantity) {
|
|
const uint8_t functionCode = 4;
|
|
uint8_t byteCount = quantity * 2;
|
|
if (id < 1 || id > 247 || !buf || quantity == 0 || quantity > 125) return false;
|
|
_buf[0] = id;
|
|
_buf[1] = functionCode;
|
|
_buf[2] = highByte(startAddress);
|
|
_buf[3] = lowByte(startAddress);
|
|
_buf[4] = highByte(quantity);
|
|
_buf[5] = lowByte(quantity);
|
|
_writeRequest(6);
|
|
uint16_t responseLength = _readResponse(id, functionCode);
|
|
if (responseLength != (uint16_t)(3 + byteCount) || _buf[2] != byteCount) return false;
|
|
for (uint16_t i = 0; i < quantity; i++) {
|
|
buf[i] = _bytesToWord(_buf[3 + (i * 2)], _buf[4 + (i * 2)]);
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool ModbusRTUMaster::writeSingleCoil(uint8_t id, uint16_t address, bool value) {
|
|
const uint8_t functionCode = 5;
|
|
if (id > 247) return false;
|
|
_buf[0] = id;
|
|
_buf[1] = functionCode;
|
|
_buf[2] = highByte(address);
|
|
_buf[3] = lowByte(address);
|
|
_buf[4] = value * 255;
|
|
_buf[5] = 0;
|
|
_writeRequest(6);
|
|
if (id == 0) return true;
|
|
uint16_t responseLength = _readResponse(id, functionCode);
|
|
if (responseLength != 6 || _bytesToWord(_buf[2], _buf[3]) != address || _buf[4] != (value * 255) || _buf[5] != 0) return false;
|
|
return true;
|
|
}
|
|
|
|
bool ModbusRTUMaster::writeSingleHoldingRegister(uint8_t id, uint16_t address, uint16_t value) {
|
|
const uint8_t functionCode = 6;
|
|
if (id > 247) return false;
|
|
_buf[0] = id;
|
|
_buf[1] = functionCode;
|
|
_buf[2] = highByte(address);
|
|
_buf[3] = lowByte(address);
|
|
_buf[4] = highByte(value);
|
|
_buf[5] = lowByte(value);
|
|
_writeRequest(6);
|
|
if (id == 0) return true;
|
|
uint16_t responseLength = _readResponse(id, functionCode);
|
|
if (responseLength != 6 || _bytesToWord(_buf[2], _buf[3]) != address || _bytesToWord(_buf[4], _buf[5]) != value) return false;
|
|
return true;
|
|
}
|
|
|
|
bool ModbusRTUMaster::writeMultipleCoils(uint8_t id, uint16_t startAddress, bool *buf, uint16_t quantity) {
|
|
const uint8_t functionCode = 15;
|
|
uint8_t byteCount = _div8RndUp(quantity);
|
|
if (id > 247 || !buf || quantity == 0 || quantity > 1968) return false;
|
|
_buf[0] = id;
|
|
_buf[1] = functionCode;
|
|
_buf[2] = highByte(startAddress);
|
|
_buf[3] = lowByte(startAddress);
|
|
_buf[4] = highByte(quantity);
|
|
_buf[5] = lowByte(quantity);
|
|
_buf[6] = byteCount;
|
|
for (uint16_t i = 0; i < quantity; i++) {
|
|
bitWrite(_buf[7 + (i >> 3)], i & 7, buf[i]);
|
|
}
|
|
for (uint16_t i = quantity; i < (byteCount * 8); i++) {
|
|
bitClear(_buf[7 + (i >> 3)], i & 7);
|
|
}
|
|
_writeRequest(7 + byteCount);
|
|
if (id == 0) return true;
|
|
uint16_t responseLength = _readResponse(id, functionCode);
|
|
if (responseLength != 6 || _bytesToWord(_buf[2], _buf[3]) != startAddress || _bytesToWord(_buf[4], _buf[5]) != quantity) return false;
|
|
return true;
|
|
}
|
|
|
|
bool ModbusRTUMaster::writeMultipleHoldingRegisters(uint8_t id, uint16_t startAddress, uint16_t *buf, uint16_t quantity) {
|
|
const uint8_t functionCode = 16;
|
|
uint8_t byteCount = quantity * 2;
|
|
if (id > 247 || !buf || quantity == 0 || quantity > 123) return false;
|
|
_buf[0] = id;
|
|
_buf[1] = functionCode;
|
|
_buf[2] = highByte(startAddress);
|
|
_buf[3] = lowByte(startAddress);
|
|
_buf[4] = highByte(quantity);
|
|
_buf[5] = lowByte(quantity);
|
|
_buf[6] = byteCount;
|
|
for (uint16_t i = 0; i < quantity; i++) {
|
|
_buf[7 + (i * 2)] = highByte(buf[i]);
|
|
_buf[8 + (i * 2)] = lowByte(buf[i]);
|
|
}
|
|
_writeRequest(7 + byteCount);
|
|
if (id == 0) return true;
|
|
uint16_t responseLength = _readResponse(id, functionCode);
|
|
if (responseLength != 6 || _bytesToWord(_buf[2], _buf[3]) != startAddress || _bytesToWord(_buf[4], _buf[5]) != quantity) return false;
|
|
return true;
|
|
}
|
|
|
|
bool ModbusRTUMaster::getTimeoutFlag() {
|
|
return _timeoutFlag;
|
|
}
|
|
|
|
void ModbusRTUMaster::clearTimeoutFlag() {
|
|
_timeoutFlag = 0;
|
|
}
|
|
|
|
uint8_t ModbusRTUMaster::getExceptionResponse() {
|
|
return _exceptionResponse;
|
|
}
|
|
|
|
void ModbusRTUMaster::clearExceptionResponse() {
|
|
_exceptionResponse = 0;
|
|
}
|
|
|
|
|
|
|
|
void ModbusRTUMaster::_writeRequest(uint8_t len) {
|
|
uint16_t crc = _crc(len);
|
|
_buf[len] = lowByte(crc);
|
|
_buf[len + 1] = highByte(crc);
|
|
if (_dePin != NO_DE_PIN) digitalWrite(_dePin, HIGH);
|
|
_serial->write(_buf, len + 2);
|
|
_serial->flush();
|
|
if (_dePin != NO_DE_PIN) digitalWrite(_dePin, LOW);
|
|
}
|
|
|
|
uint16_t ModbusRTUMaster::_readResponse(uint8_t id, uint8_t functionCode) {
|
|
unsigned long startTime = millis();
|
|
uint16_t numBytes = 0;
|
|
/* issue 241217
|
|
while (!_serial->available()) {
|
|
if (millis() - startTime >= _responseTimeout) {
|
|
_timeoutFlag = true;
|
|
return 0;
|
|
}
|
|
}
|
|
*/
|
|
bool z_filter = false;
|
|
if (_serial->peek() == 0) { // 0, rx
|
|
z_filter = true;
|
|
_serial->read();
|
|
while (!_serial->available()) {
|
|
if (millis() - startTime >= _responseTimeout) {
|
|
_timeoutFlag = true;
|
|
return 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
do {
|
|
if (_serial->available()) {
|
|
startTime = micros();
|
|
_buf[numBytes] = _serial->read();
|
|
numBytes++;
|
|
}
|
|
} while (micros() - startTime <= _charTimeout && numBytes < MODBUS_RTU_MASTER_BUF_SIZE);
|
|
while (micros() - startTime < _frameTimeout);
|
|
|
|
if (z_filter) {
|
|
if (_buf[numBytes - 1] != 0) return 0;
|
|
numBytes -= 1; // 0, rx, 0
|
|
}
|
|
|
|
if (_serial->available() || _buf[0] != id || (_buf[1] != functionCode && _buf[1] != (functionCode + 128)) || _crc(numBytes - 2) != _bytesToWord(_buf[numBytes - 1], _buf[numBytes - 2])) return 0;
|
|
else if (_buf[1] == (functionCode + 128)) {
|
|
_exceptionResponse = _buf[2];
|
|
return 0;
|
|
}
|
|
return (numBytes - 2);
|
|
}
|
|
|
|
void ModbusRTUMaster::_clearRxBuffer() {
|
|
unsigned long startTime = micros();
|
|
do {
|
|
if (_serial->available() > 0) {
|
|
startTime = micros();
|
|
_serial->read();
|
|
}
|
|
} while (micros() - startTime < _frameTimeout);
|
|
}
|
|
|
|
|
|
|
|
void ModbusRTUMaster::_calculateTimeouts(unsigned long baud, uint32_t config) {
|
|
unsigned long bitsPerChar;
|
|
if (config == SERIAL_8E2 || config == SERIAL_8O2) bitsPerChar = 12;
|
|
else if (config == SERIAL_8N2 || config == SERIAL_8E1 || config == SERIAL_8O1) bitsPerChar = 11;
|
|
else bitsPerChar = 10;
|
|
if (baud <= 19200) {
|
|
_charTimeout = (bitsPerChar * 2500000) / baud;
|
|
_frameTimeout = (bitsPerChar * 4500000) / baud;
|
|
}
|
|
else {
|
|
_charTimeout = (bitsPerChar * 1000000) / baud + 750;
|
|
_frameTimeout = (bitsPerChar * 1000000) / baud + 1750;
|
|
}
|
|
}
|
|
|
|
uint16_t ModbusRTUMaster::_crc(uint8_t len) {
|
|
uint16_t value = 0xFFFF;
|
|
for (uint8_t i = 0; i < len; i++) {
|
|
value ^= (uint16_t)_buf[i];
|
|
for (uint8_t j = 0; j < 8; j++) {
|
|
bool lsb = value & 1;
|
|
value >>= 1;
|
|
if (lsb == true) value ^= 0xA001;
|
|
}
|
|
}
|
|
return value;
|
|
}
|
|
|
|
uint16_t ModbusRTUMaster::_div8RndUp(uint16_t value) {
|
|
return (value + 7) >> 3;
|
|
}
|
|
|
|
uint16_t ModbusRTUMaster::_bytesToWord(uint8_t high, uint8_t low) {
|
|
return (high << 8) | low;
|
|
}
|