From 80bde0bdb5aa557ce239d8e8ee37162fc730a653 Mon Sep 17 00:00:00 2001 From: jaakkoiot Date: Sat, 15 Oct 2022 15:12:26 +0300 Subject: [PATCH 01/17] Add modbus leader class & tester --- Modbus/.project | 29 ++++ Modbus/inc/ModbusLeader.h | 281 ++++++++++++++++++++++++++++++++++++++ Modbus/liblinks.xml | 32 +++++ Modbus/src/tester.cpp | 45 ++++++ 4 files changed, 387 insertions(+) create mode 100644 Modbus/.project create mode 100644 Modbus/inc/ModbusLeader.h create mode 100644 Modbus/liblinks.xml create mode 100644 Modbus/src/tester.cpp diff --git a/Modbus/.project b/Modbus/.project new file mode 100644 index 0000000..c7d3ce5 --- /dev/null +++ b/Modbus/.project @@ -0,0 +1,29 @@ + + + Modbus + + + lpc_chip_15xx + DigitalIoPin + + + + org.eclipse.cdt.managedbuilder.core.genmakebuilder + clean,full,incremental, + + + + + org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder + full,incremental, + + + + + + org.eclipse.cdt.core.cnature + org.eclipse.cdt.core.ccnature + org.eclipse.cdt.managedbuilder.core.managedBuildNature + org.eclipse.cdt.managedbuilder.core.ScannerConfigNature + + diff --git a/Modbus/inc/ModbusLeader.h b/Modbus/inc/ModbusLeader.h new file mode 100644 index 0000000..d038569 --- /dev/null +++ b/Modbus/inc/ModbusLeader.h @@ -0,0 +1,281 @@ +/** +@file +Arduino library for communicating with Modbus slaves over RS232/485 (via RTU protocol). + +@defgroup setup ModbusMaster Object Instantiation/Initialization +@defgroup buffer ModbusMaster Buffer Management +@defgroup discrete Modbus Function Codes for Discrete Coils/Inputs +@defgroup register Modbus Function Codes for Holding/Input Registers +@defgroup constant Modbus Function Codes, Exception Codes +*/ +/* + + ModbusMaster.h - Arduino library for communicating with Modbus slaves + over RS232/485 (via RTU protocol). + + This file is part of ModbusMaster. + + ModbusMaster is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + ModbusMaster is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with ModbusMaster. If not, see . + + Written by Doc Walker (Rx) + Copyright © 2009-2013 Doc Walker <4-20ma at wvfans dot net> + +*/ + + +#ifndef ModbusMaster_h +#define ModbusMaster_h + + +/** +@def __MODBUSMASTER_DEBUG__ (1). +Set to 1 to enable debugging features within class: + - pin 4 cycles for each byte read in the Modbus response + - pin 5 cycles for each millisecond timeout during the Modbus response +*/ +#define __MODBUSMASTER_DEBUG__ (0) + + +/* _____STANDARD INCLUDES____________________________________________________ */ +// include types & constants of Wiring core API +#if defined(ARDUINO) && ARDUINO >= 100 +#include "Arduino.h" +#else +//#include "WProgram.h" +#include +#include +#endif +#include + +uint32_t millis(); +#define BYTE 0xA5 + +/* _____UTILITY MACROS_______________________________________________________ */ + + +/* _____PROJECT INCLUDES_____________________________________________________ */ +// functions to calculate Modbus Application Data Unit CRC +//#include "util/crc16.h" +// moved inlcuding crc16.h to ModbusMaster.cpp + +// functions to manipulate words +///#include "util/word.h" +#include "word.h" +#include "SerialPort.h" + +/* _____CLASS DEFINITIONS____________________________________________________ */ +/** +Arduino class library for communicating with Modbus slaves over +RS232/485 (via RTU protocol). +*/ +class ModbusMaster +{ + public: + ModbusMaster(); + ModbusMaster(uint8_t); + ModbusMaster(uint8_t, uint8_t); + + void begin(); + void begin(uint16_t); + void idle(void (*)()); + + // Modbus exception codes + /** + Modbus protocol illegal function exception. + + The function code received in the query is not an allowable action for + the server (or slave). This may be because the function code is only + applicable to newer devices, and was not implemented in the unit + selected. It could also indicate that the server (or slave) is in the + wrong state to process a request of this type, for example because it is + unconfigured and is being asked to return register values. + + @ingroup constant + */ + static const uint8_t ku8MBIllegalFunction = 0x01; + + /** + Modbus protocol illegal data address exception. + + The data address received in the query is not an allowable address for + the server (or slave). More specifically, the combination of reference + number and transfer length is invalid. For a controller with 100 + registers, the ADU addresses the first register as 0, and the last one + as 99. If a request is submitted with a starting register address of 96 + and a quantity of registers of 4, then this request will successfully + operate (address-wise at least) on registers 96, 97, 98, 99. If a + request is submitted with a starting register address of 96 and a + quantity of registers of 5, then this request will fail with Exception + Code 0x02 "Illegal Data Address" since it attempts to operate on + registers 96, 97, 98, 99 and 100, and there is no register with address + 100. + + @ingroup constant + */ + static const uint8_t ku8MBIllegalDataAddress = 0x02; + + /** + Modbus protocol illegal data value exception. + + A value contained in the query data field is not an allowable value for + server (or slave). This indicates a fault in the structure of the + remainder of a complex request, such as that the implied length is + incorrect. It specifically does NOT mean that a data item submitted for + storage in a register has a value outside the expectation of the + application program, since the MODBUS protocol is unaware of the + significance of any particular value of any particular register. + + @ingroup constant + */ + static const uint8_t ku8MBIllegalDataValue = 0x03; + + /** + Modbus protocol slave device failure exception. + + An unrecoverable error occurred while the server (or slave) was + attempting to perform the requested action. + + @ingroup constant + */ + static const uint8_t ku8MBSlaveDeviceFailure = 0x04; + + // Class-defined success/exception codes + /** + ModbusMaster success. + + Modbus transaction was successful; the following checks were valid: + - slave ID + - function code + - response code + - data + - CRC + + @ingroup constant + */ + static const uint8_t ku8MBSuccess = 0x00; + + /** + ModbusMaster invalid response slave ID exception. + + The slave ID in the response does not match that of the request. + + @ingroup constant + */ + static const uint8_t ku8MBInvalidSlaveID = 0xE0; + + /** + ModbusMaster invalid response function exception. + + The function code in the response does not match that of the request. + + @ingroup constant + */ + static const uint8_t ku8MBInvalidFunction = 0xE1; + + /** + ModbusMaster response timed out exception. + + The entire response was not received within the timeout period, + ModbusMaster::ku8MBResponseTimeout. + + @ingroup constant + */ + static const uint8_t ku8MBResponseTimedOut = 0xE2; + + /** + ModbusMaster invalid response CRC exception. + + The CRC in the response does not match the one calculated. + + @ingroup constant + */ + static const uint8_t ku8MBInvalidCRC = 0xE3; + + uint16_t getResponseBuffer(uint8_t); + void clearResponseBuffer(); + uint8_t setTransmitBuffer(uint8_t, uint16_t); + void clearTransmitBuffer(); + + void beginTransmission(uint16_t); + uint8_t requestFrom(uint16_t, uint16_t); + void sendBit(bool); + void send(uint8_t); + void send(uint16_t); + void send(uint32_t); + uint8_t available(void); + uint16_t receive(void); + + + uint8_t readCoils(uint16_t, uint16_t); + uint8_t readDiscreteInputs(uint16_t, uint16_t); + uint8_t readHoldingRegisters(uint16_t, uint16_t); + uint8_t readInputRegisters(uint16_t, uint8_t); + uint8_t writeSingleCoil(uint16_t, uint8_t); + uint8_t writeSingleRegister(uint16_t, uint16_t); + uint8_t writeMultipleCoils(uint16_t, uint16_t); + uint8_t writeMultipleCoils(); + uint8_t writeMultipleRegisters(uint16_t, uint16_t); + uint8_t writeMultipleRegisters(); + uint8_t maskWriteRegister(uint16_t, uint16_t, uint16_t); + uint8_t readWriteMultipleRegisters(uint16_t, uint16_t, uint16_t, uint16_t); + uint8_t readWriteMultipleRegisters(uint16_t, uint16_t); + + private: + uint8_t _u8SerialPort; ///< serial port (0..3) initialized in constructor + uint8_t _u8MBSlave; ///< Modbus slave (1..255) initialized in constructor + uint16_t _u16BaudRate; ///< baud rate (300..115200) initialized in begin() + static const uint8_t ku8MaxBufferSize = 64; ///< size of response/transmit buffers + uint16_t _u16ReadAddress; ///< slave register from which to read + uint16_t _u16ReadQty; ///< quantity of words to read + uint16_t _u16ResponseBuffer[ku8MaxBufferSize]; ///< buffer to store Modbus slave response; read via GetResponseBuffer() + uint16_t _u16WriteAddress; ///< slave register to which to write + uint16_t _u16WriteQty; ///< quantity of words to write + uint16_t _u16TransmitBuffer[ku8MaxBufferSize]; ///< buffer containing data to transmit to Modbus slave; set via SetTransmitBuffer() + uint16_t* txBuffer; // from Wire.h -- need to clean this up Rx + uint8_t _u8TransmitBufferIndex; + uint16_t u16TransmitBufferLength; + uint16_t* rxBuffer; // from Wire.h -- need to clean this up Rx + uint8_t _u8ResponseBufferIndex; + uint8_t _u8ResponseBufferLength; + + // Modbus function codes for bit access + static const uint8_t ku8MBReadCoils = 0x01; ///< Modbus function 0x01 Read Coils + static const uint8_t ku8MBReadDiscreteInputs = 0x02; ///< Modbus function 0x02 Read Discrete Inputs + static const uint8_t ku8MBWriteSingleCoil = 0x05; ///< Modbus function 0x05 Write Single Coil + static const uint8_t ku8MBWriteMultipleCoils = 0x0F; ///< Modbus function 0x0F Write Multiple Coils + + // Modbus function codes for 16 bit access + static const uint8_t ku8MBReadHoldingRegisters = 0x03; ///< Modbus function 0x03 Read Holding Registers + static const uint8_t ku8MBReadInputRegisters = 0x04; ///< Modbus function 0x04 Read Input Registers + static const uint8_t ku8MBWriteSingleRegister = 0x06; ///< Modbus function 0x06 Write Single Register + static const uint8_t ku8MBWriteMultipleRegisters = 0x10; ///< Modbus function 0x10 Write Multiple Registers + static const uint8_t ku8MBMaskWriteRegister = 0x16; ///< Modbus function 0x16 Mask Write Register + static const uint8_t ku8MBReadWriteMultipleRegisters = 0x17; ///< Modbus function 0x17 Read Write Multiple Registers + + // Modbus timeout [milliseconds] + static const uint16_t ku16MBResponseTimeout = 2000; ///< Modbus timeout [milliseconds] + + // master function that conducts Modbus transactions + uint8_t ModbusMasterTransaction(uint8_t u8MBFunction); + + // idle callback function; gets called during idle time between TX and RX + void (*_idle)(); + SerialPort *MBSerial = NULL; // added by KRL +}; +#endif + +/** +@example examples/Basic/Basic.pde +@example examples/PhoenixContact_nanoLC/PhoenixContact_nanoLC.pde +*/ \ No newline at end of file diff --git a/Modbus/liblinks.xml b/Modbus/liblinks.xml new file mode 100644 index 0000000..1dc1803 --- /dev/null +++ b/Modbus/liblinks.xml @@ -0,0 +1,32 @@ + + + + + ${MacroStart}workspace_loc:/${ProjName}/inc${MacroEnd} + + + ${MacroStart}workspace_loc:/${ProjName}/inc${MacroEnd} + + + ${ProjName} + + + ${MacroStart}workspace_loc:/${ProjName}/Debug${MacroEnd} + + + ${MacroStart}workspace_loc:/${ProjName}/Release${MacroEnd} + + + ${ProjName} + + + diff --git a/Modbus/src/tester.cpp b/Modbus/src/tester.cpp new file mode 100644 index 0000000..a2935ea --- /dev/null +++ b/Modbus/src/tester.cpp @@ -0,0 +1,45 @@ +#if defined (__USE_LPCOPEN) +#if defined(NO_BOARD_LIB) +#include "chip.h" +#else +#include "board.h" +#endif +#endif + +#include +#include "ModbusMaster.h" +// TODO: insert other include files here + +// TODO: insert other definitions and declarations here + +int main(void) { + +#if defined (__USE_LPCOPEN) + // Read clock settings and update SystemCoreClock variable + SystemCoreClockUpdate(); +#if !defined(NO_BOARD_LIB) + // Set up and initialize all required blocks and + // functions related to the board hardware + Board_Init(); + // Set the LED to the state of "On" + Board_LED_Set(0, true); +#endif +#endif + + // TODO: insert code here + ModbusMaster MIO_12V(1); + ModbusMaster CO2_sensor(240); + ModbusMaster RH_sensor(241); + + + // Force the counter to be placed into memory + volatile static int i = 0 ; + // Enter an infinite loop, just incrementing a counter + while(1) { + i++ ; + // "Dummy" NOP to allow source level single + // stepping of tight while() loop + __asm volatile ("nop"); + } + return 0 ; +} From 0857a26f0b1d21df75ab8bbfd6990eb5a5263889 Mon Sep 17 00:00:00 2001 From: jaakkoiot Date: Sat, 15 Oct 2022 15:15:12 +0300 Subject: [PATCH 02/17] Ass register class for modbus comms, without hw addresses yet --- Modbus/inc/ModbusRegister.h | 19 +++++++++++++++++++ Modbus/src/ModbusRegister.cpp | 27 +++++++++++++++++++++++++++ 2 files changed, 46 insertions(+) create mode 100644 Modbus/inc/ModbusRegister.h create mode 100644 Modbus/src/ModbusRegister.cpp diff --git a/Modbus/inc/ModbusRegister.h b/Modbus/inc/ModbusRegister.h new file mode 100644 index 0000000..af20be9 --- /dev/null +++ b/Modbus/inc/ModbusRegister.h @@ -0,0 +1,19 @@ +#ifndef MODBUSREGISTER_H_ +#define MODBUSREGISTER_H_ + +#include "ModbusLeader.h" + +class ModbusRegister { +public: + ModbusRegister(ModbusMaster *master, int address, bool holdingRegister = true); + ModbusRegister(const ModbusRegister &) = delete; + virtual ~ModbusRegister(); + int read(); + void write(int value); +private: + ModbusMaster *m; + int addr; + bool hr; +}; + +#endif /* MODBUSREGISTER_H_ */ diff --git a/Modbus/src/ModbusRegister.cpp b/Modbus/src/ModbusRegister.cpp new file mode 100644 index 0000000..a642bbc --- /dev/null +++ b/Modbus/src/ModbusRegister.cpp @@ -0,0 +1,27 @@ +#include "ModbusRegister.h" + +ModbusRegister::ModbusRegister(ModbusMaster *master, int address, bool holdingRegister) + :m(master), addr(address), hr(holdingRegister) { + // TODO Auto-generated constructor stub + +} + +ModbusRegister::~ModbusRegister() { + // TODO Auto-generated destructor stub +} + +int ModbusRegister::read() { + uint8_t result = hr ? m->readHoldingRegisters(addr, 1) : m->readInputRegisters(addr, 1) ; + // check if we were able to read + if (result == m->ku8MBSuccess) { + return m->getResponseBuffer(0); + } + return -1; +} + +void ModbusRegister::write(int value) +{ + // write only if not + if(hr) m->writeSingleRegister(addr, value); // not checking if write succeeds + +} From 467c6e6d7d75bac15e364068b162739b978461eb Mon Sep 17 00:00:00 2001 From: jaakkoiot Date: Sat, 15 Oct 2022 15:17:53 +0300 Subject: [PATCH 03/17] Add serial & UART classes for Modbus use --- Modbus/inc/SerialPort.h | 20 ++++ Modbus/inc/Uart.h | 62 +++++++++++ Modbus/src/SerialPort.cpp | 50 +++++++++ Modbus/src/Uart.cpp | 227 ++++++++++++++++++++++++++++++++++++++ 4 files changed, 359 insertions(+) create mode 100644 Modbus/inc/SerialPort.h create mode 100644 Modbus/inc/Uart.h create mode 100644 Modbus/src/SerialPort.cpp create mode 100644 Modbus/src/Uart.cpp diff --git a/Modbus/inc/SerialPort.h b/Modbus/inc/SerialPort.h new file mode 100644 index 0000000..6e89b6e --- /dev/null +++ b/Modbus/inc/SerialPort.h @@ -0,0 +1,20 @@ +#ifndef SERIALPORT_H_ +#define SERIALPORT_H_ + +#include "Uart.h" + +class SerialPort { +public: + SerialPort(); + virtual ~SerialPort(); + int available(); + void begin(int speed = 9600); + int read(); + int write(const char* buf, int len); + int print(int val, int format); + void flush(); +private: + static LpcUart *u; +}; + +#endif /* SERIALPORT_H_ */ diff --git a/Modbus/inc/Uart.h b/Modbus/inc/Uart.h new file mode 100644 index 0000000..ce3c007 --- /dev/null +++ b/Modbus/inc/Uart.h @@ -0,0 +1,62 @@ +/* + * LpcUart.h + * + * Created on: 4.2.2019 + * Author: keijo + */ + +#ifndef LPCUART_H_ +#define LPCUART_H_ + +#include +#include "chip.h" + +struct LpcPinMap { + int port; /* set to -1 to indicate unused pin */ + int pin; /* set to -1 to indicate unused pin */ +}; + +struct LpcUartConfig { + LPC_USART_T *pUART; + uint32_t speed; + uint32_t data; + bool rs485; + LpcPinMap tx; + LpcPinMap rx; + LpcPinMap rts; /* used as output enable if RS-485 mode is enabled */ + LpcPinMap cts; +}; + + +class LpcUart { +public: + LpcUart(const LpcUartConfig &cfg); + LpcUart(const LpcUart &) = delete; + virtual ~LpcUart(); + int free(); /* get amount of free space in transmit buffer */ + int peek(); /* get number of received characters in receive buffer */ + int write(char c); + int write(const char *s); + int write(const char *buffer, int len); + int read(char &c); /* get a single character. Returns number of characters read --> returns 0 if no character is available */ + int read(char *buffer, int len); + void txbreak(bool brk); /* set break signal on */ + bool rxbreak(); /* check if break is received */ + void speed(int bps); /* change transmission speed */ + bool txempty(); + + void isr(); /* ISR handler. This will be called by the HW ISR handler. Do not call from application */ +private: + LPC_USART_T *uart; + IRQn_Type irqn; + /* currently we support only fixed size ring buffers */ + static const int UART_RB_SIZE = 256; + /* Transmit and receive ring buffers */ + RINGBUFF_T txring; + RINGBUFF_T rxring; + uint8_t rxbuff[UART_RB_SIZE]; + uint8_t txbuff[UART_RB_SIZE]; + static bool init; /* set when first UART is initialized. We have a global clock setting for all UARTSs */ +}; + +#endif /* LPCUART_H_ */ diff --git a/Modbus/src/SerialPort.cpp b/Modbus/src/SerialPort.cpp new file mode 100644 index 0000000..953d99a --- /dev/null +++ b/Modbus/src/SerialPort.cpp @@ -0,0 +1,50 @@ +#include "SerialPort.h" + + + +SerialPort::SerialPort() { + if(!u) { + LpcPinMap none = {-1, -1}; // unused pin has negative values in it + LpcPinMap txpin = { 0, 28 }; // transmit pin that goes to rs485 driver chip + LpcPinMap rxpin = { 0, 24 }; // receive pin that goes to rs485 driver chip + LpcPinMap rtspin = { 1, 0 }; // handshake pin that is used to set tranmitter direction + LpcUartConfig cfg = { LPC_USART1, 9600, UART_CFG_DATALEN_8 | UART_CFG_PARITY_NONE | UART_CFG_STOPLEN_2, true, txpin, rxpin, rtspin, none }; + u = new LpcUart(cfg); + } +} + +LpcUart *SerialPort::u = nullptr; + +SerialPort::~SerialPort() { + /* DeInitialize UART peripheral */ + delete u; +} + +int SerialPort::available() { + return u->peek(); +} + +void SerialPort::begin(int speed) { + u->speed(speed); + +} + +int SerialPort::read() { + char byte; + if(u->read(byte)> 0) return (byte); + return -1; +} +int SerialPort::write(const char* buf, int len) { + return u->write(buf, len); +} + +int SerialPort::print(int val, int format) { + // here only to maintain compatibility with Arduino interface + (void) val; + (void) format; + return (0); +} + +void SerialPort::flush() { + while(!u->txempty()) __WFI(); +} diff --git a/Modbus/src/Uart.cpp b/Modbus/src/Uart.cpp new file mode 100644 index 0000000..6a45cae --- /dev/null +++ b/Modbus/src/Uart.cpp @@ -0,0 +1,227 @@ +#include +#include "Uart.h" + + +static LpcUart *u0; +static LpcUart *u1; +static LpcUart *u2; + +extern "C" { +/** + * @brief UART interrupt handler using ring buffers + * @return Nothing + */ +void UART0_IRQHandler(void) +{ + /* Want to handle any errors? Do it here. */ + + /* Use default ring buffer handler. Override this with your own + code if you need more capability. */ + if(u0) u0->isr(); +} + +void UART1_IRQHandler(void) +{ + /* Want to handle any errors? Do it here. */ + + /* Use default ring buffer handler. Override this with your own + code if you need more capability. */ + if(u1) u1->isr(); +} + +void UART2_IRQHandler(void) +{ + /* Want to handle any errors? Do it here. */ + + /* Use default ring buffer handler. Override this with your own + code if you need more capability. */ + if(u2) u2->isr(); +} + +} + + +void LpcUart::isr() { + Chip_UART_IRQRBHandler(uart, &rxring, &txring); +} + +bool LpcUart::init = false; + +LpcUart::LpcUart(const LpcUartConfig &cfg) { + CHIP_SWM_PIN_MOVABLE_T tx; + CHIP_SWM_PIN_MOVABLE_T rx; + CHIP_SWM_PIN_MOVABLE_T cts; + CHIP_SWM_PIN_MOVABLE_T rts; + bool use_rts = (cfg.rts.port >= 0); + bool use_cts = (cfg.cts.port >= 0); + + if(!init) { + init = true; + /* Before setting up the UART, the global UART clock for USARTS 1-4 + * must first be setup. This requires setting the UART divider and + * the UART base clock rate to 16x the maximum UART rate for all + * UARTs. + * */ + /* Use main clock rate as base for UART baud rate divider */ + Chip_Clock_SetUARTBaseClockRate(Chip_Clock_GetMainClockRate(), false); + } + + uart = nullptr; // set default value before checking which UART to configure + + if(cfg.pUART == LPC_USART0) { + if(u0) return; // already exists + else u0 = this; + tx = SWM_UART0_TXD_O; + rx = SWM_UART0_RXD_I; + rts = SWM_UART0_RTS_O; + cts = SWM_UART0_CTS_I; + irqn = UART0_IRQn; + } + else if(cfg.pUART == LPC_USART1) { + if(u1) return; // already exists + else u1 = this; + tx = SWM_UART1_TXD_O; + rx = SWM_UART1_RXD_I; + rts = SWM_UART1_RTS_O; + cts = SWM_UART1_CTS_I; + irqn = UART1_IRQn; + } + else if(cfg.pUART == LPC_USART2) { + if(u2) return; // already exists + else u2 = this; + tx = SWM_UART2_TXD_O; + rx = SWM_UART2_RXD_I; + use_rts = false; // UART2 does not support handshakes + use_cts = false; + irqn = UART2_IRQn; + } + else { + return; + } + + uart = cfg.pUART; // set the actual value after validity checking + + + if(cfg.tx.port >= 0) { + Chip_IOCON_PinMuxSet(LPC_IOCON, cfg.tx.port, cfg.tx.pin, (IOCON_MODE_INACT | IOCON_DIGMODE_EN)); + Chip_SWM_MovablePortPinAssign(tx, cfg.tx.port, cfg.tx.pin); + } + + if(cfg.rx.port >= 0) { + Chip_IOCON_PinMuxSet(LPC_IOCON, cfg.rx.port, cfg.rx.pin, (IOCON_MODE_INACT | IOCON_DIGMODE_EN)); + Chip_SWM_MovablePortPinAssign(rx, cfg.rx.port, cfg.rx.pin); + } + + if(use_cts) { + Chip_IOCON_PinMuxSet(LPC_IOCON, cfg.cts.port, cfg.cts.pin, (IOCON_MODE_INACT | IOCON_DIGMODE_EN)); + Chip_SWM_MovablePortPinAssign(cts, cfg.cts.port, cfg.cts.pin); + } + + if(use_rts) { + Chip_IOCON_PinMuxSet(LPC_IOCON, cfg.rts.port, cfg.rts.pin, (IOCON_MODE_INACT | IOCON_DIGMODE_EN)); + Chip_SWM_MovablePortPinAssign(rts, cfg.rts.port, cfg.rts.pin); + } + + + /* Setup UART */ + Chip_UART_Init(uart); + Chip_UART_ConfigData(uart, cfg.data); + Chip_UART_SetBaud(uart, cfg.speed); + + if(use_rts && cfg.rs485) { + uart->CFG |= (1 << 20); // enable rs485 mode + //uart->CFG |= (1 << 18); // OE turnaraound time + uart->CFG |= (1 << 21);// driver enable polarity (active high) + } + + Chip_UART_Enable(uart); + Chip_UART_TXEnable(uart); + + /* Before using the ring buffers, initialize them using the ring + buffer init function */ + RingBuffer_Init(&rxring, rxbuff, 1, UART_RB_SIZE); + RingBuffer_Init(&txring, txbuff, 1, UART_RB_SIZE); + + + /* Enable receive data and line status interrupt */ + Chip_UART_IntEnable(uart, UART_INTEN_RXRDY); + Chip_UART_IntDisable(uart, UART_INTEN_TXRDY); /* May not be needed */ + + /* Enable UART interrupt */ + NVIC_EnableIRQ(irqn); +} + +LpcUart::~LpcUart() { + if(uart != nullptr) { + NVIC_DisableIRQ(irqn); + Chip_UART_IntDisable(uart, UART_INTEN_RXRDY); + Chip_UART_IntDisable(uart, UART_INTEN_TXRDY); + + if(uart == LPC_USART0) { + u0 = nullptr; + } + else if(uart == LPC_USART1) { + u1 = nullptr; + } + else if(uart == LPC_USART2) { + u2 = nullptr; + } + } +} + + +int LpcUart::free() +{ + return RingBuffer_GetCount(&txring);; +} + +int LpcUart::peek() +{ + return RingBuffer_GetCount(&rxring); +} + +int LpcUart::read(char &c) +{ + return Chip_UART_ReadRB(uart, &rxring, &c, 1); +} + +int LpcUart::read(char *buffer, int len) +{ + return Chip_UART_ReadRB(uart, &rxring, buffer, len); +} + +int LpcUart::write(char c) +{ + return Chip_UART_SendRB(uart, &txring, &c, 1); +} + +int LpcUart::write(const char *s) +{ + return Chip_UART_SendRB(uart, &txring, s, strlen(s)); +} + +int LpcUart::write(const char *buffer, int len) +{ + return Chip_UART_SendRB(uart, &txring, buffer, len); +} + +void LpcUart::txbreak(bool brk) +{ + // break handling not implemented yeet +} + +bool LpcUart::rxbreak() +{ + // break handling not implemented yeet + return false; +} + +void LpcUart::speed(int bps) +{ + Chip_UART_SetBaud(uart, bps); +} + +bool LpcUart::txempty() +{ + return (RingBuffer_GetCount(&txring) == 0); +} From 56ec659fc4d6791af5c7ca08ad1b69c907833933 Mon Sep 17 00:00:00 2001 From: jaakkoiot Date: Sat, 15 Oct 2022 15:19:12 +0300 Subject: [PATCH 04/17] Populate source code for ModbusLeader class --- Modbus/src/ModbusLeader.cpp | 937 ++++++++++++++++++++++++++++++++++++ 1 file changed, 937 insertions(+) create mode 100644 Modbus/src/ModbusLeader.cpp diff --git a/Modbus/src/ModbusLeader.cpp b/Modbus/src/ModbusLeader.cpp new file mode 100644 index 0000000..92a8958 --- /dev/null +++ b/Modbus/src/ModbusLeader.cpp @@ -0,0 +1,937 @@ +/** +@file +Arduino library for communicating with Modbus slaves over RS232/485 (via RTU protocol). +*/ +/* + + ModbusMaster.cpp - Arduino library for communicating with Modbus slaves + over RS232/485 (via RTU protocol). + + This file is part of ModbusMaster. + + ModbusMaster is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + ModbusMaster is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with ModbusMaster. If not, see . + + Written by Doc Walker (Rx) + Copyright © 2009-2013 Doc Walker <4-20ma at wvfans dot net> + +*/ + + +/* _____PROJECT INCLUDES_____________________________________________________ */ +#include "ModbusLeader.h" +#include "crc16.h" + + +/* _____GLOBAL VARIABLES_____________________________________________________ */ +#if defined(ARDUINO_ARCH_AVR) + HardwareSerial* MBSerial = &Serial; ///< Pointer to Serial class object +#elif defined(ARDUINO_ARCH_SAM) + UARTClass* MBSerial = &Serial; ///< Pointer to Serial class object +#else +// #error "This library only supports boards with an AVR or SAM processor. Please open an issue at https://github.com/4-20ma/ModbusMaster/issues and indicate which processor/platform you're using." +#endif + + +/* _____PUBLIC FUNCTIONS_____________________________________________________ */ +/** +Constructor. + +Creates class object using default serial port 0, Modbus slave ID 1. + +@ingroup setup +*/ +ModbusMaster::ModbusMaster(void) +{ + _u8SerialPort = 0; + _u8MBSlave = 1; + _u16BaudRate = 0; +} + + +/** +Constructor. + +Creates class object using default serial port 0, specified Modbus slave ID. + +@overload void ModbusMaster::ModbusMaster(uint8_t u8MBSlave) +@param u8MBSlave Modbus slave ID (1..255) +@ingroup setup +*/ +ModbusMaster::ModbusMaster(uint8_t u8MBSlave) +{ + _u8SerialPort = 0; + _u8MBSlave = u8MBSlave; + _u16BaudRate = 0; +} + + +/** +Constructor. + +Creates class object using specified serial port, Modbus slave ID. + +@overload void ModbusMaster::ModbusMaster(uint8_t u8SerialPort, uint8_t u8MBSlave) +@param u8SerialPort serial port (Serial, Serial1..Serial3) +@param u8MBSlave Modbus slave ID (1..255) +@ingroup setup +*/ +ModbusMaster::ModbusMaster(uint8_t u8SerialPort, uint8_t u8MBSlave) +{ + _u8SerialPort = (u8SerialPort > 3) ? 0 : u8SerialPort; + _u8MBSlave = u8MBSlave; + _u16BaudRate = 0; +} + + +/** +Initialize class object. + +Sets up the serial port using default 19200 baud rate. +Call once class has been instantiated, typically within setup(). + +@ingroup setup +*/ +void ModbusMaster::begin(void) +{ + begin(19200); +} + + +/** +Initialize class object. + +Sets up the serial port using specified baud rate. +Call once class has been instantiated, typically within setup(). + +@overload ModbusMaster::begin(uint16_t u16BaudRate) +@param u16BaudRate baud rate, in standard increments (300..115200) +@ingroup setup +*/ +void ModbusMaster::begin(uint16_t u16BaudRate) +{ +// txBuffer = (uint16_t*) calloc(ku8MaxBufferSize, sizeof(uint16_t)); + _u8TransmitBufferIndex = 0; + u16TransmitBufferLength = 0; + +#if 0 + switch(_u8SerialPort) + { +#if defined(UBRR1H) + case 1: + MBSerial = &Serial1; + break; +#endif + +#if defined(UBRR2H) + case 2: + MBSerial = &Serial2; + break; +#endif + +#if defined(UBRR3H) + case 3: + MBSerial = &Serial3; + break; +#endif + + case 0: + default: + MBSerial = &Serial; + break; + } +#endif + + if(MBSerial == NULL) MBSerial = new SerialPort; + if(u16BaudRate != _u16BaudRate) { + _u16BaudRate = u16BaudRate; + MBSerial->begin(u16BaudRate); + } + _idle = NULL; + +#if __MODBUSMASTER_DEBUG__ +// pinMode(4, OUTPUT); +// pinMode(5, OUTPUT); +#endif +} + + +void ModbusMaster::beginTransmission(uint16_t u16Address) +{ + _u16WriteAddress = u16Address; + _u8TransmitBufferIndex = 0; + u16TransmitBufferLength = 0; +} + +// eliminate this function in favor of using existing MB request functions +uint8_t ModbusMaster::requestFrom(uint16_t address, uint16_t quantity) +{ + uint8_t read; + read = 1; // krl: added this to prevent warning. This method is not called anywhere... + // clamp to buffer length + if (quantity > ku8MaxBufferSize) + { + quantity = ku8MaxBufferSize; + } + // set rx buffer iterator vars + _u8ResponseBufferIndex = 0; + _u8ResponseBufferLength = read; + + return read; +} + + +void ModbusMaster::sendBit(bool data) +{ + uint8_t txBitIndex = u16TransmitBufferLength % 16; + if ((u16TransmitBufferLength >> 4) < ku8MaxBufferSize) + { + if (0 == txBitIndex) + { + _u16TransmitBuffer[_u8TransmitBufferIndex] = 0; + } + bitWrite(_u16TransmitBuffer[_u8TransmitBufferIndex], txBitIndex, data); + u16TransmitBufferLength++; + _u8TransmitBufferIndex = u16TransmitBufferLength >> 4; + } +} + + +void ModbusMaster::send(uint16_t data) +{ + if (_u8TransmitBufferIndex < ku8MaxBufferSize) + { + _u16TransmitBuffer[_u8TransmitBufferIndex++] = data; + u16TransmitBufferLength = _u8TransmitBufferIndex << 4; + } +} + + +void ModbusMaster::send(uint32_t data) +{ + send(lowWord(data)); + send(highWord(data)); +} + + +void ModbusMaster::send(uint8_t data) +{ + send(word(data)); +} + + + + + + + + + +uint8_t ModbusMaster::available(void) +{ + return _u8ResponseBufferLength - _u8ResponseBufferIndex; +} + + +uint16_t ModbusMaster::receive(void) +{ + if (_u8ResponseBufferIndex < _u8ResponseBufferLength) + { + return _u16ResponseBuffer[_u8ResponseBufferIndex++]; + } + else + { + return 0xFFFF; + } +} + + + + + + + + +/** +Set idle time callback function (cooperative multitasking). + +This function gets called in the idle time between transmission of data +and response from slave. Do not call functions that read from the serial +buffer that is used by ModbusMaster. Use of i2c/TWI, 1-Wire, other +serial ports, etc. is permitted within callback function. + +@see ModbusMaster::ModbusMasterTransaction() +*/ +void ModbusMaster::idle(void (*idle)()) +{ + _idle = idle; +} + + +/** +Retrieve data from response buffer. + +@see ModbusMaster::clearResponseBuffer() +@param u8Index index of response buffer array (0x00..0x3F) +@return value in position u8Index of response buffer (0x0000..0xFFFF) +@ingroup buffer +*/ +uint16_t ModbusMaster::getResponseBuffer(uint8_t u8Index) +{ + if (u8Index < ku8MaxBufferSize) + { + return _u16ResponseBuffer[u8Index]; + } + else + { + return 0xFFFF; + } +} + + +/** +Clear Modbus response buffer. + +@see ModbusMaster::getResponseBuffer(uint8_t u8Index) +@ingroup buffer +*/ +void ModbusMaster::clearResponseBuffer() +{ + uint8_t i; + + for (i = 0; i < ku8MaxBufferSize; i++) + { + _u16ResponseBuffer[i] = 0; + } +} + + +/** +Place data in transmit buffer. + +@see ModbusMaster::clearTransmitBuffer() +@param u8Index index of transmit buffer array (0x00..0x3F) +@param u16Value value to place in position u8Index of transmit buffer (0x0000..0xFFFF) +@return 0 on success; exception number on failure +@ingroup buffer +*/ +uint8_t ModbusMaster::setTransmitBuffer(uint8_t u8Index, uint16_t u16Value) +{ + if (u8Index < ku8MaxBufferSize) + { + _u16TransmitBuffer[u8Index] = u16Value; + return ku8MBSuccess; + } + else + { + return ku8MBIllegalDataAddress; + } +} + + +/** +Clear Modbus transmit buffer. + +@see ModbusMaster::setTransmitBuffer(uint8_t u8Index, uint16_t u16Value) +@ingroup buffer +*/ +void ModbusMaster::clearTransmitBuffer() +{ + uint8_t i; + + for (i = 0; i < ku8MaxBufferSize; i++) + { + _u16TransmitBuffer[i] = 0; + } +} + + +/** +Modbus function 0x01 Read Coils. + +This function code is used to read from 1 to 2000 contiguous status of +coils in a remote device. The request specifies the starting address, +i.e. the address of the first coil specified, and the number of coils. +Coils are addressed starting at zero. + +The coils in the response buffer are packed as one coil per bit of the +data field. Status is indicated as 1=ON and 0=OFF. The LSB of the first +data word contains the output addressed in the query. The other coils +follow toward the high order end of this word and from low order to high +order in subsequent words. + +If the returned quantity is not a multiple of sixteen, the remaining +bits in the final data word will be padded with zeros (toward the high +order end of the word). + +@param u16ReadAddress address of first coil (0x0000..0xFFFF) +@param u16BitQty quantity of coils to read (1..2000, enforced by remote device) +@return 0 on success; exception number on failure +@ingroup discrete +*/ +uint8_t ModbusMaster::readCoils(uint16_t u16ReadAddress, uint16_t u16BitQty) +{ + _u16ReadAddress = u16ReadAddress; + _u16ReadQty = u16BitQty; + return ModbusMasterTransaction(ku8MBReadCoils); +} + + +/** +Modbus function 0x02 Read Discrete Inputs. + +This function code is used to read from 1 to 2000 contiguous status of +discrete inputs in a remote device. The request specifies the starting +address, i.e. the address of the first input specified, and the number +of inputs. Discrete inputs are addressed starting at zero. + +The discrete inputs in the response buffer are packed as one input per +bit of the data field. Status is indicated as 1=ON; 0=OFF. The LSB of +the first data word contains the input addressed in the query. The other +inputs follow toward the high order end of this word, and from low order +to high order in subsequent words. + +If the returned quantity is not a multiple of sixteen, the remaining +bits in the final data word will be padded with zeros (toward the high +order end of the word). + +@param u16ReadAddress address of first discrete input (0x0000..0xFFFF) +@param u16BitQty quantity of discrete inputs to read (1..2000, enforced by remote device) +@return 0 on success; exception number on failure +@ingroup discrete +*/ +uint8_t ModbusMaster::readDiscreteInputs(uint16_t u16ReadAddress, + uint16_t u16BitQty) +{ + _u16ReadAddress = u16ReadAddress; + _u16ReadQty = u16BitQty; + return ModbusMasterTransaction(ku8MBReadDiscreteInputs); +} + + +/** +Modbus function 0x03 Read Holding Registers. + +This function code is used to read the contents of a contiguous block of +holding registers in a remote device. The request specifies the starting +register address and the number of registers. Registers are addressed +starting at zero. + +The register data in the response buffer is packed as one word per +register. + +@param u16ReadAddress address of the first holding register (0x0000..0xFFFF) +@param u16ReadQty quantity of holding registers to read (1..125, enforced by remote device) +@return 0 on success; exception number on failure +@ingroup register +*/ +uint8_t ModbusMaster::readHoldingRegisters(uint16_t u16ReadAddress, + uint16_t u16ReadQty) +{ + _u16ReadAddress = u16ReadAddress; + _u16ReadQty = u16ReadQty; + return ModbusMasterTransaction(ku8MBReadHoldingRegisters); +} + + +/** +Modbus function 0x04 Read Input Registers. + +This function code is used to read from 1 to 125 contiguous input +registers in a remote device. The request specifies the starting +register address and the number of registers. Registers are addressed +starting at zero. + +The register data in the response buffer is packed as one word per +register. + +@param u16ReadAddress address of the first input register (0x0000..0xFFFF) +@param u16ReadQty quantity of input registers to read (1..125, enforced by remote device) +@return 0 on success; exception number on failure +@ingroup register +*/ +uint8_t ModbusMaster::readInputRegisters(uint16_t u16ReadAddress, + uint8_t u16ReadQty) +{ + _u16ReadAddress = u16ReadAddress; + _u16ReadQty = u16ReadQty; + return ModbusMasterTransaction(ku8MBReadInputRegisters); +} + + +/** +Modbus function 0x05 Write Single Coil. + +This function code is used to write a single output to either ON or OFF +in a remote device. The requested ON/OFF state is specified by a +constant in the state field. A non-zero value requests the output to be +ON and a value of 0 requests it to be OFF. The request specifies the +address of the coil to be forced. Coils are addressed starting at zero. + +@param u16WriteAddress address of the coil (0x0000..0xFFFF) +@param u8State 0=OFF, non-zero=ON (0x00..0xFF) +@return 0 on success; exception number on failure +@ingroup discrete +*/ +uint8_t ModbusMaster::writeSingleCoil(uint16_t u16WriteAddress, uint8_t u8State) +{ + _u16WriteAddress = u16WriteAddress; + _u16WriteQty = (u8State ? 0xFF00 : 0x0000); + return ModbusMasterTransaction(ku8MBWriteSingleCoil); +} + + +/** +Modbus function 0x06 Write Single Register. + +This function code is used to write a single holding register in a +remote device. The request specifies the address of the register to be +written. Registers are addressed starting at zero. + +@param u16WriteAddress address of the holding register (0x0000..0xFFFF) +@param u16WriteValue value to be written to holding register (0x0000..0xFFFF) +@return 0 on success; exception number on failure +@ingroup register +*/ +uint8_t ModbusMaster::writeSingleRegister(uint16_t u16WriteAddress, + uint16_t u16WriteValue) +{ + _u16WriteAddress = u16WriteAddress; + _u16WriteQty = 0; + _u16TransmitBuffer[0] = u16WriteValue; + return ModbusMasterTransaction(ku8MBWriteSingleRegister); +} + + +/** +Modbus function 0x0F Write Multiple Coils. + +This function code is used to force each coil in a sequence of coils to +either ON or OFF in a remote device. The request specifies the coil +references to be forced. Coils are addressed starting at zero. + +The requested ON/OFF states are specified by contents of the transmit +buffer. A logical '1' in a bit position of the buffer requests the +corresponding output to be ON. A logical '0' requests it to be OFF. + +@param u16WriteAddress address of the first coil (0x0000..0xFFFF) +@param u16BitQty quantity of coils to write (1..2000, enforced by remote device) +@return 0 on success; exception number on failure +@ingroup discrete +*/ +uint8_t ModbusMaster::writeMultipleCoils(uint16_t u16WriteAddress, + uint16_t u16BitQty) +{ + _u16WriteAddress = u16WriteAddress; + _u16WriteQty = u16BitQty; + return ModbusMasterTransaction(ku8MBWriteMultipleCoils); +} +uint8_t ModbusMaster::writeMultipleCoils() +{ + _u16WriteQty = u16TransmitBufferLength; + return ModbusMasterTransaction(ku8MBWriteMultipleCoils); +} + + +/** +Modbus function 0x10 Write Multiple Registers. + +This function code is used to write a block of contiguous registers (1 +to 123 registers) in a remote device. + +The requested written values are specified in the transmit buffer. Data +is packed as one word per register. + +@param u16WriteAddress address of the holding register (0x0000..0xFFFF) +@param u16WriteQty quantity of holding registers to write (1..123, enforced by remote device) +@return 0 on success; exception number on failure +@ingroup register +*/ +uint8_t ModbusMaster::writeMultipleRegisters(uint16_t u16WriteAddress, + uint16_t u16WriteQty) +{ + _u16WriteAddress = u16WriteAddress; + _u16WriteQty = u16WriteQty; + return ModbusMasterTransaction(ku8MBWriteMultipleRegisters); +} + +// new version based on Wire.h +uint8_t ModbusMaster::writeMultipleRegisters() +{ + _u16WriteQty = _u8TransmitBufferIndex; + return ModbusMasterTransaction(ku8MBWriteMultipleRegisters); +} + + +/** +Modbus function 0x16 Mask Write Register. + +This function code is used to modify the contents of a specified holding +register using a combination of an AND mask, an OR mask, and the +register's current contents. The function can be used to set or clear +individual bits in the register. + +The request specifies the holding register to be written, the data to be +used as the AND mask, and the data to be used as the OR mask. Registers +are addressed starting at zero. + +The function's algorithm is: + +Result = (Current Contents && And_Mask) || (Or_Mask && (~And_Mask)) + +@param u16WriteAddress address of the holding register (0x0000..0xFFFF) +@param u16AndMask AND mask (0x0000..0xFFFF) +@param u16OrMask OR mask (0x0000..0xFFFF) +@return 0 on success; exception number on failure +@ingroup register +*/ +uint8_t ModbusMaster::maskWriteRegister(uint16_t u16WriteAddress, + uint16_t u16AndMask, uint16_t u16OrMask) +{ + _u16WriteAddress = u16WriteAddress; + _u16TransmitBuffer[0] = u16AndMask; + _u16TransmitBuffer[1] = u16OrMask; + return ModbusMasterTransaction(ku8MBMaskWriteRegister); +} + + +/** +Modbus function 0x17 Read Write Multiple Registers. + +This function code performs a combination of one read operation and one +write operation in a single MODBUS transaction. The write operation is +performed before the read. Holding registers are addressed starting at +zero. + +The request specifies the starting address and number of holding +registers to be read as well as the starting address, and the number of +holding registers. The data to be written is specified in the transmit +buffer. + +@param u16ReadAddress address of the first holding register (0x0000..0xFFFF) +@param u16ReadQty quantity of holding registers to read (1..125, enforced by remote device) +@param u16WriteAddress address of the first holding register (0x0000..0xFFFF) +@param u16WriteQty quantity of holding registers to write (1..121, enforced by remote device) +@return 0 on success; exception number on failure +@ingroup register +*/ +uint8_t ModbusMaster::readWriteMultipleRegisters(uint16_t u16ReadAddress, + uint16_t u16ReadQty, uint16_t u16WriteAddress, uint16_t u16WriteQty) +{ + _u16ReadAddress = u16ReadAddress; + _u16ReadQty = u16ReadQty; + _u16WriteAddress = u16WriteAddress; + _u16WriteQty = u16WriteQty; + return ModbusMasterTransaction(ku8MBReadWriteMultipleRegisters); +} +uint8_t ModbusMaster::readWriteMultipleRegisters(uint16_t u16ReadAddress, + uint16_t u16ReadQty) +{ + _u16ReadAddress = u16ReadAddress; + _u16ReadQty = u16ReadQty; + _u16WriteQty = _u8TransmitBufferIndex; + return ModbusMasterTransaction(ku8MBReadWriteMultipleRegisters); +} + + +/* _____PRIVATE FUNCTIONS____________________________________________________ */ +/** +Modbus transaction engine. +Sequence: + - assemble Modbus Request Application Data Unit (ADU), + based on particular function called + - transmit request over selected serial port + - wait for/retrieve response + - evaluate/disassemble response + - return status (success/exception) + +@param u8MBFunction Modbus function (0x01..0xFF) +@return 0 on success; exception number on failure +*/ +uint8_t ModbusMaster::ModbusMasterTransaction(uint8_t u8MBFunction) +{ + uint8_t u8ModbusADU[256]; + uint8_t u8ModbusADUSize = 0; + uint8_t i, u8Qty; + uint16_t u16CRC; + uint32_t u32StartTime; + uint8_t u8BytesLeft = 8; + uint8_t u8MBStatus = ku8MBSuccess; + + // assemble Modbus Request Application Data Unit + u8ModbusADU[u8ModbusADUSize++] = _u8MBSlave; + u8ModbusADU[u8ModbusADUSize++] = u8MBFunction; + + switch(u8MBFunction) + { + case ku8MBReadCoils: + case ku8MBReadDiscreteInputs: + case ku8MBReadInputRegisters: + case ku8MBReadHoldingRegisters: + case ku8MBReadWriteMultipleRegisters: + u8ModbusADU[u8ModbusADUSize++] = highByte(_u16ReadAddress); + u8ModbusADU[u8ModbusADUSize++] = lowByte(_u16ReadAddress); + u8ModbusADU[u8ModbusADUSize++] = highByte(_u16ReadQty); + u8ModbusADU[u8ModbusADUSize++] = lowByte(_u16ReadQty); + break; + } + + switch(u8MBFunction) + { + case ku8MBWriteSingleCoil: + case ku8MBMaskWriteRegister: + case ku8MBWriteMultipleCoils: + case ku8MBWriteSingleRegister: + case ku8MBWriteMultipleRegisters: + case ku8MBReadWriteMultipleRegisters: + u8ModbusADU[u8ModbusADUSize++] = highByte(_u16WriteAddress); + u8ModbusADU[u8ModbusADUSize++] = lowByte(_u16WriteAddress); + break; + } + + switch(u8MBFunction) + { + case ku8MBWriteSingleCoil: + u8ModbusADU[u8ModbusADUSize++] = highByte(_u16WriteQty); + u8ModbusADU[u8ModbusADUSize++] = lowByte(_u16WriteQty); + break; + + case ku8MBWriteSingleRegister: + u8ModbusADU[u8ModbusADUSize++] = highByte(_u16TransmitBuffer[0]); + u8ModbusADU[u8ModbusADUSize++] = lowByte(_u16TransmitBuffer[0]); + break; + + case ku8MBWriteMultipleCoils: + u8ModbusADU[u8ModbusADUSize++] = highByte(_u16WriteQty); + u8ModbusADU[u8ModbusADUSize++] = lowByte(_u16WriteQty); + u8Qty = (_u16WriteQty % 8) ? ((_u16WriteQty >> 3) + 1) : (_u16WriteQty >> 3); + u8ModbusADU[u8ModbusADUSize++] = u8Qty; + for (i = 0; i < u8Qty; i++) + { + switch(i % 2) + { + case 0: // i is even + u8ModbusADU[u8ModbusADUSize++] = lowByte(_u16TransmitBuffer[i >> 1]); + break; + + case 1: // i is odd + u8ModbusADU[u8ModbusADUSize++] = highByte(_u16TransmitBuffer[i >> 1]); + break; + } + } + break; + + case ku8MBWriteMultipleRegisters: + case ku8MBReadWriteMultipleRegisters: + u8ModbusADU[u8ModbusADUSize++] = highByte(_u16WriteQty); + u8ModbusADU[u8ModbusADUSize++] = lowByte(_u16WriteQty); + u8ModbusADU[u8ModbusADUSize++] = lowByte(_u16WriteQty << 1); + + for (i = 0; i < lowByte(_u16WriteQty); i++) + { + u8ModbusADU[u8ModbusADUSize++] = highByte(_u16TransmitBuffer[i]); + u8ModbusADU[u8ModbusADUSize++] = lowByte(_u16TransmitBuffer[i]); + } + break; + + case ku8MBMaskWriteRegister: + u8ModbusADU[u8ModbusADUSize++] = highByte(_u16TransmitBuffer[0]); + u8ModbusADU[u8ModbusADUSize++] = lowByte(_u16TransmitBuffer[0]); + u8ModbusADU[u8ModbusADUSize++] = highByte(_u16TransmitBuffer[1]); + u8ModbusADU[u8ModbusADUSize++] = lowByte(_u16TransmitBuffer[1]); + break; + } + + // append CRC + u16CRC = 0xFFFF; + for (i = 0; i < u8ModbusADUSize; i++) + { + u16CRC = crc16_update(u16CRC, u8ModbusADU[i]); + } + u8ModbusADU[u8ModbusADUSize++] = lowByte(u16CRC); + u8ModbusADU[u8ModbusADUSize++] = highByte(u16CRC); + u8ModbusADU[u8ModbusADUSize] = 0; + + // flush receive buffer before transmitting request + while (MBSerial->read() != -1); + +#if 0 + // transmit request + for (i = 0; i < u8ModbusADUSize; i++) + { +#if defined(ARDUINO) && ARDUINO >= 100 + MBSerial->write(u8ModbusADU[i]); +#else + MBSerial->print(u8ModbusADU[i], BYTE); +#endif + } +#else + MBSerial->write((char *)u8ModbusADU, u8ModbusADUSize); +#endif + //printf("TX: %02X\n", u8ModbusADU[0]); + u8ModbusADUSize = 0; + MBSerial->flush(); // flush transmit buffer + + // loop until we run out of time or bytes, or an error occurs + u32StartTime = millis(); + while (u8BytesLeft && !u8MBStatus) + { + if (MBSerial->available()) + { +#if __MODBUSMASTER_DEBUG__ + digitalWrite(4, true); +#endif + u8ModbusADU[u8ModbusADUSize++] = MBSerial->read(); + u8BytesLeft--; +#if __MODBUSMASTER_DEBUG__ + digitalWrite(4, false); +#endif + } + else + { +#if __MODBUSMASTER_DEBUG__ + digitalWrite(5, true); +#endif + if (_idle) + { + _idle(); + } +#if __MODBUSMASTER_DEBUG__ + digitalWrite(5, false); +#endif + } + + // evaluate slave ID, function code once enough bytes have been read + if (u8ModbusADUSize == 5) + { + // verify response is for correct Modbus slave + if (u8ModbusADU[0] != _u8MBSlave) + { + u8MBStatus = ku8MBInvalidSlaveID; + break; + } + + // verify response is for correct Modbus function code (mask exception bit 7) + if ((u8ModbusADU[1] & 0x7F) != u8MBFunction) + { + u8MBStatus = ku8MBInvalidFunction; + break; + } + + // check whether Modbus exception occurred; return Modbus Exception Code + if (bitRead(u8ModbusADU[1], 7)) + { + u8MBStatus = u8ModbusADU[2]; + break; + } + + // evaluate returned Modbus function code + switch(u8ModbusADU[1]) + { + case ku8MBReadCoils: + case ku8MBReadDiscreteInputs: + case ku8MBReadInputRegisters: + case ku8MBReadHoldingRegisters: + case ku8MBReadWriteMultipleRegisters: + u8BytesLeft = u8ModbusADU[2]; + break; + + case ku8MBWriteSingleCoil: + case ku8MBWriteMultipleCoils: + case ku8MBWriteSingleRegister: + case ku8MBWriteMultipleRegisters: + u8BytesLeft = 3; + break; + + case ku8MBMaskWriteRegister: + u8BytesLeft = 5; + break; + } + } + if ((millis() - u32StartTime) > ku16MBResponseTimeout) + { + u8MBStatus = ku8MBResponseTimedOut; + } + } + + // verify response is large enough to inspect further + if (!u8MBStatus && u8ModbusADUSize >= 5) + { + // calculate CRC + u16CRC = 0xFFFF; + for (i = 0; i < (u8ModbusADUSize - 2); i++) + { + u16CRC = crc16_update(u16CRC, u8ModbusADU[i]); + } + + // verify CRC + if (!u8MBStatus && (lowByte(u16CRC) != u8ModbusADU[u8ModbusADUSize - 2] || + highByte(u16CRC) != u8ModbusADU[u8ModbusADUSize - 1])) + { + u8MBStatus = ku8MBInvalidCRC; + } + } + + // disassemble ADU into words + if (!u8MBStatus) + { + // evaluate returned Modbus function code + switch(u8ModbusADU[1]) + { + case ku8MBReadCoils: + case ku8MBReadDiscreteInputs: + // load bytes into word; response bytes are ordered L, H, L, H, ... + for (i = 0; i < (u8ModbusADU[2] >> 1); i++) + { + if (i < ku8MaxBufferSize) + { + _u16ResponseBuffer[i] = word(u8ModbusADU[2 * i + 4], u8ModbusADU[2 * i + 3]); + } + + _u8ResponseBufferLength = i; + } + + // in the event of an odd number of bytes, load last byte into zero-padded word + if (u8ModbusADU[2] % 2) + { + if (i < ku8MaxBufferSize) + { + _u16ResponseBuffer[i] = word(0, u8ModbusADU[2 * i + 3]); + } + + _u8ResponseBufferLength = i + 1; + } + break; + + case ku8MBReadInputRegisters: + case ku8MBReadHoldingRegisters: + case ku8MBReadWriteMultipleRegisters: + // load bytes into word; response bytes are ordered H, L, H, L, ... + for (i = 0; i < (u8ModbusADU[2] >> 1); i++) + { + if (i < ku8MaxBufferSize) + { + _u16ResponseBuffer[i] = word(u8ModbusADU[2 * i + 3], u8ModbusADU[2 * i + 4]); + } + + _u8ResponseBufferLength = i; + } + break; + } + } + + _u8TransmitBufferIndex = 0; + u16TransmitBufferLength = 0; + _u8ResponseBufferIndex = 0; + return u8MBStatus; +} From a625f689102eb7fb817ff82bfd29913cfb163c3e Mon Sep 17 00:00:00 2001 From: jaakkoiot Date: Sat, 15 Oct 2022 15:20:09 +0300 Subject: [PATCH 05/17] Add data structure ie. word for modbus comms --- Modbus/inc/word.h | 97 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 97 insertions(+) create mode 100644 Modbus/inc/word.h diff --git a/Modbus/inc/word.h b/Modbus/inc/word.h new file mode 100644 index 0000000..667fd4a --- /dev/null +++ b/Modbus/inc/word.h @@ -0,0 +1,97 @@ +/** +@file +Utility Functions for Manipulating Words + +@defgroup util_word "util/word.h": Utility Functions for Manipulating Words +@code#include "util/word.h"@endcode + +This header file provides utility functions for manipulating words. + +*/ +/* + + word.h - Utility Functions for Manipulating Words + + This file is part of ModbusMaster. + + ModbusMaster is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + ModbusMaster is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with ModbusMaster. If not, see . + + Written by Doc Walker (Rx) + Copyright © 2009-2015 Doc Walker <4-20ma at wvfans dot net> + +*/ + + +#ifndef _UTIL_WORD_H_ +#define _UTIL_WORD_H_ +#include + +/** @ingroup util_word + Return low word of a 32-bit integer. + + @param uint32_t ww (0x00000000..0xFFFFFFFF) + @return low word of input (0x0000..0xFFFF) +*/ +static inline uint16_t lowWord(uint32_t ww) { +return (uint16_t) ((ww) & 0xFFFF); +} + + +/** @ingroup util_word + Return high word of a 32-bit integer. + + @param uint32_t ww (0x00000000..0xFFFFFFFF) + @return high word of input (0x0000..0xFFFF) +*/ +static inline uint16_t highWord(uint32_t ww) +{ + return (uint16_t) ((ww) >> 16); +} + +/* utility functions for porting ModbusMaster to LPCXpresso + * added by krl + */ +static inline uint16_t word(uint8_t ww) +{ + return (uint16_t) (ww); +} + +static inline uint16_t word(uint8_t h, uint8_t l) +{ + return (uint16_t) ((h << 8) | l); +} + +static inline uint8_t highByte(uint16_t v) +{ + return (uint8_t) ((v >> 8) & 0xFF); +} + +static inline uint16_t lowByte(uint16_t v) +{ + return (uint8_t) (v & 0xFF); +} + +static inline uint8_t bitRead(uint8_t v, uint8_t n) +{ + return (uint8_t) (v & (1 << n) ? 1 : 0); +} + +static inline void bitWrite(uint16_t& v, uint8_t n, uint8_t b) +{ + if(b) v = v | (1 << n); + else v = v & ~(1 << n); +} + + +#endif /* _UTIL_WORD_H_ */ From 5f0d233a82f5f9bb6d87d27aef5d9a7fa65407eb Mon Sep 17 00:00:00 2001 From: jaakkoiot Date: Sat, 15 Oct 2022 15:21:28 +0300 Subject: [PATCH 06/17] 16 bit CRC error checking for modbus --- Modbus/inc/crc16.h | 88 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 88 insertions(+) create mode 100644 Modbus/inc/crc16.h diff --git a/Modbus/inc/crc16.h b/Modbus/inc/crc16.h new file mode 100644 index 0000000..0f50fdd --- /dev/null +++ b/Modbus/inc/crc16.h @@ -0,0 +1,88 @@ +/** +@file +CRC Computations + +@defgroup util_crc16 "util/crc16.h": CRC Computations +@code#include "util/crc16.h"@endcode + +This header file provides functions for calculating +cyclic redundancy checks (CRC) using common polynomials. +Modified by Doc Walker to be processor-independent (removed inline +assembler to allow it to compile on SAM3X8E processors). + +@par References: +Jack Crenshaw's "Implementing CRCs" article in the January 1992 issue of @e +Embedded @e Systems @e Programming. This may be difficult to find, but it +explains CRC's in very clear and concise terms. Well worth the effort to +obtain a copy. + +*/ +/* Copyright (c) 2002, 2003, 2004 Marek Michalkiewicz + Copyright (c) 2005, 2007 Joerg Wunsch + Copyright (c) 2013 Dave Hylands + Copyright (c) 2013 Frederic Nadeau + Copyright (c) 2015 Doc Walker + All rights reserved. + + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in + the documentation and/or other materials provided with the + distribution. + + * Neither the name of the copyright holders nor the names of + contributors may be used to endorse or promote products derived + from this software without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + POSSIBILITY OF SUCH DAMAGE. */ + + +#ifndef _UTIL_CRC16_H_ +#define _UTIL_CRC16_H_ +#include + +/** @ingroup util_crc16 + Processor-independent CRC-16 calculation. + + Polynomial: x^16 + x^15 + x^2 + 1 (0xA001)
+ Initial value: 0xFFFF + + This CRC is normally used in disk-drive controllers. + + @param uint16_t crc (0x0000..0xFFFF) + @param uint8_t a (0x00..0xFF) + @return calculated CRC (0x0000..0xFFFF) +*/ +static uint16_t crc16_update(uint16_t crc, uint8_t a) +{ + int i; + + crc ^= a; + for (i = 0; i < 8; ++i) + { + if (crc & 1) + crc = (crc >> 1) ^ 0xA001; + else + crc = (crc >> 1); + } + + return crc; +} + + +#endif /* _UTIL_CRC16_H_ */ From bada194ebe7490c25ca7589366224d4ae0f80eab Mon Sep 17 00:00:00 2001 From: jaakkoiot Date: Mon, 17 Oct 2022 09:15:24 +0300 Subject: [PATCH 07/17] Modbus changed into static library --- Modbus/.cproject | 186 ++++++++++++++++++ Modbus/.project | 3 - Modbus/inc/{ModbusLeader.h => ModbusMaster.h} | 5 +- Modbus/inc/ModbusRegister.h | 2 +- Modbus/inc/SerialPort.h | 2 + Modbus/inc/Uart.h | 1 - Modbus/inc/crc16.h | 2 +- Modbus/inc/word.h | 7 +- .../{ModbusLeader.cpp => ModbusMaster.cpp} | 2 +- Modbus/src/SerialPort.cpp | 1 - Modbus/src/Uart.cpp | 13 -- Modbus/src/tester.cpp | 45 ----- 12 files changed, 198 insertions(+), 71 deletions(-) create mode 100644 Modbus/.cproject rename Modbus/inc/{ModbusLeader.h => ModbusMaster.h} (99%) rename Modbus/src/{ModbusLeader.cpp => ModbusMaster.cpp} (99%) delete mode 100644 Modbus/src/tester.cpp diff --git a/Modbus/.cproject b/Modbus/.cproject new file mode 100644 index 0000000..5e35dbd --- /dev/null +++ b/Modbus/.cproject @@ -0,0 +1,186 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + <?xml version="1.0" encoding="UTF-8"?> +<TargetConfig> +<Properties property_2="LPC15xx_256K.cfx" property_3="NXP" property_4="LPC1549" property_count="5" version="100300"/> +<infoList vendor="NXP"> +<info chip="LPC1549" connectscript="LPC15RunBootRomConnect.scp" flash_driver="LPC15xx_256K.cfx" match_id="0x0" name="LPC1549" resetscript="LPC15RunBootRomReset.scp" stub="crt_emu_cm3_gen"> +<chip> +<name>LPC1549</name> +<family>LPC15xx</family> +<vendor>NXP (formerly Philips)</vendor> +<reset board="None" core="Real" sys="Real"/> +<clock changeable="TRUE" freq="12MHz" is_accurate="TRUE"/> +<memory can_program="true" id="Flash" is_ro="true" type="Flash"/> +<memory id="RAM" type="RAM"/> +<memory id="Periph" is_volatile="true" type="Peripheral"/> +<memoryInstance derived_from="Flash" id="MFlash256" location="0x0" size="0x40000"/> +<memoryInstance derived_from="RAM" id="Ram0_16" location="0x2000000" size="0x4000"/> +<memoryInstance derived_from="RAM" id="Ram1_16" location="0x2004000" size="0x4000"/> +<memoryInstance derived_from="RAM" id="Ram2_4" location="0x2008000" size="0x1000"/> +</chip> +<processor> +<name gcc_name="cortex-m3">Cortex-M3</name> +<family>Cortex-M</family> +</processor> +</info> +</infoList> +</TargetConfig> + + + LPCXpresso1549 + + diff --git a/Modbus/.project b/Modbus/.project index c7d3ce5..3e28dd2 100644 --- a/Modbus/.project +++ b/Modbus/.project @@ -3,8 +3,6 @@ Modbus - lpc_chip_15xx - DigitalIoPin @@ -22,7 +20,6 @@ org.eclipse.cdt.core.cnature - org.eclipse.cdt.core.ccnature org.eclipse.cdt.managedbuilder.core.managedBuildNature org.eclipse.cdt.managedbuilder.core.ScannerConfigNature diff --git a/Modbus/inc/ModbusLeader.h b/Modbus/inc/ModbusMaster.h similarity index 99% rename from Modbus/inc/ModbusLeader.h rename to Modbus/inc/ModbusMaster.h index d038569..18e7978 100644 --- a/Modbus/inc/ModbusLeader.h +++ b/Modbus/inc/ModbusMaster.h @@ -56,7 +56,6 @@ Set to 1 to enable debugging features within class: #include #include #endif -#include uint32_t millis(); #define BYTE 0xA5 @@ -72,6 +71,8 @@ uint32_t millis(); // functions to manipulate words ///#include "util/word.h" #include "word.h" + + #include "SerialPort.h" /* _____CLASS DEFINITIONS____________________________________________________ */ @@ -278,4 +279,4 @@ class ModbusMaster /** @example examples/Basic/Basic.pde @example examples/PhoenixContact_nanoLC/PhoenixContact_nanoLC.pde -*/ \ No newline at end of file +*/ diff --git a/Modbus/inc/ModbusRegister.h b/Modbus/inc/ModbusRegister.h index af20be9..198d94e 100644 --- a/Modbus/inc/ModbusRegister.h +++ b/Modbus/inc/ModbusRegister.h @@ -1,7 +1,7 @@ #ifndef MODBUSREGISTER_H_ #define MODBUSREGISTER_H_ -#include "ModbusLeader.h" +#include "ModbusMaster.h" class ModbusRegister { public: diff --git a/Modbus/inc/SerialPort.h b/Modbus/inc/SerialPort.h index 6e89b6e..f84581d 100644 --- a/Modbus/inc/SerialPort.h +++ b/Modbus/inc/SerialPort.h @@ -1,3 +1,5 @@ + + #ifndef SERIALPORT_H_ #define SERIALPORT_H_ diff --git a/Modbus/inc/Uart.h b/Modbus/inc/Uart.h index ce3c007..c72ac3b 100644 --- a/Modbus/inc/Uart.h +++ b/Modbus/inc/Uart.h @@ -8,7 +8,6 @@ #ifndef LPCUART_H_ #define LPCUART_H_ -#include #include "chip.h" struct LpcPinMap { diff --git a/Modbus/inc/crc16.h b/Modbus/inc/crc16.h index 0f50fdd..27ea89e 100644 --- a/Modbus/inc/crc16.h +++ b/Modbus/inc/crc16.h @@ -54,7 +54,7 @@ obtain a copy. #ifndef _UTIL_CRC16_H_ #define _UTIL_CRC16_H_ -#include + /** @ingroup util_crc16 Processor-independent CRC-16 calculation. diff --git a/Modbus/inc/word.h b/Modbus/inc/word.h index 667fd4a..78db36c 100644 --- a/Modbus/inc/word.h +++ b/Modbus/inc/word.h @@ -35,7 +35,7 @@ This header file provides utility functions for manipulating words. #ifndef _UTIL_WORD_H_ #define _UTIL_WORD_H_ -#include + /** @ingroup util_word Return low word of a 32-bit integer. @@ -43,8 +43,9 @@ This header file provides utility functions for manipulating words. @param uint32_t ww (0x00000000..0xFFFFFFFF) @return low word of input (0x0000..0xFFFF) */ -static inline uint16_t lowWord(uint32_t ww) { -return (uint16_t) ((ww) & 0xFFFF); +static inline uint16_t lowWord(uint32_t ww) +{ + return (uint16_t) ((ww) & 0xFFFF); } diff --git a/Modbus/src/ModbusLeader.cpp b/Modbus/src/ModbusMaster.cpp similarity index 99% rename from Modbus/src/ModbusLeader.cpp rename to Modbus/src/ModbusMaster.cpp index 92a8958..4aef169 100644 --- a/Modbus/src/ModbusLeader.cpp +++ b/Modbus/src/ModbusMaster.cpp @@ -29,7 +29,7 @@ Arduino library for communicating with Modbus slaves over RS232/485 (via RTU pro /* _____PROJECT INCLUDES_____________________________________________________ */ -#include "ModbusLeader.h" +#include "ModbusMaster.h" #include "crc16.h" diff --git a/Modbus/src/SerialPort.cpp b/Modbus/src/SerialPort.cpp index 953d99a..36084a4 100644 --- a/Modbus/src/SerialPort.cpp +++ b/Modbus/src/SerialPort.cpp @@ -1,7 +1,6 @@ #include "SerialPort.h" - SerialPort::SerialPort() { if(!u) { LpcPinMap none = {-1, -1}; // unused pin has negative values in it diff --git a/Modbus/src/Uart.cpp b/Modbus/src/Uart.cpp index 6a45cae..87b725a 100644 --- a/Modbus/src/Uart.cpp +++ b/Modbus/src/Uart.cpp @@ -1,7 +1,6 @@ #include #include "Uart.h" - static LpcUart *u0; static LpcUart *u1; static LpcUart *u2; @@ -13,28 +12,16 @@ extern "C" { */ void UART0_IRQHandler(void) { - /* Want to handle any errors? Do it here. */ - - /* Use default ring buffer handler. Override this with your own - code if you need more capability. */ if(u0) u0->isr(); } void UART1_IRQHandler(void) { - /* Want to handle any errors? Do it here. */ - - /* Use default ring buffer handler. Override this with your own - code if you need more capability. */ if(u1) u1->isr(); } void UART2_IRQHandler(void) { - /* Want to handle any errors? Do it here. */ - - /* Use default ring buffer handler. Override this with your own - code if you need more capability. */ if(u2) u2->isr(); } diff --git a/Modbus/src/tester.cpp b/Modbus/src/tester.cpp deleted file mode 100644 index a2935ea..0000000 --- a/Modbus/src/tester.cpp +++ /dev/null @@ -1,45 +0,0 @@ -#if defined (__USE_LPCOPEN) -#if defined(NO_BOARD_LIB) -#include "chip.h" -#else -#include "board.h" -#endif -#endif - -#include -#include "ModbusMaster.h" -// TODO: insert other include files here - -// TODO: insert other definitions and declarations here - -int main(void) { - -#if defined (__USE_LPCOPEN) - // Read clock settings and update SystemCoreClock variable - SystemCoreClockUpdate(); -#if !defined(NO_BOARD_LIB) - // Set up and initialize all required blocks and - // functions related to the board hardware - Board_Init(); - // Set the LED to the state of "On" - Board_LED_Set(0, true); -#endif -#endif - - // TODO: insert code here - ModbusMaster MIO_12V(1); - ModbusMaster CO2_sensor(240); - ModbusMaster RH_sensor(241); - - - // Force the counter to be placed into memory - volatile static int i = 0 ; - // Enter an infinite loop, just incrementing a counter - while(1) { - i++ ; - // "Dummy" NOP to allow source level single - // stepping of tight while() loop - __asm volatile ("nop"); - } - return 0 ; -} From 1c42148a642f1a10fa4647d22970c3ad2f5ac61c Mon Sep 17 00:00:00 2001 From: jaakkoiot Date: Sat, 15 Oct 2022 15:12:26 +0300 Subject: [PATCH 08/17] Add modbus leader class & tester --- Modbus/.project | 29 ++++ Modbus/inc/ModbusLeader.h | 281 ++++++++++++++++++++++++++++++++++++++ Modbus/liblinks.xml | 32 +++++ Modbus/src/tester.cpp | 45 ++++++ 4 files changed, 387 insertions(+) create mode 100644 Modbus/.project create mode 100644 Modbus/inc/ModbusLeader.h create mode 100644 Modbus/liblinks.xml create mode 100644 Modbus/src/tester.cpp diff --git a/Modbus/.project b/Modbus/.project new file mode 100644 index 0000000..c7d3ce5 --- /dev/null +++ b/Modbus/.project @@ -0,0 +1,29 @@ + + + Modbus + + + lpc_chip_15xx + DigitalIoPin + + + + org.eclipse.cdt.managedbuilder.core.genmakebuilder + clean,full,incremental, + + + + + org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder + full,incremental, + + + + + + org.eclipse.cdt.core.cnature + org.eclipse.cdt.core.ccnature + org.eclipse.cdt.managedbuilder.core.managedBuildNature + org.eclipse.cdt.managedbuilder.core.ScannerConfigNature + + diff --git a/Modbus/inc/ModbusLeader.h b/Modbus/inc/ModbusLeader.h new file mode 100644 index 0000000..d038569 --- /dev/null +++ b/Modbus/inc/ModbusLeader.h @@ -0,0 +1,281 @@ +/** +@file +Arduino library for communicating with Modbus slaves over RS232/485 (via RTU protocol). + +@defgroup setup ModbusMaster Object Instantiation/Initialization +@defgroup buffer ModbusMaster Buffer Management +@defgroup discrete Modbus Function Codes for Discrete Coils/Inputs +@defgroup register Modbus Function Codes for Holding/Input Registers +@defgroup constant Modbus Function Codes, Exception Codes +*/ +/* + + ModbusMaster.h - Arduino library for communicating with Modbus slaves + over RS232/485 (via RTU protocol). + + This file is part of ModbusMaster. + + ModbusMaster is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + ModbusMaster is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with ModbusMaster. If not, see . + + Written by Doc Walker (Rx) + Copyright © 2009-2013 Doc Walker <4-20ma at wvfans dot net> + +*/ + + +#ifndef ModbusMaster_h +#define ModbusMaster_h + + +/** +@def __MODBUSMASTER_DEBUG__ (1). +Set to 1 to enable debugging features within class: + - pin 4 cycles for each byte read in the Modbus response + - pin 5 cycles for each millisecond timeout during the Modbus response +*/ +#define __MODBUSMASTER_DEBUG__ (0) + + +/* _____STANDARD INCLUDES____________________________________________________ */ +// include types & constants of Wiring core API +#if defined(ARDUINO) && ARDUINO >= 100 +#include "Arduino.h" +#else +//#include "WProgram.h" +#include +#include +#endif +#include + +uint32_t millis(); +#define BYTE 0xA5 + +/* _____UTILITY MACROS_______________________________________________________ */ + + +/* _____PROJECT INCLUDES_____________________________________________________ */ +// functions to calculate Modbus Application Data Unit CRC +//#include "util/crc16.h" +// moved inlcuding crc16.h to ModbusMaster.cpp + +// functions to manipulate words +///#include "util/word.h" +#include "word.h" +#include "SerialPort.h" + +/* _____CLASS DEFINITIONS____________________________________________________ */ +/** +Arduino class library for communicating with Modbus slaves over +RS232/485 (via RTU protocol). +*/ +class ModbusMaster +{ + public: + ModbusMaster(); + ModbusMaster(uint8_t); + ModbusMaster(uint8_t, uint8_t); + + void begin(); + void begin(uint16_t); + void idle(void (*)()); + + // Modbus exception codes + /** + Modbus protocol illegal function exception. + + The function code received in the query is not an allowable action for + the server (or slave). This may be because the function code is only + applicable to newer devices, and was not implemented in the unit + selected. It could also indicate that the server (or slave) is in the + wrong state to process a request of this type, for example because it is + unconfigured and is being asked to return register values. + + @ingroup constant + */ + static const uint8_t ku8MBIllegalFunction = 0x01; + + /** + Modbus protocol illegal data address exception. + + The data address received in the query is not an allowable address for + the server (or slave). More specifically, the combination of reference + number and transfer length is invalid. For a controller with 100 + registers, the ADU addresses the first register as 0, and the last one + as 99. If a request is submitted with a starting register address of 96 + and a quantity of registers of 4, then this request will successfully + operate (address-wise at least) on registers 96, 97, 98, 99. If a + request is submitted with a starting register address of 96 and a + quantity of registers of 5, then this request will fail with Exception + Code 0x02 "Illegal Data Address" since it attempts to operate on + registers 96, 97, 98, 99 and 100, and there is no register with address + 100. + + @ingroup constant + */ + static const uint8_t ku8MBIllegalDataAddress = 0x02; + + /** + Modbus protocol illegal data value exception. + + A value contained in the query data field is not an allowable value for + server (or slave). This indicates a fault in the structure of the + remainder of a complex request, such as that the implied length is + incorrect. It specifically does NOT mean that a data item submitted for + storage in a register has a value outside the expectation of the + application program, since the MODBUS protocol is unaware of the + significance of any particular value of any particular register. + + @ingroup constant + */ + static const uint8_t ku8MBIllegalDataValue = 0x03; + + /** + Modbus protocol slave device failure exception. + + An unrecoverable error occurred while the server (or slave) was + attempting to perform the requested action. + + @ingroup constant + */ + static const uint8_t ku8MBSlaveDeviceFailure = 0x04; + + // Class-defined success/exception codes + /** + ModbusMaster success. + + Modbus transaction was successful; the following checks were valid: + - slave ID + - function code + - response code + - data + - CRC + + @ingroup constant + */ + static const uint8_t ku8MBSuccess = 0x00; + + /** + ModbusMaster invalid response slave ID exception. + + The slave ID in the response does not match that of the request. + + @ingroup constant + */ + static const uint8_t ku8MBInvalidSlaveID = 0xE0; + + /** + ModbusMaster invalid response function exception. + + The function code in the response does not match that of the request. + + @ingroup constant + */ + static const uint8_t ku8MBInvalidFunction = 0xE1; + + /** + ModbusMaster response timed out exception. + + The entire response was not received within the timeout period, + ModbusMaster::ku8MBResponseTimeout. + + @ingroup constant + */ + static const uint8_t ku8MBResponseTimedOut = 0xE2; + + /** + ModbusMaster invalid response CRC exception. + + The CRC in the response does not match the one calculated. + + @ingroup constant + */ + static const uint8_t ku8MBInvalidCRC = 0xE3; + + uint16_t getResponseBuffer(uint8_t); + void clearResponseBuffer(); + uint8_t setTransmitBuffer(uint8_t, uint16_t); + void clearTransmitBuffer(); + + void beginTransmission(uint16_t); + uint8_t requestFrom(uint16_t, uint16_t); + void sendBit(bool); + void send(uint8_t); + void send(uint16_t); + void send(uint32_t); + uint8_t available(void); + uint16_t receive(void); + + + uint8_t readCoils(uint16_t, uint16_t); + uint8_t readDiscreteInputs(uint16_t, uint16_t); + uint8_t readHoldingRegisters(uint16_t, uint16_t); + uint8_t readInputRegisters(uint16_t, uint8_t); + uint8_t writeSingleCoil(uint16_t, uint8_t); + uint8_t writeSingleRegister(uint16_t, uint16_t); + uint8_t writeMultipleCoils(uint16_t, uint16_t); + uint8_t writeMultipleCoils(); + uint8_t writeMultipleRegisters(uint16_t, uint16_t); + uint8_t writeMultipleRegisters(); + uint8_t maskWriteRegister(uint16_t, uint16_t, uint16_t); + uint8_t readWriteMultipleRegisters(uint16_t, uint16_t, uint16_t, uint16_t); + uint8_t readWriteMultipleRegisters(uint16_t, uint16_t); + + private: + uint8_t _u8SerialPort; ///< serial port (0..3) initialized in constructor + uint8_t _u8MBSlave; ///< Modbus slave (1..255) initialized in constructor + uint16_t _u16BaudRate; ///< baud rate (300..115200) initialized in begin() + static const uint8_t ku8MaxBufferSize = 64; ///< size of response/transmit buffers + uint16_t _u16ReadAddress; ///< slave register from which to read + uint16_t _u16ReadQty; ///< quantity of words to read + uint16_t _u16ResponseBuffer[ku8MaxBufferSize]; ///< buffer to store Modbus slave response; read via GetResponseBuffer() + uint16_t _u16WriteAddress; ///< slave register to which to write + uint16_t _u16WriteQty; ///< quantity of words to write + uint16_t _u16TransmitBuffer[ku8MaxBufferSize]; ///< buffer containing data to transmit to Modbus slave; set via SetTransmitBuffer() + uint16_t* txBuffer; // from Wire.h -- need to clean this up Rx + uint8_t _u8TransmitBufferIndex; + uint16_t u16TransmitBufferLength; + uint16_t* rxBuffer; // from Wire.h -- need to clean this up Rx + uint8_t _u8ResponseBufferIndex; + uint8_t _u8ResponseBufferLength; + + // Modbus function codes for bit access + static const uint8_t ku8MBReadCoils = 0x01; ///< Modbus function 0x01 Read Coils + static const uint8_t ku8MBReadDiscreteInputs = 0x02; ///< Modbus function 0x02 Read Discrete Inputs + static const uint8_t ku8MBWriteSingleCoil = 0x05; ///< Modbus function 0x05 Write Single Coil + static const uint8_t ku8MBWriteMultipleCoils = 0x0F; ///< Modbus function 0x0F Write Multiple Coils + + // Modbus function codes for 16 bit access + static const uint8_t ku8MBReadHoldingRegisters = 0x03; ///< Modbus function 0x03 Read Holding Registers + static const uint8_t ku8MBReadInputRegisters = 0x04; ///< Modbus function 0x04 Read Input Registers + static const uint8_t ku8MBWriteSingleRegister = 0x06; ///< Modbus function 0x06 Write Single Register + static const uint8_t ku8MBWriteMultipleRegisters = 0x10; ///< Modbus function 0x10 Write Multiple Registers + static const uint8_t ku8MBMaskWriteRegister = 0x16; ///< Modbus function 0x16 Mask Write Register + static const uint8_t ku8MBReadWriteMultipleRegisters = 0x17; ///< Modbus function 0x17 Read Write Multiple Registers + + // Modbus timeout [milliseconds] + static const uint16_t ku16MBResponseTimeout = 2000; ///< Modbus timeout [milliseconds] + + // master function that conducts Modbus transactions + uint8_t ModbusMasterTransaction(uint8_t u8MBFunction); + + // idle callback function; gets called during idle time between TX and RX + void (*_idle)(); + SerialPort *MBSerial = NULL; // added by KRL +}; +#endif + +/** +@example examples/Basic/Basic.pde +@example examples/PhoenixContact_nanoLC/PhoenixContact_nanoLC.pde +*/ \ No newline at end of file diff --git a/Modbus/liblinks.xml b/Modbus/liblinks.xml new file mode 100644 index 0000000..1dc1803 --- /dev/null +++ b/Modbus/liblinks.xml @@ -0,0 +1,32 @@ + + + + + ${MacroStart}workspace_loc:/${ProjName}/inc${MacroEnd} + + + ${MacroStart}workspace_loc:/${ProjName}/inc${MacroEnd} + + + ${ProjName} + + + ${MacroStart}workspace_loc:/${ProjName}/Debug${MacroEnd} + + + ${MacroStart}workspace_loc:/${ProjName}/Release${MacroEnd} + + + ${ProjName} + + + diff --git a/Modbus/src/tester.cpp b/Modbus/src/tester.cpp new file mode 100644 index 0000000..a2935ea --- /dev/null +++ b/Modbus/src/tester.cpp @@ -0,0 +1,45 @@ +#if defined (__USE_LPCOPEN) +#if defined(NO_BOARD_LIB) +#include "chip.h" +#else +#include "board.h" +#endif +#endif + +#include +#include "ModbusMaster.h" +// TODO: insert other include files here + +// TODO: insert other definitions and declarations here + +int main(void) { + +#if defined (__USE_LPCOPEN) + // Read clock settings and update SystemCoreClock variable + SystemCoreClockUpdate(); +#if !defined(NO_BOARD_LIB) + // Set up and initialize all required blocks and + // functions related to the board hardware + Board_Init(); + // Set the LED to the state of "On" + Board_LED_Set(0, true); +#endif +#endif + + // TODO: insert code here + ModbusMaster MIO_12V(1); + ModbusMaster CO2_sensor(240); + ModbusMaster RH_sensor(241); + + + // Force the counter to be placed into memory + volatile static int i = 0 ; + // Enter an infinite loop, just incrementing a counter + while(1) { + i++ ; + // "Dummy" NOP to allow source level single + // stepping of tight while() loop + __asm volatile ("nop"); + } + return 0 ; +} From 43748c19ccd37856e902932637b2eca332e65691 Mon Sep 17 00:00:00 2001 From: jaakkoiot Date: Sat, 15 Oct 2022 15:15:12 +0300 Subject: [PATCH 09/17] Ass register class for modbus comms, without hw addresses yet --- Modbus/inc/ModbusRegister.h | 19 +++++++++++++++++++ Modbus/src/ModbusRegister.cpp | 27 +++++++++++++++++++++++++++ 2 files changed, 46 insertions(+) create mode 100644 Modbus/inc/ModbusRegister.h create mode 100644 Modbus/src/ModbusRegister.cpp diff --git a/Modbus/inc/ModbusRegister.h b/Modbus/inc/ModbusRegister.h new file mode 100644 index 0000000..af20be9 --- /dev/null +++ b/Modbus/inc/ModbusRegister.h @@ -0,0 +1,19 @@ +#ifndef MODBUSREGISTER_H_ +#define MODBUSREGISTER_H_ + +#include "ModbusLeader.h" + +class ModbusRegister { +public: + ModbusRegister(ModbusMaster *master, int address, bool holdingRegister = true); + ModbusRegister(const ModbusRegister &) = delete; + virtual ~ModbusRegister(); + int read(); + void write(int value); +private: + ModbusMaster *m; + int addr; + bool hr; +}; + +#endif /* MODBUSREGISTER_H_ */ diff --git a/Modbus/src/ModbusRegister.cpp b/Modbus/src/ModbusRegister.cpp new file mode 100644 index 0000000..a642bbc --- /dev/null +++ b/Modbus/src/ModbusRegister.cpp @@ -0,0 +1,27 @@ +#include "ModbusRegister.h" + +ModbusRegister::ModbusRegister(ModbusMaster *master, int address, bool holdingRegister) + :m(master), addr(address), hr(holdingRegister) { + // TODO Auto-generated constructor stub + +} + +ModbusRegister::~ModbusRegister() { + // TODO Auto-generated destructor stub +} + +int ModbusRegister::read() { + uint8_t result = hr ? m->readHoldingRegisters(addr, 1) : m->readInputRegisters(addr, 1) ; + // check if we were able to read + if (result == m->ku8MBSuccess) { + return m->getResponseBuffer(0); + } + return -1; +} + +void ModbusRegister::write(int value) +{ + // write only if not + if(hr) m->writeSingleRegister(addr, value); // not checking if write succeeds + +} From 8aebe25c07185d51258dd306aee1123de4656baa Mon Sep 17 00:00:00 2001 From: jaakkoiot Date: Sat, 15 Oct 2022 15:17:53 +0300 Subject: [PATCH 10/17] Add serial & UART classes for Modbus use --- Modbus/inc/SerialPort.h | 20 ++++ Modbus/inc/Uart.h | 62 +++++++++++ Modbus/src/SerialPort.cpp | 50 +++++++++ Modbus/src/Uart.cpp | 227 ++++++++++++++++++++++++++++++++++++++ 4 files changed, 359 insertions(+) create mode 100644 Modbus/inc/SerialPort.h create mode 100644 Modbus/inc/Uart.h create mode 100644 Modbus/src/SerialPort.cpp create mode 100644 Modbus/src/Uart.cpp diff --git a/Modbus/inc/SerialPort.h b/Modbus/inc/SerialPort.h new file mode 100644 index 0000000..6e89b6e --- /dev/null +++ b/Modbus/inc/SerialPort.h @@ -0,0 +1,20 @@ +#ifndef SERIALPORT_H_ +#define SERIALPORT_H_ + +#include "Uart.h" + +class SerialPort { +public: + SerialPort(); + virtual ~SerialPort(); + int available(); + void begin(int speed = 9600); + int read(); + int write(const char* buf, int len); + int print(int val, int format); + void flush(); +private: + static LpcUart *u; +}; + +#endif /* SERIALPORT_H_ */ diff --git a/Modbus/inc/Uart.h b/Modbus/inc/Uart.h new file mode 100644 index 0000000..ce3c007 --- /dev/null +++ b/Modbus/inc/Uart.h @@ -0,0 +1,62 @@ +/* + * LpcUart.h + * + * Created on: 4.2.2019 + * Author: keijo + */ + +#ifndef LPCUART_H_ +#define LPCUART_H_ + +#include +#include "chip.h" + +struct LpcPinMap { + int port; /* set to -1 to indicate unused pin */ + int pin; /* set to -1 to indicate unused pin */ +}; + +struct LpcUartConfig { + LPC_USART_T *pUART; + uint32_t speed; + uint32_t data; + bool rs485; + LpcPinMap tx; + LpcPinMap rx; + LpcPinMap rts; /* used as output enable if RS-485 mode is enabled */ + LpcPinMap cts; +}; + + +class LpcUart { +public: + LpcUart(const LpcUartConfig &cfg); + LpcUart(const LpcUart &) = delete; + virtual ~LpcUart(); + int free(); /* get amount of free space in transmit buffer */ + int peek(); /* get number of received characters in receive buffer */ + int write(char c); + int write(const char *s); + int write(const char *buffer, int len); + int read(char &c); /* get a single character. Returns number of characters read --> returns 0 if no character is available */ + int read(char *buffer, int len); + void txbreak(bool brk); /* set break signal on */ + bool rxbreak(); /* check if break is received */ + void speed(int bps); /* change transmission speed */ + bool txempty(); + + void isr(); /* ISR handler. This will be called by the HW ISR handler. Do not call from application */ +private: + LPC_USART_T *uart; + IRQn_Type irqn; + /* currently we support only fixed size ring buffers */ + static const int UART_RB_SIZE = 256; + /* Transmit and receive ring buffers */ + RINGBUFF_T txring; + RINGBUFF_T rxring; + uint8_t rxbuff[UART_RB_SIZE]; + uint8_t txbuff[UART_RB_SIZE]; + static bool init; /* set when first UART is initialized. We have a global clock setting for all UARTSs */ +}; + +#endif /* LPCUART_H_ */ diff --git a/Modbus/src/SerialPort.cpp b/Modbus/src/SerialPort.cpp new file mode 100644 index 0000000..953d99a --- /dev/null +++ b/Modbus/src/SerialPort.cpp @@ -0,0 +1,50 @@ +#include "SerialPort.h" + + + +SerialPort::SerialPort() { + if(!u) { + LpcPinMap none = {-1, -1}; // unused pin has negative values in it + LpcPinMap txpin = { 0, 28 }; // transmit pin that goes to rs485 driver chip + LpcPinMap rxpin = { 0, 24 }; // receive pin that goes to rs485 driver chip + LpcPinMap rtspin = { 1, 0 }; // handshake pin that is used to set tranmitter direction + LpcUartConfig cfg = { LPC_USART1, 9600, UART_CFG_DATALEN_8 | UART_CFG_PARITY_NONE | UART_CFG_STOPLEN_2, true, txpin, rxpin, rtspin, none }; + u = new LpcUart(cfg); + } +} + +LpcUart *SerialPort::u = nullptr; + +SerialPort::~SerialPort() { + /* DeInitialize UART peripheral */ + delete u; +} + +int SerialPort::available() { + return u->peek(); +} + +void SerialPort::begin(int speed) { + u->speed(speed); + +} + +int SerialPort::read() { + char byte; + if(u->read(byte)> 0) return (byte); + return -1; +} +int SerialPort::write(const char* buf, int len) { + return u->write(buf, len); +} + +int SerialPort::print(int val, int format) { + // here only to maintain compatibility with Arduino interface + (void) val; + (void) format; + return (0); +} + +void SerialPort::flush() { + while(!u->txempty()) __WFI(); +} diff --git a/Modbus/src/Uart.cpp b/Modbus/src/Uart.cpp new file mode 100644 index 0000000..6a45cae --- /dev/null +++ b/Modbus/src/Uart.cpp @@ -0,0 +1,227 @@ +#include +#include "Uart.h" + + +static LpcUart *u0; +static LpcUart *u1; +static LpcUart *u2; + +extern "C" { +/** + * @brief UART interrupt handler using ring buffers + * @return Nothing + */ +void UART0_IRQHandler(void) +{ + /* Want to handle any errors? Do it here. */ + + /* Use default ring buffer handler. Override this with your own + code if you need more capability. */ + if(u0) u0->isr(); +} + +void UART1_IRQHandler(void) +{ + /* Want to handle any errors? Do it here. */ + + /* Use default ring buffer handler. Override this with your own + code if you need more capability. */ + if(u1) u1->isr(); +} + +void UART2_IRQHandler(void) +{ + /* Want to handle any errors? Do it here. */ + + /* Use default ring buffer handler. Override this with your own + code if you need more capability. */ + if(u2) u2->isr(); +} + +} + + +void LpcUart::isr() { + Chip_UART_IRQRBHandler(uart, &rxring, &txring); +} + +bool LpcUart::init = false; + +LpcUart::LpcUart(const LpcUartConfig &cfg) { + CHIP_SWM_PIN_MOVABLE_T tx; + CHIP_SWM_PIN_MOVABLE_T rx; + CHIP_SWM_PIN_MOVABLE_T cts; + CHIP_SWM_PIN_MOVABLE_T rts; + bool use_rts = (cfg.rts.port >= 0); + bool use_cts = (cfg.cts.port >= 0); + + if(!init) { + init = true; + /* Before setting up the UART, the global UART clock for USARTS 1-4 + * must first be setup. This requires setting the UART divider and + * the UART base clock rate to 16x the maximum UART rate for all + * UARTs. + * */ + /* Use main clock rate as base for UART baud rate divider */ + Chip_Clock_SetUARTBaseClockRate(Chip_Clock_GetMainClockRate(), false); + } + + uart = nullptr; // set default value before checking which UART to configure + + if(cfg.pUART == LPC_USART0) { + if(u0) return; // already exists + else u0 = this; + tx = SWM_UART0_TXD_O; + rx = SWM_UART0_RXD_I; + rts = SWM_UART0_RTS_O; + cts = SWM_UART0_CTS_I; + irqn = UART0_IRQn; + } + else if(cfg.pUART == LPC_USART1) { + if(u1) return; // already exists + else u1 = this; + tx = SWM_UART1_TXD_O; + rx = SWM_UART1_RXD_I; + rts = SWM_UART1_RTS_O; + cts = SWM_UART1_CTS_I; + irqn = UART1_IRQn; + } + else if(cfg.pUART == LPC_USART2) { + if(u2) return; // already exists + else u2 = this; + tx = SWM_UART2_TXD_O; + rx = SWM_UART2_RXD_I; + use_rts = false; // UART2 does not support handshakes + use_cts = false; + irqn = UART2_IRQn; + } + else { + return; + } + + uart = cfg.pUART; // set the actual value after validity checking + + + if(cfg.tx.port >= 0) { + Chip_IOCON_PinMuxSet(LPC_IOCON, cfg.tx.port, cfg.tx.pin, (IOCON_MODE_INACT | IOCON_DIGMODE_EN)); + Chip_SWM_MovablePortPinAssign(tx, cfg.tx.port, cfg.tx.pin); + } + + if(cfg.rx.port >= 0) { + Chip_IOCON_PinMuxSet(LPC_IOCON, cfg.rx.port, cfg.rx.pin, (IOCON_MODE_INACT | IOCON_DIGMODE_EN)); + Chip_SWM_MovablePortPinAssign(rx, cfg.rx.port, cfg.rx.pin); + } + + if(use_cts) { + Chip_IOCON_PinMuxSet(LPC_IOCON, cfg.cts.port, cfg.cts.pin, (IOCON_MODE_INACT | IOCON_DIGMODE_EN)); + Chip_SWM_MovablePortPinAssign(cts, cfg.cts.port, cfg.cts.pin); + } + + if(use_rts) { + Chip_IOCON_PinMuxSet(LPC_IOCON, cfg.rts.port, cfg.rts.pin, (IOCON_MODE_INACT | IOCON_DIGMODE_EN)); + Chip_SWM_MovablePortPinAssign(rts, cfg.rts.port, cfg.rts.pin); + } + + + /* Setup UART */ + Chip_UART_Init(uart); + Chip_UART_ConfigData(uart, cfg.data); + Chip_UART_SetBaud(uart, cfg.speed); + + if(use_rts && cfg.rs485) { + uart->CFG |= (1 << 20); // enable rs485 mode + //uart->CFG |= (1 << 18); // OE turnaraound time + uart->CFG |= (1 << 21);// driver enable polarity (active high) + } + + Chip_UART_Enable(uart); + Chip_UART_TXEnable(uart); + + /* Before using the ring buffers, initialize them using the ring + buffer init function */ + RingBuffer_Init(&rxring, rxbuff, 1, UART_RB_SIZE); + RingBuffer_Init(&txring, txbuff, 1, UART_RB_SIZE); + + + /* Enable receive data and line status interrupt */ + Chip_UART_IntEnable(uart, UART_INTEN_RXRDY); + Chip_UART_IntDisable(uart, UART_INTEN_TXRDY); /* May not be needed */ + + /* Enable UART interrupt */ + NVIC_EnableIRQ(irqn); +} + +LpcUart::~LpcUart() { + if(uart != nullptr) { + NVIC_DisableIRQ(irqn); + Chip_UART_IntDisable(uart, UART_INTEN_RXRDY); + Chip_UART_IntDisable(uart, UART_INTEN_TXRDY); + + if(uart == LPC_USART0) { + u0 = nullptr; + } + else if(uart == LPC_USART1) { + u1 = nullptr; + } + else if(uart == LPC_USART2) { + u2 = nullptr; + } + } +} + + +int LpcUart::free() +{ + return RingBuffer_GetCount(&txring);; +} + +int LpcUart::peek() +{ + return RingBuffer_GetCount(&rxring); +} + +int LpcUart::read(char &c) +{ + return Chip_UART_ReadRB(uart, &rxring, &c, 1); +} + +int LpcUart::read(char *buffer, int len) +{ + return Chip_UART_ReadRB(uart, &rxring, buffer, len); +} + +int LpcUart::write(char c) +{ + return Chip_UART_SendRB(uart, &txring, &c, 1); +} + +int LpcUart::write(const char *s) +{ + return Chip_UART_SendRB(uart, &txring, s, strlen(s)); +} + +int LpcUart::write(const char *buffer, int len) +{ + return Chip_UART_SendRB(uart, &txring, buffer, len); +} + +void LpcUart::txbreak(bool brk) +{ + // break handling not implemented yeet +} + +bool LpcUart::rxbreak() +{ + // break handling not implemented yeet + return false; +} + +void LpcUart::speed(int bps) +{ + Chip_UART_SetBaud(uart, bps); +} + +bool LpcUart::txempty() +{ + return (RingBuffer_GetCount(&txring) == 0); +} From 72058434e363224c5938d40ae8016de29d1f7fde Mon Sep 17 00:00:00 2001 From: jaakkoiot Date: Sat, 15 Oct 2022 15:19:12 +0300 Subject: [PATCH 11/17] Populate source code for ModbusLeader class --- Modbus/src/ModbusLeader.cpp | 937 ++++++++++++++++++++++++++++++++++++ 1 file changed, 937 insertions(+) create mode 100644 Modbus/src/ModbusLeader.cpp diff --git a/Modbus/src/ModbusLeader.cpp b/Modbus/src/ModbusLeader.cpp new file mode 100644 index 0000000..92a8958 --- /dev/null +++ b/Modbus/src/ModbusLeader.cpp @@ -0,0 +1,937 @@ +/** +@file +Arduino library for communicating with Modbus slaves over RS232/485 (via RTU protocol). +*/ +/* + + ModbusMaster.cpp - Arduino library for communicating with Modbus slaves + over RS232/485 (via RTU protocol). + + This file is part of ModbusMaster. + + ModbusMaster is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + ModbusMaster is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with ModbusMaster. If not, see . + + Written by Doc Walker (Rx) + Copyright © 2009-2013 Doc Walker <4-20ma at wvfans dot net> + +*/ + + +/* _____PROJECT INCLUDES_____________________________________________________ */ +#include "ModbusLeader.h" +#include "crc16.h" + + +/* _____GLOBAL VARIABLES_____________________________________________________ */ +#if defined(ARDUINO_ARCH_AVR) + HardwareSerial* MBSerial = &Serial; ///< Pointer to Serial class object +#elif defined(ARDUINO_ARCH_SAM) + UARTClass* MBSerial = &Serial; ///< Pointer to Serial class object +#else +// #error "This library only supports boards with an AVR or SAM processor. Please open an issue at https://github.com/4-20ma/ModbusMaster/issues and indicate which processor/platform you're using." +#endif + + +/* _____PUBLIC FUNCTIONS_____________________________________________________ */ +/** +Constructor. + +Creates class object using default serial port 0, Modbus slave ID 1. + +@ingroup setup +*/ +ModbusMaster::ModbusMaster(void) +{ + _u8SerialPort = 0; + _u8MBSlave = 1; + _u16BaudRate = 0; +} + + +/** +Constructor. + +Creates class object using default serial port 0, specified Modbus slave ID. + +@overload void ModbusMaster::ModbusMaster(uint8_t u8MBSlave) +@param u8MBSlave Modbus slave ID (1..255) +@ingroup setup +*/ +ModbusMaster::ModbusMaster(uint8_t u8MBSlave) +{ + _u8SerialPort = 0; + _u8MBSlave = u8MBSlave; + _u16BaudRate = 0; +} + + +/** +Constructor. + +Creates class object using specified serial port, Modbus slave ID. + +@overload void ModbusMaster::ModbusMaster(uint8_t u8SerialPort, uint8_t u8MBSlave) +@param u8SerialPort serial port (Serial, Serial1..Serial3) +@param u8MBSlave Modbus slave ID (1..255) +@ingroup setup +*/ +ModbusMaster::ModbusMaster(uint8_t u8SerialPort, uint8_t u8MBSlave) +{ + _u8SerialPort = (u8SerialPort > 3) ? 0 : u8SerialPort; + _u8MBSlave = u8MBSlave; + _u16BaudRate = 0; +} + + +/** +Initialize class object. + +Sets up the serial port using default 19200 baud rate. +Call once class has been instantiated, typically within setup(). + +@ingroup setup +*/ +void ModbusMaster::begin(void) +{ + begin(19200); +} + + +/** +Initialize class object. + +Sets up the serial port using specified baud rate. +Call once class has been instantiated, typically within setup(). + +@overload ModbusMaster::begin(uint16_t u16BaudRate) +@param u16BaudRate baud rate, in standard increments (300..115200) +@ingroup setup +*/ +void ModbusMaster::begin(uint16_t u16BaudRate) +{ +// txBuffer = (uint16_t*) calloc(ku8MaxBufferSize, sizeof(uint16_t)); + _u8TransmitBufferIndex = 0; + u16TransmitBufferLength = 0; + +#if 0 + switch(_u8SerialPort) + { +#if defined(UBRR1H) + case 1: + MBSerial = &Serial1; + break; +#endif + +#if defined(UBRR2H) + case 2: + MBSerial = &Serial2; + break; +#endif + +#if defined(UBRR3H) + case 3: + MBSerial = &Serial3; + break; +#endif + + case 0: + default: + MBSerial = &Serial; + break; + } +#endif + + if(MBSerial == NULL) MBSerial = new SerialPort; + if(u16BaudRate != _u16BaudRate) { + _u16BaudRate = u16BaudRate; + MBSerial->begin(u16BaudRate); + } + _idle = NULL; + +#if __MODBUSMASTER_DEBUG__ +// pinMode(4, OUTPUT); +// pinMode(5, OUTPUT); +#endif +} + + +void ModbusMaster::beginTransmission(uint16_t u16Address) +{ + _u16WriteAddress = u16Address; + _u8TransmitBufferIndex = 0; + u16TransmitBufferLength = 0; +} + +// eliminate this function in favor of using existing MB request functions +uint8_t ModbusMaster::requestFrom(uint16_t address, uint16_t quantity) +{ + uint8_t read; + read = 1; // krl: added this to prevent warning. This method is not called anywhere... + // clamp to buffer length + if (quantity > ku8MaxBufferSize) + { + quantity = ku8MaxBufferSize; + } + // set rx buffer iterator vars + _u8ResponseBufferIndex = 0; + _u8ResponseBufferLength = read; + + return read; +} + + +void ModbusMaster::sendBit(bool data) +{ + uint8_t txBitIndex = u16TransmitBufferLength % 16; + if ((u16TransmitBufferLength >> 4) < ku8MaxBufferSize) + { + if (0 == txBitIndex) + { + _u16TransmitBuffer[_u8TransmitBufferIndex] = 0; + } + bitWrite(_u16TransmitBuffer[_u8TransmitBufferIndex], txBitIndex, data); + u16TransmitBufferLength++; + _u8TransmitBufferIndex = u16TransmitBufferLength >> 4; + } +} + + +void ModbusMaster::send(uint16_t data) +{ + if (_u8TransmitBufferIndex < ku8MaxBufferSize) + { + _u16TransmitBuffer[_u8TransmitBufferIndex++] = data; + u16TransmitBufferLength = _u8TransmitBufferIndex << 4; + } +} + + +void ModbusMaster::send(uint32_t data) +{ + send(lowWord(data)); + send(highWord(data)); +} + + +void ModbusMaster::send(uint8_t data) +{ + send(word(data)); +} + + + + + + + + + +uint8_t ModbusMaster::available(void) +{ + return _u8ResponseBufferLength - _u8ResponseBufferIndex; +} + + +uint16_t ModbusMaster::receive(void) +{ + if (_u8ResponseBufferIndex < _u8ResponseBufferLength) + { + return _u16ResponseBuffer[_u8ResponseBufferIndex++]; + } + else + { + return 0xFFFF; + } +} + + + + + + + + +/** +Set idle time callback function (cooperative multitasking). + +This function gets called in the idle time between transmission of data +and response from slave. Do not call functions that read from the serial +buffer that is used by ModbusMaster. Use of i2c/TWI, 1-Wire, other +serial ports, etc. is permitted within callback function. + +@see ModbusMaster::ModbusMasterTransaction() +*/ +void ModbusMaster::idle(void (*idle)()) +{ + _idle = idle; +} + + +/** +Retrieve data from response buffer. + +@see ModbusMaster::clearResponseBuffer() +@param u8Index index of response buffer array (0x00..0x3F) +@return value in position u8Index of response buffer (0x0000..0xFFFF) +@ingroup buffer +*/ +uint16_t ModbusMaster::getResponseBuffer(uint8_t u8Index) +{ + if (u8Index < ku8MaxBufferSize) + { + return _u16ResponseBuffer[u8Index]; + } + else + { + return 0xFFFF; + } +} + + +/** +Clear Modbus response buffer. + +@see ModbusMaster::getResponseBuffer(uint8_t u8Index) +@ingroup buffer +*/ +void ModbusMaster::clearResponseBuffer() +{ + uint8_t i; + + for (i = 0; i < ku8MaxBufferSize; i++) + { + _u16ResponseBuffer[i] = 0; + } +} + + +/** +Place data in transmit buffer. + +@see ModbusMaster::clearTransmitBuffer() +@param u8Index index of transmit buffer array (0x00..0x3F) +@param u16Value value to place in position u8Index of transmit buffer (0x0000..0xFFFF) +@return 0 on success; exception number on failure +@ingroup buffer +*/ +uint8_t ModbusMaster::setTransmitBuffer(uint8_t u8Index, uint16_t u16Value) +{ + if (u8Index < ku8MaxBufferSize) + { + _u16TransmitBuffer[u8Index] = u16Value; + return ku8MBSuccess; + } + else + { + return ku8MBIllegalDataAddress; + } +} + + +/** +Clear Modbus transmit buffer. + +@see ModbusMaster::setTransmitBuffer(uint8_t u8Index, uint16_t u16Value) +@ingroup buffer +*/ +void ModbusMaster::clearTransmitBuffer() +{ + uint8_t i; + + for (i = 0; i < ku8MaxBufferSize; i++) + { + _u16TransmitBuffer[i] = 0; + } +} + + +/** +Modbus function 0x01 Read Coils. + +This function code is used to read from 1 to 2000 contiguous status of +coils in a remote device. The request specifies the starting address, +i.e. the address of the first coil specified, and the number of coils. +Coils are addressed starting at zero. + +The coils in the response buffer are packed as one coil per bit of the +data field. Status is indicated as 1=ON and 0=OFF. The LSB of the first +data word contains the output addressed in the query. The other coils +follow toward the high order end of this word and from low order to high +order in subsequent words. + +If the returned quantity is not a multiple of sixteen, the remaining +bits in the final data word will be padded with zeros (toward the high +order end of the word). + +@param u16ReadAddress address of first coil (0x0000..0xFFFF) +@param u16BitQty quantity of coils to read (1..2000, enforced by remote device) +@return 0 on success; exception number on failure +@ingroup discrete +*/ +uint8_t ModbusMaster::readCoils(uint16_t u16ReadAddress, uint16_t u16BitQty) +{ + _u16ReadAddress = u16ReadAddress; + _u16ReadQty = u16BitQty; + return ModbusMasterTransaction(ku8MBReadCoils); +} + + +/** +Modbus function 0x02 Read Discrete Inputs. + +This function code is used to read from 1 to 2000 contiguous status of +discrete inputs in a remote device. The request specifies the starting +address, i.e. the address of the first input specified, and the number +of inputs. Discrete inputs are addressed starting at zero. + +The discrete inputs in the response buffer are packed as one input per +bit of the data field. Status is indicated as 1=ON; 0=OFF. The LSB of +the first data word contains the input addressed in the query. The other +inputs follow toward the high order end of this word, and from low order +to high order in subsequent words. + +If the returned quantity is not a multiple of sixteen, the remaining +bits in the final data word will be padded with zeros (toward the high +order end of the word). + +@param u16ReadAddress address of first discrete input (0x0000..0xFFFF) +@param u16BitQty quantity of discrete inputs to read (1..2000, enforced by remote device) +@return 0 on success; exception number on failure +@ingroup discrete +*/ +uint8_t ModbusMaster::readDiscreteInputs(uint16_t u16ReadAddress, + uint16_t u16BitQty) +{ + _u16ReadAddress = u16ReadAddress; + _u16ReadQty = u16BitQty; + return ModbusMasterTransaction(ku8MBReadDiscreteInputs); +} + + +/** +Modbus function 0x03 Read Holding Registers. + +This function code is used to read the contents of a contiguous block of +holding registers in a remote device. The request specifies the starting +register address and the number of registers. Registers are addressed +starting at zero. + +The register data in the response buffer is packed as one word per +register. + +@param u16ReadAddress address of the first holding register (0x0000..0xFFFF) +@param u16ReadQty quantity of holding registers to read (1..125, enforced by remote device) +@return 0 on success; exception number on failure +@ingroup register +*/ +uint8_t ModbusMaster::readHoldingRegisters(uint16_t u16ReadAddress, + uint16_t u16ReadQty) +{ + _u16ReadAddress = u16ReadAddress; + _u16ReadQty = u16ReadQty; + return ModbusMasterTransaction(ku8MBReadHoldingRegisters); +} + + +/** +Modbus function 0x04 Read Input Registers. + +This function code is used to read from 1 to 125 contiguous input +registers in a remote device. The request specifies the starting +register address and the number of registers. Registers are addressed +starting at zero. + +The register data in the response buffer is packed as one word per +register. + +@param u16ReadAddress address of the first input register (0x0000..0xFFFF) +@param u16ReadQty quantity of input registers to read (1..125, enforced by remote device) +@return 0 on success; exception number on failure +@ingroup register +*/ +uint8_t ModbusMaster::readInputRegisters(uint16_t u16ReadAddress, + uint8_t u16ReadQty) +{ + _u16ReadAddress = u16ReadAddress; + _u16ReadQty = u16ReadQty; + return ModbusMasterTransaction(ku8MBReadInputRegisters); +} + + +/** +Modbus function 0x05 Write Single Coil. + +This function code is used to write a single output to either ON or OFF +in a remote device. The requested ON/OFF state is specified by a +constant in the state field. A non-zero value requests the output to be +ON and a value of 0 requests it to be OFF. The request specifies the +address of the coil to be forced. Coils are addressed starting at zero. + +@param u16WriteAddress address of the coil (0x0000..0xFFFF) +@param u8State 0=OFF, non-zero=ON (0x00..0xFF) +@return 0 on success; exception number on failure +@ingroup discrete +*/ +uint8_t ModbusMaster::writeSingleCoil(uint16_t u16WriteAddress, uint8_t u8State) +{ + _u16WriteAddress = u16WriteAddress; + _u16WriteQty = (u8State ? 0xFF00 : 0x0000); + return ModbusMasterTransaction(ku8MBWriteSingleCoil); +} + + +/** +Modbus function 0x06 Write Single Register. + +This function code is used to write a single holding register in a +remote device. The request specifies the address of the register to be +written. Registers are addressed starting at zero. + +@param u16WriteAddress address of the holding register (0x0000..0xFFFF) +@param u16WriteValue value to be written to holding register (0x0000..0xFFFF) +@return 0 on success; exception number on failure +@ingroup register +*/ +uint8_t ModbusMaster::writeSingleRegister(uint16_t u16WriteAddress, + uint16_t u16WriteValue) +{ + _u16WriteAddress = u16WriteAddress; + _u16WriteQty = 0; + _u16TransmitBuffer[0] = u16WriteValue; + return ModbusMasterTransaction(ku8MBWriteSingleRegister); +} + + +/** +Modbus function 0x0F Write Multiple Coils. + +This function code is used to force each coil in a sequence of coils to +either ON or OFF in a remote device. The request specifies the coil +references to be forced. Coils are addressed starting at zero. + +The requested ON/OFF states are specified by contents of the transmit +buffer. A logical '1' in a bit position of the buffer requests the +corresponding output to be ON. A logical '0' requests it to be OFF. + +@param u16WriteAddress address of the first coil (0x0000..0xFFFF) +@param u16BitQty quantity of coils to write (1..2000, enforced by remote device) +@return 0 on success; exception number on failure +@ingroup discrete +*/ +uint8_t ModbusMaster::writeMultipleCoils(uint16_t u16WriteAddress, + uint16_t u16BitQty) +{ + _u16WriteAddress = u16WriteAddress; + _u16WriteQty = u16BitQty; + return ModbusMasterTransaction(ku8MBWriteMultipleCoils); +} +uint8_t ModbusMaster::writeMultipleCoils() +{ + _u16WriteQty = u16TransmitBufferLength; + return ModbusMasterTransaction(ku8MBWriteMultipleCoils); +} + + +/** +Modbus function 0x10 Write Multiple Registers. + +This function code is used to write a block of contiguous registers (1 +to 123 registers) in a remote device. + +The requested written values are specified in the transmit buffer. Data +is packed as one word per register. + +@param u16WriteAddress address of the holding register (0x0000..0xFFFF) +@param u16WriteQty quantity of holding registers to write (1..123, enforced by remote device) +@return 0 on success; exception number on failure +@ingroup register +*/ +uint8_t ModbusMaster::writeMultipleRegisters(uint16_t u16WriteAddress, + uint16_t u16WriteQty) +{ + _u16WriteAddress = u16WriteAddress; + _u16WriteQty = u16WriteQty; + return ModbusMasterTransaction(ku8MBWriteMultipleRegisters); +} + +// new version based on Wire.h +uint8_t ModbusMaster::writeMultipleRegisters() +{ + _u16WriteQty = _u8TransmitBufferIndex; + return ModbusMasterTransaction(ku8MBWriteMultipleRegisters); +} + + +/** +Modbus function 0x16 Mask Write Register. + +This function code is used to modify the contents of a specified holding +register using a combination of an AND mask, an OR mask, and the +register's current contents. The function can be used to set or clear +individual bits in the register. + +The request specifies the holding register to be written, the data to be +used as the AND mask, and the data to be used as the OR mask. Registers +are addressed starting at zero. + +The function's algorithm is: + +Result = (Current Contents && And_Mask) || (Or_Mask && (~And_Mask)) + +@param u16WriteAddress address of the holding register (0x0000..0xFFFF) +@param u16AndMask AND mask (0x0000..0xFFFF) +@param u16OrMask OR mask (0x0000..0xFFFF) +@return 0 on success; exception number on failure +@ingroup register +*/ +uint8_t ModbusMaster::maskWriteRegister(uint16_t u16WriteAddress, + uint16_t u16AndMask, uint16_t u16OrMask) +{ + _u16WriteAddress = u16WriteAddress; + _u16TransmitBuffer[0] = u16AndMask; + _u16TransmitBuffer[1] = u16OrMask; + return ModbusMasterTransaction(ku8MBMaskWriteRegister); +} + + +/** +Modbus function 0x17 Read Write Multiple Registers. + +This function code performs a combination of one read operation and one +write operation in a single MODBUS transaction. The write operation is +performed before the read. Holding registers are addressed starting at +zero. + +The request specifies the starting address and number of holding +registers to be read as well as the starting address, and the number of +holding registers. The data to be written is specified in the transmit +buffer. + +@param u16ReadAddress address of the first holding register (0x0000..0xFFFF) +@param u16ReadQty quantity of holding registers to read (1..125, enforced by remote device) +@param u16WriteAddress address of the first holding register (0x0000..0xFFFF) +@param u16WriteQty quantity of holding registers to write (1..121, enforced by remote device) +@return 0 on success; exception number on failure +@ingroup register +*/ +uint8_t ModbusMaster::readWriteMultipleRegisters(uint16_t u16ReadAddress, + uint16_t u16ReadQty, uint16_t u16WriteAddress, uint16_t u16WriteQty) +{ + _u16ReadAddress = u16ReadAddress; + _u16ReadQty = u16ReadQty; + _u16WriteAddress = u16WriteAddress; + _u16WriteQty = u16WriteQty; + return ModbusMasterTransaction(ku8MBReadWriteMultipleRegisters); +} +uint8_t ModbusMaster::readWriteMultipleRegisters(uint16_t u16ReadAddress, + uint16_t u16ReadQty) +{ + _u16ReadAddress = u16ReadAddress; + _u16ReadQty = u16ReadQty; + _u16WriteQty = _u8TransmitBufferIndex; + return ModbusMasterTransaction(ku8MBReadWriteMultipleRegisters); +} + + +/* _____PRIVATE FUNCTIONS____________________________________________________ */ +/** +Modbus transaction engine. +Sequence: + - assemble Modbus Request Application Data Unit (ADU), + based on particular function called + - transmit request over selected serial port + - wait for/retrieve response + - evaluate/disassemble response + - return status (success/exception) + +@param u8MBFunction Modbus function (0x01..0xFF) +@return 0 on success; exception number on failure +*/ +uint8_t ModbusMaster::ModbusMasterTransaction(uint8_t u8MBFunction) +{ + uint8_t u8ModbusADU[256]; + uint8_t u8ModbusADUSize = 0; + uint8_t i, u8Qty; + uint16_t u16CRC; + uint32_t u32StartTime; + uint8_t u8BytesLeft = 8; + uint8_t u8MBStatus = ku8MBSuccess; + + // assemble Modbus Request Application Data Unit + u8ModbusADU[u8ModbusADUSize++] = _u8MBSlave; + u8ModbusADU[u8ModbusADUSize++] = u8MBFunction; + + switch(u8MBFunction) + { + case ku8MBReadCoils: + case ku8MBReadDiscreteInputs: + case ku8MBReadInputRegisters: + case ku8MBReadHoldingRegisters: + case ku8MBReadWriteMultipleRegisters: + u8ModbusADU[u8ModbusADUSize++] = highByte(_u16ReadAddress); + u8ModbusADU[u8ModbusADUSize++] = lowByte(_u16ReadAddress); + u8ModbusADU[u8ModbusADUSize++] = highByte(_u16ReadQty); + u8ModbusADU[u8ModbusADUSize++] = lowByte(_u16ReadQty); + break; + } + + switch(u8MBFunction) + { + case ku8MBWriteSingleCoil: + case ku8MBMaskWriteRegister: + case ku8MBWriteMultipleCoils: + case ku8MBWriteSingleRegister: + case ku8MBWriteMultipleRegisters: + case ku8MBReadWriteMultipleRegisters: + u8ModbusADU[u8ModbusADUSize++] = highByte(_u16WriteAddress); + u8ModbusADU[u8ModbusADUSize++] = lowByte(_u16WriteAddress); + break; + } + + switch(u8MBFunction) + { + case ku8MBWriteSingleCoil: + u8ModbusADU[u8ModbusADUSize++] = highByte(_u16WriteQty); + u8ModbusADU[u8ModbusADUSize++] = lowByte(_u16WriteQty); + break; + + case ku8MBWriteSingleRegister: + u8ModbusADU[u8ModbusADUSize++] = highByte(_u16TransmitBuffer[0]); + u8ModbusADU[u8ModbusADUSize++] = lowByte(_u16TransmitBuffer[0]); + break; + + case ku8MBWriteMultipleCoils: + u8ModbusADU[u8ModbusADUSize++] = highByte(_u16WriteQty); + u8ModbusADU[u8ModbusADUSize++] = lowByte(_u16WriteQty); + u8Qty = (_u16WriteQty % 8) ? ((_u16WriteQty >> 3) + 1) : (_u16WriteQty >> 3); + u8ModbusADU[u8ModbusADUSize++] = u8Qty; + for (i = 0; i < u8Qty; i++) + { + switch(i % 2) + { + case 0: // i is even + u8ModbusADU[u8ModbusADUSize++] = lowByte(_u16TransmitBuffer[i >> 1]); + break; + + case 1: // i is odd + u8ModbusADU[u8ModbusADUSize++] = highByte(_u16TransmitBuffer[i >> 1]); + break; + } + } + break; + + case ku8MBWriteMultipleRegisters: + case ku8MBReadWriteMultipleRegisters: + u8ModbusADU[u8ModbusADUSize++] = highByte(_u16WriteQty); + u8ModbusADU[u8ModbusADUSize++] = lowByte(_u16WriteQty); + u8ModbusADU[u8ModbusADUSize++] = lowByte(_u16WriteQty << 1); + + for (i = 0; i < lowByte(_u16WriteQty); i++) + { + u8ModbusADU[u8ModbusADUSize++] = highByte(_u16TransmitBuffer[i]); + u8ModbusADU[u8ModbusADUSize++] = lowByte(_u16TransmitBuffer[i]); + } + break; + + case ku8MBMaskWriteRegister: + u8ModbusADU[u8ModbusADUSize++] = highByte(_u16TransmitBuffer[0]); + u8ModbusADU[u8ModbusADUSize++] = lowByte(_u16TransmitBuffer[0]); + u8ModbusADU[u8ModbusADUSize++] = highByte(_u16TransmitBuffer[1]); + u8ModbusADU[u8ModbusADUSize++] = lowByte(_u16TransmitBuffer[1]); + break; + } + + // append CRC + u16CRC = 0xFFFF; + for (i = 0; i < u8ModbusADUSize; i++) + { + u16CRC = crc16_update(u16CRC, u8ModbusADU[i]); + } + u8ModbusADU[u8ModbusADUSize++] = lowByte(u16CRC); + u8ModbusADU[u8ModbusADUSize++] = highByte(u16CRC); + u8ModbusADU[u8ModbusADUSize] = 0; + + // flush receive buffer before transmitting request + while (MBSerial->read() != -1); + +#if 0 + // transmit request + for (i = 0; i < u8ModbusADUSize; i++) + { +#if defined(ARDUINO) && ARDUINO >= 100 + MBSerial->write(u8ModbusADU[i]); +#else + MBSerial->print(u8ModbusADU[i], BYTE); +#endif + } +#else + MBSerial->write((char *)u8ModbusADU, u8ModbusADUSize); +#endif + //printf("TX: %02X\n", u8ModbusADU[0]); + u8ModbusADUSize = 0; + MBSerial->flush(); // flush transmit buffer + + // loop until we run out of time or bytes, or an error occurs + u32StartTime = millis(); + while (u8BytesLeft && !u8MBStatus) + { + if (MBSerial->available()) + { +#if __MODBUSMASTER_DEBUG__ + digitalWrite(4, true); +#endif + u8ModbusADU[u8ModbusADUSize++] = MBSerial->read(); + u8BytesLeft--; +#if __MODBUSMASTER_DEBUG__ + digitalWrite(4, false); +#endif + } + else + { +#if __MODBUSMASTER_DEBUG__ + digitalWrite(5, true); +#endif + if (_idle) + { + _idle(); + } +#if __MODBUSMASTER_DEBUG__ + digitalWrite(5, false); +#endif + } + + // evaluate slave ID, function code once enough bytes have been read + if (u8ModbusADUSize == 5) + { + // verify response is for correct Modbus slave + if (u8ModbusADU[0] != _u8MBSlave) + { + u8MBStatus = ku8MBInvalidSlaveID; + break; + } + + // verify response is for correct Modbus function code (mask exception bit 7) + if ((u8ModbusADU[1] & 0x7F) != u8MBFunction) + { + u8MBStatus = ku8MBInvalidFunction; + break; + } + + // check whether Modbus exception occurred; return Modbus Exception Code + if (bitRead(u8ModbusADU[1], 7)) + { + u8MBStatus = u8ModbusADU[2]; + break; + } + + // evaluate returned Modbus function code + switch(u8ModbusADU[1]) + { + case ku8MBReadCoils: + case ku8MBReadDiscreteInputs: + case ku8MBReadInputRegisters: + case ku8MBReadHoldingRegisters: + case ku8MBReadWriteMultipleRegisters: + u8BytesLeft = u8ModbusADU[2]; + break; + + case ku8MBWriteSingleCoil: + case ku8MBWriteMultipleCoils: + case ku8MBWriteSingleRegister: + case ku8MBWriteMultipleRegisters: + u8BytesLeft = 3; + break; + + case ku8MBMaskWriteRegister: + u8BytesLeft = 5; + break; + } + } + if ((millis() - u32StartTime) > ku16MBResponseTimeout) + { + u8MBStatus = ku8MBResponseTimedOut; + } + } + + // verify response is large enough to inspect further + if (!u8MBStatus && u8ModbusADUSize >= 5) + { + // calculate CRC + u16CRC = 0xFFFF; + for (i = 0; i < (u8ModbusADUSize - 2); i++) + { + u16CRC = crc16_update(u16CRC, u8ModbusADU[i]); + } + + // verify CRC + if (!u8MBStatus && (lowByte(u16CRC) != u8ModbusADU[u8ModbusADUSize - 2] || + highByte(u16CRC) != u8ModbusADU[u8ModbusADUSize - 1])) + { + u8MBStatus = ku8MBInvalidCRC; + } + } + + // disassemble ADU into words + if (!u8MBStatus) + { + // evaluate returned Modbus function code + switch(u8ModbusADU[1]) + { + case ku8MBReadCoils: + case ku8MBReadDiscreteInputs: + // load bytes into word; response bytes are ordered L, H, L, H, ... + for (i = 0; i < (u8ModbusADU[2] >> 1); i++) + { + if (i < ku8MaxBufferSize) + { + _u16ResponseBuffer[i] = word(u8ModbusADU[2 * i + 4], u8ModbusADU[2 * i + 3]); + } + + _u8ResponseBufferLength = i; + } + + // in the event of an odd number of bytes, load last byte into zero-padded word + if (u8ModbusADU[2] % 2) + { + if (i < ku8MaxBufferSize) + { + _u16ResponseBuffer[i] = word(0, u8ModbusADU[2 * i + 3]); + } + + _u8ResponseBufferLength = i + 1; + } + break; + + case ku8MBReadInputRegisters: + case ku8MBReadHoldingRegisters: + case ku8MBReadWriteMultipleRegisters: + // load bytes into word; response bytes are ordered H, L, H, L, ... + for (i = 0; i < (u8ModbusADU[2] >> 1); i++) + { + if (i < ku8MaxBufferSize) + { + _u16ResponseBuffer[i] = word(u8ModbusADU[2 * i + 3], u8ModbusADU[2 * i + 4]); + } + + _u8ResponseBufferLength = i; + } + break; + } + } + + _u8TransmitBufferIndex = 0; + u16TransmitBufferLength = 0; + _u8ResponseBufferIndex = 0; + return u8MBStatus; +} From a0635c90b07526ab57bab8cbb1b1007bb2d9b3f1 Mon Sep 17 00:00:00 2001 From: jaakkoiot Date: Sat, 15 Oct 2022 15:20:09 +0300 Subject: [PATCH 12/17] Add data structure ie. word for modbus comms --- Modbus/inc/word.h | 97 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 97 insertions(+) create mode 100644 Modbus/inc/word.h diff --git a/Modbus/inc/word.h b/Modbus/inc/word.h new file mode 100644 index 0000000..667fd4a --- /dev/null +++ b/Modbus/inc/word.h @@ -0,0 +1,97 @@ +/** +@file +Utility Functions for Manipulating Words + +@defgroup util_word "util/word.h": Utility Functions for Manipulating Words +@code#include "util/word.h"@endcode + +This header file provides utility functions for manipulating words. + +*/ +/* + + word.h - Utility Functions for Manipulating Words + + This file is part of ModbusMaster. + + ModbusMaster is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + ModbusMaster is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with ModbusMaster. If not, see . + + Written by Doc Walker (Rx) + Copyright © 2009-2015 Doc Walker <4-20ma at wvfans dot net> + +*/ + + +#ifndef _UTIL_WORD_H_ +#define _UTIL_WORD_H_ +#include + +/** @ingroup util_word + Return low word of a 32-bit integer. + + @param uint32_t ww (0x00000000..0xFFFFFFFF) + @return low word of input (0x0000..0xFFFF) +*/ +static inline uint16_t lowWord(uint32_t ww) { +return (uint16_t) ((ww) & 0xFFFF); +} + + +/** @ingroup util_word + Return high word of a 32-bit integer. + + @param uint32_t ww (0x00000000..0xFFFFFFFF) + @return high word of input (0x0000..0xFFFF) +*/ +static inline uint16_t highWord(uint32_t ww) +{ + return (uint16_t) ((ww) >> 16); +} + +/* utility functions for porting ModbusMaster to LPCXpresso + * added by krl + */ +static inline uint16_t word(uint8_t ww) +{ + return (uint16_t) (ww); +} + +static inline uint16_t word(uint8_t h, uint8_t l) +{ + return (uint16_t) ((h << 8) | l); +} + +static inline uint8_t highByte(uint16_t v) +{ + return (uint8_t) ((v >> 8) & 0xFF); +} + +static inline uint16_t lowByte(uint16_t v) +{ + return (uint8_t) (v & 0xFF); +} + +static inline uint8_t bitRead(uint8_t v, uint8_t n) +{ + return (uint8_t) (v & (1 << n) ? 1 : 0); +} + +static inline void bitWrite(uint16_t& v, uint8_t n, uint8_t b) +{ + if(b) v = v | (1 << n); + else v = v & ~(1 << n); +} + + +#endif /* _UTIL_WORD_H_ */ From 2c443737091c94d4e53e7bad090ba3dd4cc5775e Mon Sep 17 00:00:00 2001 From: jaakkoiot Date: Sat, 15 Oct 2022 15:21:28 +0300 Subject: [PATCH 13/17] 16 bit CRC error checking for modbus --- Modbus/inc/crc16.h | 88 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 88 insertions(+) create mode 100644 Modbus/inc/crc16.h diff --git a/Modbus/inc/crc16.h b/Modbus/inc/crc16.h new file mode 100644 index 0000000..0f50fdd --- /dev/null +++ b/Modbus/inc/crc16.h @@ -0,0 +1,88 @@ +/** +@file +CRC Computations + +@defgroup util_crc16 "util/crc16.h": CRC Computations +@code#include "util/crc16.h"@endcode + +This header file provides functions for calculating +cyclic redundancy checks (CRC) using common polynomials. +Modified by Doc Walker to be processor-independent (removed inline +assembler to allow it to compile on SAM3X8E processors). + +@par References: +Jack Crenshaw's "Implementing CRCs" article in the January 1992 issue of @e +Embedded @e Systems @e Programming. This may be difficult to find, but it +explains CRC's in very clear and concise terms. Well worth the effort to +obtain a copy. + +*/ +/* Copyright (c) 2002, 2003, 2004 Marek Michalkiewicz + Copyright (c) 2005, 2007 Joerg Wunsch + Copyright (c) 2013 Dave Hylands + Copyright (c) 2013 Frederic Nadeau + Copyright (c) 2015 Doc Walker + All rights reserved. + + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in + the documentation and/or other materials provided with the + distribution. + + * Neither the name of the copyright holders nor the names of + contributors may be used to endorse or promote products derived + from this software without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + POSSIBILITY OF SUCH DAMAGE. */ + + +#ifndef _UTIL_CRC16_H_ +#define _UTIL_CRC16_H_ +#include + +/** @ingroup util_crc16 + Processor-independent CRC-16 calculation. + + Polynomial: x^16 + x^15 + x^2 + 1 (0xA001)
+ Initial value: 0xFFFF + + This CRC is normally used in disk-drive controllers. + + @param uint16_t crc (0x0000..0xFFFF) + @param uint8_t a (0x00..0xFF) + @return calculated CRC (0x0000..0xFFFF) +*/ +static uint16_t crc16_update(uint16_t crc, uint8_t a) +{ + int i; + + crc ^= a; + for (i = 0; i < 8; ++i) + { + if (crc & 1) + crc = (crc >> 1) ^ 0xA001; + else + crc = (crc >> 1); + } + + return crc; +} + + +#endif /* _UTIL_CRC16_H_ */ From 443290a9ffe44c20a216a957a48caacf10179619 Mon Sep 17 00:00:00 2001 From: jaakkoiot Date: Mon, 17 Oct 2022 09:15:24 +0300 Subject: [PATCH 14/17] Modbus changed into static library --- Modbus/.cproject | 186 ++++++++++++++++++ Modbus/.project | 3 - Modbus/inc/{ModbusLeader.h => ModbusMaster.h} | 5 +- Modbus/inc/ModbusRegister.h | 2 +- Modbus/inc/SerialPort.h | 2 + Modbus/inc/Uart.h | 1 - Modbus/inc/crc16.h | 2 +- Modbus/inc/word.h | 7 +- .../{ModbusLeader.cpp => ModbusMaster.cpp} | 2 +- Modbus/src/SerialPort.cpp | 1 - Modbus/src/Uart.cpp | 13 -- Modbus/src/tester.cpp | 45 ----- 12 files changed, 198 insertions(+), 71 deletions(-) create mode 100644 Modbus/.cproject rename Modbus/inc/{ModbusLeader.h => ModbusMaster.h} (99%) rename Modbus/src/{ModbusLeader.cpp => ModbusMaster.cpp} (99%) delete mode 100644 Modbus/src/tester.cpp diff --git a/Modbus/.cproject b/Modbus/.cproject new file mode 100644 index 0000000..5e35dbd --- /dev/null +++ b/Modbus/.cproject @@ -0,0 +1,186 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + <?xml version="1.0" encoding="UTF-8"?> +<TargetConfig> +<Properties property_2="LPC15xx_256K.cfx" property_3="NXP" property_4="LPC1549" property_count="5" version="100300"/> +<infoList vendor="NXP"> +<info chip="LPC1549" connectscript="LPC15RunBootRomConnect.scp" flash_driver="LPC15xx_256K.cfx" match_id="0x0" name="LPC1549" resetscript="LPC15RunBootRomReset.scp" stub="crt_emu_cm3_gen"> +<chip> +<name>LPC1549</name> +<family>LPC15xx</family> +<vendor>NXP (formerly Philips)</vendor> +<reset board="None" core="Real" sys="Real"/> +<clock changeable="TRUE" freq="12MHz" is_accurate="TRUE"/> +<memory can_program="true" id="Flash" is_ro="true" type="Flash"/> +<memory id="RAM" type="RAM"/> +<memory id="Periph" is_volatile="true" type="Peripheral"/> +<memoryInstance derived_from="Flash" id="MFlash256" location="0x0" size="0x40000"/> +<memoryInstance derived_from="RAM" id="Ram0_16" location="0x2000000" size="0x4000"/> +<memoryInstance derived_from="RAM" id="Ram1_16" location="0x2004000" size="0x4000"/> +<memoryInstance derived_from="RAM" id="Ram2_4" location="0x2008000" size="0x1000"/> +</chip> +<processor> +<name gcc_name="cortex-m3">Cortex-M3</name> +<family>Cortex-M</family> +</processor> +</info> +</infoList> +</TargetConfig> + + + LPCXpresso1549 + + diff --git a/Modbus/.project b/Modbus/.project index c7d3ce5..3e28dd2 100644 --- a/Modbus/.project +++ b/Modbus/.project @@ -3,8 +3,6 @@ Modbus - lpc_chip_15xx - DigitalIoPin @@ -22,7 +20,6 @@ org.eclipse.cdt.core.cnature - org.eclipse.cdt.core.ccnature org.eclipse.cdt.managedbuilder.core.managedBuildNature org.eclipse.cdt.managedbuilder.core.ScannerConfigNature diff --git a/Modbus/inc/ModbusLeader.h b/Modbus/inc/ModbusMaster.h similarity index 99% rename from Modbus/inc/ModbusLeader.h rename to Modbus/inc/ModbusMaster.h index d038569..18e7978 100644 --- a/Modbus/inc/ModbusLeader.h +++ b/Modbus/inc/ModbusMaster.h @@ -56,7 +56,6 @@ Set to 1 to enable debugging features within class: #include #include #endif -#include uint32_t millis(); #define BYTE 0xA5 @@ -72,6 +71,8 @@ uint32_t millis(); // functions to manipulate words ///#include "util/word.h" #include "word.h" + + #include "SerialPort.h" /* _____CLASS DEFINITIONS____________________________________________________ */ @@ -278,4 +279,4 @@ class ModbusMaster /** @example examples/Basic/Basic.pde @example examples/PhoenixContact_nanoLC/PhoenixContact_nanoLC.pde -*/ \ No newline at end of file +*/ diff --git a/Modbus/inc/ModbusRegister.h b/Modbus/inc/ModbusRegister.h index af20be9..198d94e 100644 --- a/Modbus/inc/ModbusRegister.h +++ b/Modbus/inc/ModbusRegister.h @@ -1,7 +1,7 @@ #ifndef MODBUSREGISTER_H_ #define MODBUSREGISTER_H_ -#include "ModbusLeader.h" +#include "ModbusMaster.h" class ModbusRegister { public: diff --git a/Modbus/inc/SerialPort.h b/Modbus/inc/SerialPort.h index 6e89b6e..f84581d 100644 --- a/Modbus/inc/SerialPort.h +++ b/Modbus/inc/SerialPort.h @@ -1,3 +1,5 @@ + + #ifndef SERIALPORT_H_ #define SERIALPORT_H_ diff --git a/Modbus/inc/Uart.h b/Modbus/inc/Uart.h index ce3c007..c72ac3b 100644 --- a/Modbus/inc/Uart.h +++ b/Modbus/inc/Uart.h @@ -8,7 +8,6 @@ #ifndef LPCUART_H_ #define LPCUART_H_ -#include #include "chip.h" struct LpcPinMap { diff --git a/Modbus/inc/crc16.h b/Modbus/inc/crc16.h index 0f50fdd..27ea89e 100644 --- a/Modbus/inc/crc16.h +++ b/Modbus/inc/crc16.h @@ -54,7 +54,7 @@ obtain a copy. #ifndef _UTIL_CRC16_H_ #define _UTIL_CRC16_H_ -#include + /** @ingroup util_crc16 Processor-independent CRC-16 calculation. diff --git a/Modbus/inc/word.h b/Modbus/inc/word.h index 667fd4a..78db36c 100644 --- a/Modbus/inc/word.h +++ b/Modbus/inc/word.h @@ -35,7 +35,7 @@ This header file provides utility functions for manipulating words. #ifndef _UTIL_WORD_H_ #define _UTIL_WORD_H_ -#include + /** @ingroup util_word Return low word of a 32-bit integer. @@ -43,8 +43,9 @@ This header file provides utility functions for manipulating words. @param uint32_t ww (0x00000000..0xFFFFFFFF) @return low word of input (0x0000..0xFFFF) */ -static inline uint16_t lowWord(uint32_t ww) { -return (uint16_t) ((ww) & 0xFFFF); +static inline uint16_t lowWord(uint32_t ww) +{ + return (uint16_t) ((ww) & 0xFFFF); } diff --git a/Modbus/src/ModbusLeader.cpp b/Modbus/src/ModbusMaster.cpp similarity index 99% rename from Modbus/src/ModbusLeader.cpp rename to Modbus/src/ModbusMaster.cpp index 92a8958..4aef169 100644 --- a/Modbus/src/ModbusLeader.cpp +++ b/Modbus/src/ModbusMaster.cpp @@ -29,7 +29,7 @@ Arduino library for communicating with Modbus slaves over RS232/485 (via RTU pro /* _____PROJECT INCLUDES_____________________________________________________ */ -#include "ModbusLeader.h" +#include "ModbusMaster.h" #include "crc16.h" diff --git a/Modbus/src/SerialPort.cpp b/Modbus/src/SerialPort.cpp index 953d99a..36084a4 100644 --- a/Modbus/src/SerialPort.cpp +++ b/Modbus/src/SerialPort.cpp @@ -1,7 +1,6 @@ #include "SerialPort.h" - SerialPort::SerialPort() { if(!u) { LpcPinMap none = {-1, -1}; // unused pin has negative values in it diff --git a/Modbus/src/Uart.cpp b/Modbus/src/Uart.cpp index 6a45cae..87b725a 100644 --- a/Modbus/src/Uart.cpp +++ b/Modbus/src/Uart.cpp @@ -1,7 +1,6 @@ #include #include "Uart.h" - static LpcUart *u0; static LpcUart *u1; static LpcUart *u2; @@ -13,28 +12,16 @@ extern "C" { */ void UART0_IRQHandler(void) { - /* Want to handle any errors? Do it here. */ - - /* Use default ring buffer handler. Override this with your own - code if you need more capability. */ if(u0) u0->isr(); } void UART1_IRQHandler(void) { - /* Want to handle any errors? Do it here. */ - - /* Use default ring buffer handler. Override this with your own - code if you need more capability. */ if(u1) u1->isr(); } void UART2_IRQHandler(void) { - /* Want to handle any errors? Do it here. */ - - /* Use default ring buffer handler. Override this with your own - code if you need more capability. */ if(u2) u2->isr(); } diff --git a/Modbus/src/tester.cpp b/Modbus/src/tester.cpp deleted file mode 100644 index a2935ea..0000000 --- a/Modbus/src/tester.cpp +++ /dev/null @@ -1,45 +0,0 @@ -#if defined (__USE_LPCOPEN) -#if defined(NO_BOARD_LIB) -#include "chip.h" -#else -#include "board.h" -#endif -#endif - -#include -#include "ModbusMaster.h" -// TODO: insert other include files here - -// TODO: insert other definitions and declarations here - -int main(void) { - -#if defined (__USE_LPCOPEN) - // Read clock settings and update SystemCoreClock variable - SystemCoreClockUpdate(); -#if !defined(NO_BOARD_LIB) - // Set up and initialize all required blocks and - // functions related to the board hardware - Board_Init(); - // Set the LED to the state of "On" - Board_LED_Set(0, true); -#endif -#endif - - // TODO: insert code here - ModbusMaster MIO_12V(1); - ModbusMaster CO2_sensor(240); - ModbusMaster RH_sensor(241); - - - // Force the counter to be placed into memory - volatile static int i = 0 ; - // Enter an infinite loop, just incrementing a counter - while(1) { - i++ ; - // "Dummy" NOP to allow source level single - // stepping of tight while() loop - __asm volatile ("nop"); - } - return 0 ; -} From 92fd637a26b3dcd4506ba19737ea245ede54b4f5 Mon Sep 17 00:00:00 2001 From: jaakkoiot Date: Tue, 18 Oct 2022 02:21:50 +0300 Subject: [PATCH 15/17] Some cleaning --- Modbus/inc/ModbusMaster.h | 7 +++++-- Modbus/inc/SerialPort.h | 2 -- Modbus/src/ModbusMaster.cpp | 1 + 3 files changed, 6 insertions(+), 4 deletions(-) diff --git a/Modbus/inc/ModbusMaster.h b/Modbus/inc/ModbusMaster.h index 18e7978..21e2254 100644 --- a/Modbus/inc/ModbusMaster.h +++ b/Modbus/inc/ModbusMaster.h @@ -53,6 +53,7 @@ Set to 1 to enable debugging features within class: #include "Arduino.h" #else //#include "WProgram.h" +#include "SerialPort.h" #include #include #endif @@ -243,10 +244,10 @@ class ModbusMaster uint16_t _u16WriteAddress; ///< slave register to which to write uint16_t _u16WriteQty; ///< quantity of words to write uint16_t _u16TransmitBuffer[ku8MaxBufferSize]; ///< buffer containing data to transmit to Modbus slave; set via SetTransmitBuffer() - uint16_t* txBuffer; // from Wire.h -- need to clean this up Rx + uint16_t* txBuffer; uint8_t _u8TransmitBufferIndex; uint16_t u16TransmitBufferLength; - uint16_t* rxBuffer; // from Wire.h -- need to clean this up Rx + uint16_t* rxBuffer; uint8_t _u8ResponseBufferIndex; uint8_t _u8ResponseBufferLength; @@ -255,6 +256,8 @@ class ModbusMaster static const uint8_t ku8MBReadDiscreteInputs = 0x02; ///< Modbus function 0x02 Read Discrete Inputs static const uint8_t ku8MBWriteSingleCoil = 0x05; ///< Modbus function 0x05 Write Single Coil static const uint8_t ku8MBWriteMultipleCoils = 0x0F; ///< Modbus function 0x0F Write Multiple Coils + static const uint8_t ku8MBWriteMultipleRegisters = 0x10; ///< Modbus function 0x10 Write Multiple Registers + // Modbus function codes for 16 bit access static const uint8_t ku8MBReadHoldingRegisters = 0x03; ///< Modbus function 0x03 Read Holding Registers diff --git a/Modbus/inc/SerialPort.h b/Modbus/inc/SerialPort.h index f84581d..6e89b6e 100644 --- a/Modbus/inc/SerialPort.h +++ b/Modbus/inc/SerialPort.h @@ -1,5 +1,3 @@ - - #ifndef SERIALPORT_H_ #define SERIALPORT_H_ diff --git a/Modbus/src/ModbusMaster.cpp b/Modbus/src/ModbusMaster.cpp index 4aef169..4dee284 100644 --- a/Modbus/src/ModbusMaster.cpp +++ b/Modbus/src/ModbusMaster.cpp @@ -39,6 +39,7 @@ Arduino library for communicating with Modbus slaves over RS232/485 (via RTU pro #elif defined(ARDUINO_ARCH_SAM) UARTClass* MBSerial = &Serial; ///< Pointer to Serial class object #else +//In the case of undefined Serial the code should still function // #error "This library only supports boards with an AVR or SAM processor. Please open an issue at https://github.com/4-20ma/ModbusMaster/issues and indicate which processor/platform you're using." #endif From 2e731f59a002eb2c0a9d2835611ab289bcd306a7 Mon Sep 17 00:00:00 2001 From: jaakkoiot Date: Tue, 18 Oct 2022 03:01:21 +0300 Subject: [PATCH 16/17] Remove comment --- Modbus/inc/Uart.h | 7 ------- 1 file changed, 7 deletions(-) diff --git a/Modbus/inc/Uart.h b/Modbus/inc/Uart.h index c72ac3b..2b88dac 100644 --- a/Modbus/inc/Uart.h +++ b/Modbus/inc/Uart.h @@ -1,10 +1,3 @@ -/* - * LpcUart.h - * - * Created on: 4.2.2019 - * Author: keijo - */ - #ifndef LPCUART_H_ #define LPCUART_H_ From aa6d51b3da8296b45a2175685bc987e347999ab4 Mon Sep 17 00:00:00 2001 From: jaakkoiot Date: Tue, 18 Oct 2022 03:15:57 +0300 Subject: [PATCH 17/17] Add comment on serial port object --- Modbus/src/ModbusMaster.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/Modbus/src/ModbusMaster.cpp b/Modbus/src/ModbusMaster.cpp index bf532c5..4dee284 100644 --- a/Modbus/src/ModbusMaster.cpp +++ b/Modbus/src/ModbusMaster.cpp @@ -39,10 +39,7 @@ Arduino library for communicating with Modbus slaves over RS232/485 (via RTU pro #elif defined(ARDUINO_ARCH_SAM) UARTClass* MBSerial = &Serial; ///< Pointer to Serial class object #else -<<<<<<< HEAD //In the case of undefined Serial the code should still function -======= ->>>>>>> bada194ebe7490c25ca7589366224d4ae0f80eab // #error "This library only supports boards with an AVR or SAM processor. Please open an issue at https://github.com/4-20ma/ModbusMaster/issues and indicate which processor/platform you're using." #endif