From 8da50630bdae25e1a12f483912e9dde11ed44439 Mon Sep 17 00:00:00 2001 From: epsilonrt Date: Sat, 27 May 2023 17:20:31 +0200 Subject: [PATCH] fix: Fixes epsilonrt/modbus-arduino#5 This issue was due to a bad management of the dynamic memory. The implementation of the Modbus class has been revised to correct this problem, but also that of writeMultipleCoils() which was not working correctly. The Reading methods have been reviewed and optimized. --- issues/modbus-arduino-Issue5/.gitignore | 5 + issues/modbus-arduino-Issue5/platformio.ini | 71 ++++ issues/modbus-arduino-Issue5/src/main.cpp | 41 ++ library.json | 2 +- library.properties | 2 +- src/Modbus.cpp | 435 +++++++------------- src/Modbus.h | 17 +- 7 files changed, 284 insertions(+), 289 deletions(-) create mode 100644 issues/modbus-arduino-Issue5/.gitignore create mode 100644 issues/modbus-arduino-Issue5/platformio.ini create mode 100644 issues/modbus-arduino-Issue5/src/main.cpp diff --git a/issues/modbus-arduino-Issue5/.gitignore b/issues/modbus-arduino-Issue5/.gitignore new file mode 100644 index 0000000..89cc49c --- /dev/null +++ b/issues/modbus-arduino-Issue5/.gitignore @@ -0,0 +1,5 @@ +.pio +.vscode/.browse.c_cpp.db* +.vscode/c_cpp_properties.json +.vscode/launch.json +.vscode/ipch diff --git a/issues/modbus-arduino-Issue5/platformio.ini b/issues/modbus-arduino-Issue5/platformio.ini new file mode 100644 index 0000000..d962896 --- /dev/null +++ b/issues/modbus-arduino-Issue5/platformio.ini @@ -0,0 +1,71 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + +[platformio] +default_envs = mkrvidor4000 + +[env:teensy41] +platform = teensy +board = teensy41 +framework = arduino +lib_extra_dirs = ../../.. +;lib_deps = epsilonrt/Modbus-Serial@^2.0.3 + +[env:mkrvidor4000] +platform = atmelsam +board = mkrvidor4000 +framework = arduino +lib_extra_dirs = ../../.. +;lib_deps = epsilonrt/Modbus-Serial@^2.0.3 + +[env:mkrvidor4000-debug] +platform = atmelsam +framework = arduino +board = mkrvidor4000 +debug_tool = atmel-ice + + +; Debugger (gdb) support +; https://docs.arduino.cc/tutorials/mkr-wifi-1010/mkr-jlink-setup +; https://gojimmypi.blogspot.com/2018/12/swd-debugging-arduino-mkr-wifi-1010.html + +build_type = debug +lib_extra_dirs = ../../.. +;lib_deps = epsilonrt/Modbus-Serial@^2.0.3 + +; activate Dual USB just as README says +build_flags = + -DDEBUG # Comment out to disable debugging. +;debug_build_flags = -O0 -g2 -ggdb2 +debug_build_flags = -O0 -ggdb3 -g3 + +[env:esp32] +framework = arduino +platform = espressif32 +; change for your board : https://registry.platformio.org/platforms/platformio/espressif32/boards +board = esp32doit-devkit-v1 +;board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. +;lib_deps = epsilonrt/Modbus-Serial@^2.0.3 +;upload_port = COM9 + +[env:esp32-debug] +; https://dzone.com/articles/eclipse-jtag-debugging-the-esp32-with-a-segger-j-l +; https://docs.platformio.org/en/latest/tutorials/espressif32/arduino_debugging_unit_testing.html +; https://community.platformio.org/t/esp32-and-segger-jlink-tip-for-interface-setup-configuration/25964 +framework = arduino +platform = espressif32 +; change for your board : https://registry.platformio.org/platforms/platformio/espressif32/boards +board = esp32doit-devkit-v1 +;board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. +;lib_deps = epsilonrt/Modbus-Serial@^2.0.3 +upload_port = COM1 +debug_tool = jlink \ No newline at end of file diff --git a/issues/modbus-arduino-Issue5/src/main.cpp b/issues/modbus-arduino-Issue5/src/main.cpp new file mode 100644 index 0000000..15f3855 --- /dev/null +++ b/issues/modbus-arduino-Issue5/src/main.cpp @@ -0,0 +1,41 @@ +#include + +// Used Pins +const byte LedPin = LED_BUILTIN; +const byte TxenPin = -1; // -1 disables the feature, change that if you are using an RS485 driver, this pin would be connected to the DE and /RE pins of the driver. + +#define MySerial Serial1 // define serial port used, Serial most of the time, or Serial1, Serial2 ... if available +const unsigned long Baudrate = 38400; + +const byte SlaveId = 10; + +const byte FirstReg = 0; +const byte Lamp1Coil = FirstReg; +const byte NofRegs = 32; + +ModbusSerial mb(MySerial, SlaveId, TxenPin); + +void setup() { + + // MySerial.begin(Baudrate); // works on all boards but the configuration is 8N1 which is incompatible with the MODBUS standard + // prefer the line below instead if possible + MySerial.begin (Baudrate, MB_PARITY_EVEN); + + mb.config(Baudrate); + + for (byte i = 0; i < NofRegs; i++) { + + mb.addHreg(FirstReg + i, i); + mb.addIreg(FirstReg + i, i); + mb.addCoil(FirstReg + i, i % 2); + mb.addIsts(FirstReg + i, (i + 1) % 2); + } + + pinMode(LedPin, OUTPUT); +} + +void loop() { + + mb.task(); + digitalWrite(LedPin, !mb.Coil(Lamp1Coil)); +} diff --git a/library.json b/library.json index 4a6ff77..e58fa60 100644 --- a/library.json +++ b/library.json @@ -1,6 +1,6 @@ { "name": "Modbus-Arduino", - "version": "1.1.2", + "version": "1.2.0", "keywords": "modbus, server, slave, rtu, tcp", "description": "A library that allows your Arduino to communicate via Modbus protocol, acting as a slave. Application layer library (OSI 7), used by all implementations over serial line and TCP/IP.", "homepage": "https://epsilonrt.github.io/modbus-arduino", diff --git a/library.properties b/library.properties index 018d97b..63ccf59 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=Modbus-Arduino -version=1.1.2 +version=1.2.0 author=Pascal Jean aka epsilonrt,André Sarmento Barbosa maintainer=epsilonrt sentence=A library that allows your Arduino to communicate via Modbus protocol, acting as a slave. diff --git a/src/Modbus.cpp b/src/Modbus.cpp index 91874ce..3a4df0c 100644 --- a/src/Modbus.cpp +++ b/src/Modbus.cpp @@ -6,10 +6,13 @@ #include "Modbus.h" //------------------------------------------------------------------------------- -Modbus::Modbus() { - _regs_head = 0; - _regs_last = 0; - _additional_data = 0; +Modbus::Modbus() : + _regs_head (0), + _regs_last (0), + _additional_data (0), + _frame (nullptr), + _len (0), + _reply (0) { } //------------------------------------------------------------------------------- @@ -138,35 +141,26 @@ void Modbus::receivePDU (byte *frame) { writeSingleRegister (field1, field2); break; - case MB_FC_READ_REGS: - //field1 = startreg, field2 = numregs - readRegisters (field1, field2); - break; - case MB_FC_WRITE_REGS: //field1 = startreg, field2 = status writeMultipleRegisters (frame, field1, field2, frame[5]); break; - #ifndef USE_HOLDING_REGISTERS_ONLY - - case MB_FC_REPORT_SERVER_ID: - reportServerId(); - break; - - case MB_FC_READ_COILS: + case MB_FC_READ_REGS: //field1 = startreg, field2 = numregs - readCoils (field1, field2); + readRegisters (fcode, field1, field2); break; - case MB_FC_READ_INPUT_STAT: + #ifndef USE_HOLDING_REGISTERS_ONLY + case MB_FC_READ_INPUT_REGS: //field1 = startreg, field2 = numregs - readInputStatus (field1, field2); + readRegisters (fcode, field1, field2); break; - case MB_FC_READ_INPUT_REGS: + case MB_FC_READ_INPUT_STAT: + case MB_FC_READ_COILS: //field1 = startreg, field2 = numregs - readInputRegisters (field1, field2); + readBits (fcode, field1, field2); break; case MB_FC_WRITE_COIL: @@ -179,6 +173,10 @@ void Modbus::receivePDU (byte *frame) { writeMultipleCoils (frame, field1, field2, frame[5]); break; + case MB_FC_REPORT_SERVER_ID: + reportServerId(); + break; + #endif default: @@ -188,11 +186,10 @@ void Modbus::receivePDU (byte *frame) { //------------------------------------------------------------------------------- // private -void Modbus::exceptionResponse (byte fcode, byte excode) { - //Clean frame buffer - free (_frame); +void Modbus::exceptionResponse (const byte fcode, const byte excode) { + _len = 2; - _frame = (byte *) malloc (_len); + _frame = (byte *) realloc (_frame, _len); _frame[0] = fcode + 0x80; _frame[1] = excode; @@ -211,7 +208,7 @@ void Modbus::exceptionResponse (byte fcode, byte excode) { */ //------------------------------------------------------------------------------- // private -void Modbus::writeSingleRegister (word reg, word value) { +void Modbus::writeSingleRegister (const word reg, const word value) { if (hregOutOfBounds (reg, value)) { exceptionResponse (MB_FC_WRITE_REG, MB_EX_ILLEGAL_VALUE); @@ -234,67 +231,6 @@ void Modbus::writeSingleRegister (word reg, word value) { _reply = MB_REPLY_ECHO; // reply with received frame } -/* - Func 03: Read Holding Registers - Request - Function code 1 Byte 0x03 - Starting Address 2 Bytes 0x0000 to 0xFFFF - Quantity of Registers 2 Bytes 1 to 125 (0x7D) - Response - Function code 1 Byte 0x03 - Byte count 1 Byte 2 x N* - Register value N* x 2 Bytes - N = Quantity of Registers -*/ -//------------------------------------------------------------------------------- -// private -void Modbus::readRegisters (word startreg, word numregs) { - //Check value (numregs) - if (numregs < 1 || numregs > 125) { - exceptionResponse (MB_FC_READ_REGS, MB_EX_ILLEGAL_VALUE); - return; - } - - //Check Address (startreg...startreg + numregs - 1) - for (word k = 0; k < numregs; k++) { - if (!searchRegister (startreg + TRegister::HregOffset + k)) { - exceptionResponse (MB_FC_WRITE_REGS, MB_EX_ILLEGAL_ADDRESS); - return; - } - } - - //Clean frame buffer - free (_frame); - _len = 0; - - //calculate the query reply message length - //for each register queried add 2 bytes - _len = 2 + numregs * 2; - - _frame = (byte *) malloc (_len); - if (!_frame) { - exceptionResponse (MB_FC_READ_REGS, MB_EX_SLAVE_FAILURE); - return; - } - - _frame[0] = MB_FC_READ_REGS; - _frame[1] = _len - 2; //byte count - - word val; - word i = 0; - while (numregs--) { - //retrieve the value from the register bank for the current register - val = hreg (startreg + i); - //write the high byte of the register value - _frame[2 + i * 2] = val >> 8; - //write the low byte of the register value - _frame[3 + i * 2] = val & 0xFF; - i++; - } - - _reply = MB_REPLY_NORMAL; -} - /* Func 16: Write Multiple registers Request Function code 1 Byte 0x10 @@ -310,7 +246,7 @@ void Modbus::readRegisters (word startreg, word numregs) { */ //------------------------------------------------------------------------------- // private -void Modbus::writeMultipleRegisters (byte *frame, word startreg, word numoutputs, byte bytecount) { +void Modbus::writeMultipleRegisters (const byte *frame, const word startreg, const word numoutputs, const byte bytecount) { //Check value if (numoutputs < 1 || numoutputs > 123 || bytecount != 2 * numoutputs) { exceptionResponse (MB_FC_WRITE_REGS, MB_EX_ILLEGAL_VALUE); @@ -325,6 +261,17 @@ void Modbus::writeMultipleRegisters (byte *frame, word startreg, word numoutputs } } + for (word i = 0; i < numoutputs; i++) { + + word val = (word) frame[6 + i * 2] << 8 | (word) frame[7 + i * 2]; + if (hregOutOfBounds (startreg + i, val)) { + + exceptionResponse (MB_FC_WRITE_REG, MB_EX_ILLEGAL_VALUE); + return; + } + setHreg (startreg + i, val); + } + //Clean frame buffer free (_frame); _len = 5; @@ -339,100 +286,95 @@ void Modbus::writeMultipleRegisters (byte *frame, word startreg, word numoutputs _frame[2] = startreg & 0x00FF; _frame[3] = numoutputs >> 8; _frame[4] = numoutputs & 0x00FF; - - word val; - word i = 0; - while (numoutputs--) { - val = (word) frame[6 + i * 2] << 8 | (word) frame[7 + i * 2]; - if (hregOutOfBounds (startreg + i, val)) { - exceptionResponse (MB_FC_WRITE_REG, MB_EX_ILLEGAL_VALUE); - return; - } - setHreg (startreg + i, val); - i++; - } - _reply = MB_REPLY_NORMAL; } -#ifndef USE_HOLDING_REGISTERS_ONLY -//------------------------------------------------------------------------------- -/* Func 01: Read Coils +/* + Func 03: Read Holding Registers Request - Function code 1 Byte 0x01 - Starting Address 2 Bytes 0x0000 to 0xFFFF - Quantity of coils 2 Bytes 1 to 2000 (0x7D0) + Function code 1 Byte 0x03 + Starting Address 2 Bytes 0x0000 to 0xFFFF + Quantity of Registers 2 Bytes 1 to 125 (0x7D) Response - Function code 1 Byte 0x01 - Byte count 1 Byte N* - Coil Status n Byte n = N or N+1 - N = Quantity of Outputs / 8, if the remainder is different of 0 -> N = N+1 + Function code 1 Byte 0x03 + Byte count 1 Byte 2 x N* + Register value N* x 2 Bytes + N = Quantity of Registers + Func 04: Read Input Registers + Request + Function code 1 Byte 0x04 + Starting Address 2 Bytes 0x0000 to 0xFFFF + Quantity of Input Registers 2 Bytes 0x0001 to 0x007D + Response + Function code 1 Byte 0x04 + Byte count 1 Byte 2 x N* + Input Registers N* x 2 Bytes + N = Quantity of Input Registers */ //------------------------------------------------------------------------------- // private -void Modbus::readCoils (word startreg, word numregs) { - //Check value (numregs) - if (numregs < 1 || numregs > 2000) { - exceptionResponse (MB_FC_READ_COILS, MB_EX_ILLEGAL_VALUE); - return; - } +void Modbus::readRegisters (const byte fcode, const word startreg, const word numregs) { - //When I check all registers in range I got errors in ScadaBR - //I think that ScadaBR request more than one in the single request - //when you have more then one datapoint configured from same type. - // epsilonrt -> We absolutely must not return values that do not exist !! + // Check value (numregs) + if (numregs < 1 || numregs > 0x7D) { - //Check Address (startreg...startreg + numregs - 1) - for (word k = 0; k < numregs; k++) { - if (!searchRegister (startreg + TRegister::CoilOffset + k)) { - exceptionResponse (MB_FC_WRITE_REGS, MB_EX_ILLEGAL_ADDRESS); - return; - } + exceptionResponse (fcode, MB_EX_ILLEGAL_VALUE); + return; } - //Clean frame buffer - free (_frame); - _len = 0; - - //Determine the message length = function type, byte count and - //for each group of 8 registers the message length increases by 1 - _len = 2 + numregs / 8; - if (numregs % 8) { - _len++; //Add 1 to the message length for the partial byte. - } + // calculate the query reply message length + // for each register queried add 2 bytes + _len = 2 + numregs * 2; - _frame = (byte *) malloc (_len); + _frame = (byte *) realloc (_frame, _len); if (!_frame) { - exceptionResponse (MB_FC_READ_COILS, MB_EX_SLAVE_FAILURE); + + exceptionResponse (fcode, MB_EX_SLAVE_FAILURE); return; } - _frame[0] = MB_FC_READ_COILS; - _frame[1] = _len - 2; //byte count (_len - function code and byte count) + _frame[0] = fcode; + _frame[1] = _len - 2; //byte count - byte bitn = 0; - word totregs = numregs; - word i; - while (numregs) { - i = (totregs - numregs) / 8; - if (coil (startreg)) { - bitSet (_frame[2 + i], bitn); + const word offset = (fcode == MB_FC_READ_REGS) ? TRegister::HregOffset : TRegister::IregOffset; + for (word i = 0; i < numregs; i++) { + + const word address = startreg + offset + i; + if (searchRegister (address)) { + + // retrieve the value from the register bank for the current register + word val = reg (address); + + // write the high byte of the register value + _frame[2 + i * 2] = val >> 8; + // write the low byte of the register value + _frame[3 + i * 2] = val & 0xFF; } else { - bitClear (_frame[2 + i], bitn); + + exceptionResponse (fcode, MB_EX_ILLEGAL_ADDRESS); + return; } - //increment the bit index - bitn = (bitn + 1) % 8; - //increment the register - startreg++; - numregs--; } _reply = MB_REPLY_NORMAL; } -/* Func 02: Read Discrete Inputs + +#ifndef USE_HOLDING_REGISTERS_ONLY +//------------------------------------------------------------------------------- +/* Func 01: Read Coils + Request + Function code 1 Byte 0x01 + Starting Address 2 Bytes 0x0000 to 0xFFFF + Quantity of coils 2 Bytes 1 to 2000 (0x7D0) + Response + Function code 1 Byte 0x01 + Byte count 1 Byte N* + Coil Status n Byte n = N or N+1 + N = Quantity of Outputs / 8, if the remainder is different of 0 -> N = N+1 + Func 02: Read Discrete Inputs Request Function code 1 Byte 0x02 Starting Address 2 Bytes 0x0000 to 0xFFFF @@ -441,127 +383,64 @@ void Modbus::readCoils (word startreg, word numregs) { Function code 1 Byte 0x02 Byte count 1 Byte N* Input Status N* x 1 Byte - N = Quantity of Inputs / 8 if the remainder is different of 0  N = N+1 - Error + N = Quantity of Inputs / 8, if the remainder is different of 0 -> N = N+1 */ //------------------------------------------------------------------------------- // private -void Modbus::readInputStatus (word startreg, word numregs) { +void Modbus::readBits (const byte fcode, const word startreg, const word numregs) { + //Check value (numregs) if (numregs < 0x0001 || numregs > 0x07D0) { - exceptionResponse (MB_FC_READ_INPUT_STAT, MB_EX_ILLEGAL_VALUE); + + exceptionResponse (fcode, MB_EX_ILLEGAL_VALUE); return; } - //Check Address (startreg...startreg + numregs - 1) - for (word k = 0; k < numregs; k++) { - if (!searchRegister (startreg + TRegister::IstsOffset + k)) { - exceptionResponse (MB_FC_WRITE_REGS, MB_EX_ILLEGAL_ADDRESS); - return; - } - } - - //Clean frame buffer - free (_frame); - _len = 0; - - //Determine the message length = function type, byte count and - //for each group of 8 registers the message length increases by 1 + // Determine the message length = function type, byte count and + // for each group of 8 registers the message length increases by 1 _len = 2 + numregs / 8; if (numregs % 8) { - _len++; //Add 1 to the message length for the partial byte. + _len++; // Add 1 to the message length for the partial byte. } - _frame = (byte *) malloc (_len); + _frame = (byte *) realloc (_frame, _len); if (!_frame) { - exceptionResponse (MB_FC_READ_INPUT_STAT, MB_EX_SLAVE_FAILURE); - return; - } - - _frame[0] = MB_FC_READ_INPUT_STAT; - _frame[1] = _len - 2; - - byte bitn = 0; - word totregs = numregs; - word i; - while (numregs) { - i = (totregs - numregs) / 8; - if (ists (startreg)) { - bitSet (_frame[2 + i], bitn); - } - else { - bitClear (_frame[2 + i], bitn); - } - //increment the bit index - bitn = (bitn + 1) % 8; - //increment the register - startreg++; - numregs--; - } - - _reply = MB_REPLY_NORMAL; -} -/* Func 04: Read Input Registers - Request - Function code 1 Byte 0x04 - Starting Address 2 Bytes 0x0000 to 0xFFFF - Quantity of Input Registers 2 Bytes 0x0001 to 0x007D - Response - Function code 1 Byte 0x04 - Byte count 1 Byte 2 x N* - Input Registers N* x 2 Bytes - N = Quantity of Input Registers -*/ -//------------------------------------------------------------------------------- -// private -void Modbus::readInputRegisters (word startreg, word numregs) { - //Check value (numregs) - if (numregs < 0x0001 || numregs > 0x007D) { - exceptionResponse (MB_FC_READ_INPUT_REGS, MB_EX_ILLEGAL_VALUE); + exceptionResponse (fcode, MB_EX_SLAVE_FAILURE); return; } - //Check Address (startreg...startreg + numregs - 1) - for (word k = 0; k < numregs; k++) { - if (!searchRegister (startreg + TRegister::IregOffset + k)) { - exceptionResponse (MB_FC_WRITE_REGS, MB_EX_ILLEGAL_ADDRESS); - return; - } - } + _frame[0] = fcode; + _frame[1] = _len - 2; // byte count (_len - function code and byte count) - //Clean frame buffer - free (_frame); - _len = 0; + const word offset = (fcode == MB_FC_READ_COILS) ? TRegister::CoilOffset : TRegister::IstsOffset; + for (word i = 0; i < numregs; i++) { - //calculate the query reply message length - //for each register queried add 2 bytes - _len = 2 + numregs * 2; + const word address = startreg + offset + i; + if (searchRegister (address)) { - _frame = (byte *) malloc (_len); - if (!_frame) { - exceptionResponse (MB_FC_READ_INPUT_REGS, MB_EX_SLAVE_FAILURE); - return; - } + word byteIndex = (i / 8) + 2; + byte bitIndex = i % 8; + if (reg (address) == 0xFF00) { - _frame[0] = MB_FC_READ_INPUT_REGS; - _frame[1] = _len - 2; + bitSet (_frame[byteIndex], bitIndex); + } + else { - word val; - word i = 0; - while (numregs--) { - //retrieve the value from the register bank for the current register - val = ireg (startreg + i); - //write the high byte of the register value - _frame[2 + i * 2] = val >> 8; - //write the low byte of the register value - _frame[3 + i * 2] = val & 0xFF; - i++; + bitClear (_frame[byteIndex], bitIndex); + } + } + else { + + exceptionResponse (fcode, MB_EX_ILLEGAL_ADDRESS); + return; + } } _reply = MB_REPLY_NORMAL; } + /* Func 05: Write Single Coil Request Function code 1 Byte 0x05 @@ -574,7 +453,7 @@ void Modbus::readInputRegisters (word startreg, word numregs) { */ //------------------------------------------------------------------------------- // private -void Modbus::writeSingleCoil (word reg, word status) { +void Modbus::writeSingleCoil (const word reg, const word status) { //Check value (status) if (status != 0xFF00 && status != 0x0000) { exceptionResponse (MB_FC_WRITE_COIL, MB_EX_ILLEGAL_VALUE); @@ -589,10 +468,10 @@ void Modbus::writeSingleCoil (word reg, word status) { //Check for failure ?? Why ? /* - if (coil (reg) != (bool) status) { + if (coil (reg) != (bool) status) { exceptionResponse (MB_FC_WRITE_COIL, MB_EX_SLAVE_FAILURE); return; - } + } */ _reply = MB_REPLY_ECHO; // reply with received frame @@ -612,30 +491,41 @@ void Modbus::writeSingleCoil (word reg, word status) { */ //------------------------------------------------------------------------------- // private -void Modbus::writeMultipleCoils (byte *frame, word startreg, word numoutputs, byte bytecount) { - //Check value +void Modbus::writeMultipleCoils (const byte *frame, const word startreg, const word numoutputs, const byte bytecount) { + + // Check value word bytecount_calc = numoutputs / 8; if (numoutputs % 8) { + bytecount_calc++; } + if (numoutputs < 0x0001 || numoutputs > 0x07B0 || bytecount != bytecount_calc) { + exceptionResponse (MB_FC_WRITE_COILS, MB_EX_ILLEGAL_VALUE); return; } - //Check Address (startreg...startreg + numregs) - for (word k = 0; k < numoutputs; k++) { - if (!searchRegister (startreg + TRegister::CoilOffset + k)) { + for (word i = 0; i < numoutputs; i++) { + + const word address = startreg + i; + if (searchRegister (address + TRegister::CoilOffset)) { + + word byteIndex = (i / 8) + 6; + byte bitIndex = i % 8; + setCoil (address, bitRead (frame[byteIndex], bitIndex)); + } + else { + exceptionResponse (MB_FC_WRITE_COILS, MB_EX_ILLEGAL_ADDRESS); return; } } - //Clean frame buffer - free (_frame); _len = 5; - _frame = (byte *) malloc (_len); + _frame = (byte *) realloc (_frame, _len); if (!_frame) { + exceptionResponse (MB_FC_WRITE_COILS, MB_EX_SLAVE_FAILURE); return; } @@ -646,21 +536,6 @@ void Modbus::writeMultipleCoils (byte *frame, word startreg, word numoutputs, by _frame[3] = numoutputs >> 8; _frame[4] = numoutputs & 0x00FF; - byte bitn = 0; - word totoutputs = numoutputs; - word i; - while (numoutputs--) { - i = (totoutputs - numoutputs) / 8; - setCoil (startreg, bitRead (frame[6 + i], bitn)); - //increment the bit index - bitn++; - if (bitn == 8) { - bitn = 0; - } - //increment the register - startreg++; - } - _reply = MB_REPLY_NORMAL; } @@ -678,22 +553,26 @@ void Modbus::writeMultipleCoils (byte *frame, word startreg, word numoutputs, by //------------------------------------------------------------------------------- // private void Modbus::reportServerId() { - //Clean frame buffer - free (_frame); + _len = 4; if (_additional_data) { + _len += strlen (_additional_data); } - _frame = (byte *) malloc (_len); + + _frame = (byte *) realloc (_frame, _len); if (!_frame) { + exceptionResponse (MB_FC_REPORT_SERVER_ID, MB_EX_SLAVE_FAILURE); return; } + _frame[0] = MB_FC_REPORT_SERVER_ID; _frame[1] = _len - 2; //byte count _frame[2] = 0x00; // Server ID _frame[3] = 0xFF; // Run Indicator Status if (_additional_data) { // Additional Data + strcpy ( (char *) &_frame[4], _additional_data); } _reply = MB_REPLY_NORMAL; diff --git a/src/Modbus.h b/src/Modbus.h index a0eef0a..64d2bf7 100644 --- a/src/Modbus.h +++ b/src/Modbus.h @@ -69,16 +69,15 @@ class Modbus { TRegister *_regs_last; char *_additional_data; - void readRegisters (word startreg, word numregs); - void writeSingleRegister (word reg, word value); - void writeMultipleRegisters (byte *frame, word startreg, word numoutputs, byte bytecount); - void exceptionResponse (byte fcode, byte excode); + void readRegisters (const byte fcode, const word startreg, const word numregs); + void writeSingleRegister (const word reg, const word value); + void writeMultipleRegisters (const byte *frame, const word startreg, const word numoutputs, const byte bytecount); + void exceptionResponse (const byte fcode, const byte excode); + #ifndef USE_HOLDING_REGISTERS_ONLY - void readCoils (word startreg, word numregs); - void readInputStatus (word startreg, word numregs); - void readInputRegisters (word startreg, word numregs); - void writeSingleCoil (word reg, word status); - void writeMultipleCoils (byte *frame, word startreg, word numoutputs, byte bytecount); + void readBits (const byte fcode,const word startreg, const word numregs); + void writeSingleCoil (const word reg, const word status); + void writeMultipleCoils (const byte *frame, const word startreg, const word numoutputs, const byte bytecount); #endif TRegister *searchRegister (word addr);