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
new file mode 100644
index 0000000..3e28dd2
--- /dev/null
+++ b/Modbus/.project
@@ -0,0 +1,26 @@
+
+
+ Modbus
+
+
+
+
+
+ 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.managedbuilder.core.managedBuildNature
+ org.eclipse.cdt.managedbuilder.core.ScannerConfigNature
+
+
diff --git a/Modbus/inc/ModbusMaster.h b/Modbus/inc/ModbusMaster.h
new file mode 100644
index 0000000..18e7978
--- /dev/null
+++ b/Modbus/inc/ModbusMaster.h
@@ -0,0 +1,282 @@
+/**
+@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
+
+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
+*/
diff --git a/Modbus/inc/ModbusRegister.h b/Modbus/inc/ModbusRegister.h
new file mode 100644
index 0000000..198d94e
--- /dev/null
+++ b/Modbus/inc/ModbusRegister.h
@@ -0,0 +1,19 @@
+#ifndef MODBUSREGISTER_H_
+#define MODBUSREGISTER_H_
+
+#include "ModbusMaster.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/inc/SerialPort.h b/Modbus/inc/SerialPort.h
new file mode 100644
index 0000000..743e068
--- /dev/null
+++ b/Modbus/inc/SerialPort.h
@@ -0,0 +1,25 @@
+<<<<<<< HEAD
+=======
+
+
+>>>>>>> bada194ebe7490c25ca7589366224d4ae0f80eab
+#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..2b88dac
--- /dev/null
+++ b/Modbus/inc/Uart.h
@@ -0,0 +1,54 @@
+#ifndef LPCUART_H_
+#define LPCUART_H_
+
+#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/inc/crc16.h b/Modbus/inc/crc16.h
new file mode 100644
index 0000000..27ea89e
--- /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_
+
+
+/** @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_ */
diff --git a/Modbus/inc/word.h b/Modbus/inc/word.h
new file mode 100644
index 0000000..78db36c
--- /dev/null
+++ b/Modbus/inc/word.h
@@ -0,0 +1,98 @@
+/**
+@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_
+
+
+/** @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_ */
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/ModbusMaster.cpp b/Modbus/src/ModbusMaster.cpp
new file mode 100644
index 0000000..4dee284
--- /dev/null
+++ b/Modbus/src/ModbusMaster.cpp
@@ -0,0 +1,938 @@
+/**
+@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 "ModbusMaster.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
+//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
+
+
+/* _____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;
+}
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
+
+}
diff --git a/Modbus/src/SerialPort.cpp b/Modbus/src/SerialPort.cpp
new file mode 100644
index 0000000..36084a4
--- /dev/null
+++ b/Modbus/src/SerialPort.cpp
@@ -0,0 +1,49 @@
+#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..87b725a
--- /dev/null
+++ b/Modbus/src/Uart.cpp
@@ -0,0 +1,214 @@
+#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)
+{
+ if(u0) u0->isr();
+}
+
+void UART1_IRQHandler(void)
+{
+ if(u1) u1->isr();
+}
+
+void UART2_IRQHandler(void)
+{
+ 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);
+}