API tools faq
paste
Guest User

Untitled

a guest
Apr 6th, 2017
268
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 37.33 KB | None | 0 0
raw download clone embed print report
  1. /**
  2.  * @file    ModbusRtu.h
  3.  * @version     1.21
  4.  * @date        2016.02.21
  5.  * @author  Samuel Marco i Armengol
  6.  * @contact     [email protected]
  7.  * @contribution Helium6072
  8.  *
  9.  * @description
  10.  *  Arduino library for communicating with Modbus devices
  11.  *  over RS232/USB/485 via RTU protocol.
  12.  *
  13.  *  Further information:
  14.  *  http://modbus.org/
  15.  *  http://modbus.org/docs/Modbus_over_serial_line_V1_02.pdf
  16.  *
  17.  * @license
  18.  *  This library is free software; you can redistribute it and/or
  19.  *  modify it under the terms of the GNU Lesser General Public
  20.  *  License as published by the Free Software Foundation; version
  21.  *  2.1 of the License.
  22.  *
  23.  *  This library is distributed in the hope that it will be useful,
  24.  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  25.  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  26.  *  Lesser General Public License for more details.
  27.  *
  28.  *  You should have received a copy of the GNU Lesser General Public
  29.  *  License along with this library; if not, write to the Free Software
  30.  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  31.  *
  32.  * @defgroup setup Modbus Object Instantiation/Initialization
  33.  * @defgroup loop Modbus Object Management
  34.  * @defgroup buffer Modbus Buffer Management
  35.  * @defgroup discrete Modbus Function Codes for Discrete Coils/Inputs
  36.  * @defgroup register Modbus Function Codes for Holding/Input Registers
  37.  *
  38.  */
  39.  
  40. #include <inttypes.h>
  41. #include "Arduino.h"
  42. #include "Print.h"
  43. #include <SoftwareSerial.h>
  44.  
  45. /**
  46.  * @struct modbus_t
  47.  * @brief
  48.  * Master query structure:
  49.  * This includes all the necessary fields to make the Master generate a Modbus query.
  50.  * A Master may keep several of these structures and send them cyclically or
  51.  * use them according to program needs.
  52.  */
  53. typedef struct
  54. {
  55.     uint8_t u8id;          /*!< Slave address between 1 and 247. 0 means broadcast */
  56.     uint8_t u8fct;         /*!< Function code: 1, 2, 3, 4, 5, 6, 15 or 16 */
  57.     uint16_t u16RegAdd;    /*!< Address of the first register to access at slave/s */
  58.     uint16_t u16CoilsNo;   /*!< Number of coils or registers to access */
  59.     uint16_t *au16reg;     /*!< Pointer to memory image in master */
  60. }
  61. modbus_t;
  62.  
  63. enum
  64. {
  65.     RESPONSE_SIZE = 6,
  66.     EXCEPTION_SIZE = 3,
  67.     CHECKSUM_SIZE = 2
  68. };
  69.  
  70. /**
  71.  * @enum MESSAGE
  72.  * @brief
  73.  * Indexes to telegram frame positions
  74.  */
  75. enum MESSAGE
  76. {
  77.     ID                             = 0, //!< ID field
  78.     FUNC, //!< Function code position
  79.     ADD_HI, //!< Address high byte
  80.     ADD_LO, //!< Address low byte
  81.     NB_HI, //!< Number of coils or registers high byte
  82.     NB_LO, //!< Number of coils or registers low byte
  83.     BYTE_CNT  //!< byte counter
  84. };
  85.  
  86. /**
  87.  * @enum MB_FC
  88.  * @brief
  89.  * Modbus function codes summary.
  90.  * These are the implement function codes either for Master or for Slave.
  91.  *
  92.  * @see also fctsupported
  93.  * @see also modbus_t
  94.  */
  95. enum MB_FC
  96. {
  97.     MB_FC_NONE                     = 0,   /*!< null operator */
  98.     MB_FC_READ_COILS               = 1, /*!< FCT=1 -> read coils or digital outputs */
  99.     MB_FC_READ_DISCRETE_INPUT      = 2, /*!< FCT=2 -> read digital inputs */
  100.     MB_FC_READ_REGISTERS           = 3, /*!< FCT=3 -> read registers or analog outputs */
  101.     MB_FC_READ_INPUT_REGISTER      = 4, /*!< FCT=4 -> read analog inputs */
  102.     MB_FC_WRITE_COIL               = 5, /*!< FCT=5 -> write single coil or output */
  103.     MB_FC_WRITE_REGISTER           = 6, /*!< FCT=6 -> write single register */
  104.     MB_FC_WRITE_MULTIPLE_COILS     = 15,    /*!< FCT=15 -> write multiple coils or outputs */
  105.     MB_FC_WRITE_MULTIPLE_REGISTERS = 16 /*!< FCT=16 -> write multiple registers */
  106. };
  107.  
  108. enum COM_STATES
  109. {
  110.     COM_IDLE                     = 0,
  111.     COM_WAITING                  = 1
  112.  
  113. };
  114.  
  115. enum ERR_LIST
  116. {
  117.     ERR_NOT_MASTER                = -1,
  118.     ERR_POLLING                   = -2,
  119.     ERR_BUFF_OVERFLOW             = -3,
  120.     ERR_BAD_CRC                   = -4,
  121.     ERR_EXCEPTION                 = -5
  122. };
  123.  
  124. enum
  125. {
  126.     NO_REPLY = 255,
  127.     EXC_FUNC_CODE = 1,
  128.     EXC_ADDR_RANGE = 2,
  129.     EXC_REGS_QUANT = 3,
  130.     EXC_EXECUTE = 4
  131. };
  132.  
  133. const unsigned char fctsupported[] =
  134. {
  135.     MB_FC_READ_COILS,
  136.     MB_FC_READ_DISCRETE_INPUT,
  137.     MB_FC_READ_REGISTERS,
  138.     MB_FC_READ_INPUT_REGISTER,
  139.     MB_FC_WRITE_COIL,
  140.     MB_FC_WRITE_REGISTER,
  141.     MB_FC_WRITE_MULTIPLE_COILS,
  142.     MB_FC_WRITE_MULTIPLE_REGISTERS
  143. };
  144.  
  145. #define T35  5
  146. #define  MAX_BUFFER  64 //!< maximum size for the communication buffer in bytes
  147.  
  148. /**
  149.  * @class Modbus
  150.  * @brief
  151.  * Arduino class library for communicating with Modbus devices over
  152.  * USB/RS232/485 (via RTU protocol).
  153.  */
  154. class Modbus
  155. {
  156. private:
  157.     HardwareSerial *port; //!< Pointer to Serial class object
  158.     SoftwareSerial *softPort; //!< Pointer to SoftwareSerial class object
  159.     uint8_t u8id; //!< 0=master, 1..247=slave number
  160.     uint8_t u8serno; //!< serial port: 0-Serial, 1..3-Serial1..Serial3; 4: use software serial
  161.     uint8_t u8txenpin; //!< flow control pin: 0=USB or RS-232 mode, >0=RS-485 mode
  162.     uint8_t u8state;
  163.     uint8_t u8lastError;
  164.     uint8_t au8Buffer[MAX_BUFFER];
  165.     uint8_t u8BufferSize;
  166.     uint8_t u8lastRec;
  167.     uint16_t *au16regs;
  168.     uint16_t u16InCnt, u16OutCnt, u16errCnt;
  169.     uint16_t u16timeOut;
  170.     uint32_t u32time, u32timeOut;
  171.     uint8_t u8regsize;
  172.  
  173.     void init(uint8_t u8id, uint8_t u8serno, uint8_t u8txenpin);
  174.     void init(uint8_t u8id);
  175.     void sendTxBuffer();
  176.     int8_t getRxBuffer();
  177.     uint16_t calcCRC(uint8_t u8length);
  178.     uint8_t validateAnswer();
  179.     uint8_t validateRequest();
  180.     void get_FC1();
  181.     void get_FC3();
  182.     int8_t process_FC1( uint16_t *regs, uint8_t u8size );
  183.     int8_t process_FC3( uint16_t *regs, uint8_t u8size );
  184.     int8_t process_FC5( uint16_t *regs, uint8_t u8size );
  185.     int8_t process_FC6( uint16_t *regs, uint8_t u8size );
  186.     int8_t process_FC15( uint16_t *regs, uint8_t u8size );
  187.     int8_t process_FC16( uint16_t *regs, uint8_t u8size );
  188.     void buildException( uint8_t u8exception ); // build exception message
  189.  
  190. public:
  191.     Modbus();
  192.     Modbus(uint8_t u8id, uint8_t u8serno);
  193.     Modbus(uint8_t u8id, uint8_t u8serno, uint8_t u8txenpin);
  194.     Modbus(uint8_t u8id);
  195.     void begin(long u32speed);
  196.     void begin(SoftwareSerial *sPort, long u32speed);
  197.     void begin(long u32speed, uint8_t u8config);
  198.     void begin();
  199.     void setTimeOut( uint16_t u16timeout); //!<write communication watch-dog timer
  200.     uint16_t getTimeOut(); //!<get communication watch-dog timer value
  201.     boolean getTimeOutState(); //!<get communication watch-dog timer state
  202.     int8_t query( modbus_t telegram ); //!<only for master
  203.     int8_t poll(); //!<cyclic poll for master
  204.     int8_t poll( uint16_t *regs, uint8_t u8size ); //!<cyclic poll for slave
  205.     uint16_t getInCnt(); //!<number of incoming messages
  206.     uint16_t getOutCnt(); //!<number of outcoming messages
  207.     uint16_t getErrCnt(); //!<error counter
  208.     uint8_t getID(); //!<get slave ID between 1 and 247
  209.     uint8_t getState();
  210.     uint8_t getLastError(); //!<get last error message
  211.     void setID( uint8_t u8id ); //!<write new ID for the slave
  212.     void end(); //!<finish any communication and release serial communication port
  213. };
  214.  
  215. /* _____PUBLIC FUNCTIONS_____________________________________________________ */
  216.  
  217. /**
  218.  * @brief
  219.  * Default Constructor for Master through Serial
  220.  *
  221.  * @ingroup setup
  222.  */
  223. Modbus::Modbus()
  224. {
  225.     init(0, 0, 0);
  226. }
  227.  
  228. /**
  229.  * @brief
  230.  * Full constructor for a Master/Slave through USB/RS232C
  231.  *
  232.  * @param u8id   node address 0=master, 1..247=slave
  233.  * @param u8serno  serial port used 0..3
  234.  * @ingroup setup
  235.  * @overload Modbus::Modbus(uint8_t u8id, uint8_t u8serno)
  236.  * @overload Modbus::Modbus(uint8_t u8id)
  237.  * @overload Modbus::Modbus()
  238.  */
  239. Modbus::Modbus(uint8_t u8id, uint8_t u8serno)
  240. {
  241.     init(u8id, u8serno, 0);
  242. }
  243.  
  244. /**
  245.  * @brief
  246.  * Full constructor for a Master/Slave through USB/RS232C/RS485
  247.  * It needs a pin for flow control only for RS485 mode
  248.  *
  249.  * @param u8id   node address 0=master, 1..247=slave
  250.  * @param u8serno  serial port used 0..3
  251.  * @param u8txenpin pin for txen RS-485 (=0 means USB/RS232C mode)
  252.  * @ingroup setup
  253.  * @overload Modbus::Modbus(uint8_t u8id, uint8_t u8serno, uint8_t u8txenpin)
  254.  * @overload Modbus::Modbus(uint8_t u8id)
  255.  * @overload Modbus::Modbus()
  256.  */
  257. Modbus::Modbus(uint8_t u8id, uint8_t u8serno, uint8_t u8txenpin)
  258. {
  259.     init(u8id, u8serno, u8txenpin);
  260. }
  261.  
  262. /**
  263.  * @brief
  264.  * Constructor for a Master/Slave through USB/RS232C via software serial
  265.  * This constructor only specifies u8id (node address) and should be only
  266.  * used if you want to use software serial instead of hardware serial.
  267.  * If you use this constructor you have to begin ModBus object by
  268.  * using "void Modbus::begin(SoftwareSerial *softPort, long u32speed)".
  269.  *
  270.  * @param u8id   node address 0=master, 1..247=slave
  271.  * @ingroup setup
  272.  * @overload Modbus::Modbus(uint8_t u8id, uint8_t u8serno, uint8_t u8txenpin)
  273.  * @overload Modbus::Modbus(uint8_t u8id, uint8_t u8serno)
  274.  * @overload Modbus::Modbus()
  275.  */
  276. Modbus::Modbus(uint8_t u8id)
  277. {
  278.     init(u8id);
  279. }
  280.  
  281. /**
  282.  * @brief
  283.  * Initialize class object.
  284.  *
  285.  * Sets up the serial port using specified baud rate.
  286.  * Call once class has been instantiated, typically within setup().
  287.  *
  288.  * @see http://arduino.cc/en/Serial/Begin#.Uy4CJ6aKlHY
  289.  * @param speed   baud rate, in standard increments (300..115200)
  290.  * @ingroup setup
  291.  */
  292. void Modbus::begin(long u32speed)
  293. {
  294.  
  295.     switch( u8serno )
  296.     {
  297. #if defined(UBRR1H)
  298.     case 1:
  299.         port = &Serial1;
  300.         break;
  301. #endif
  302.  
  303. #if defined(UBRR2H)
  304.     case 2:
  305.         port = &Serial2;
  306.         break;
  307. #endif
  308.  
  309. #if defined(UBRR3H)
  310.     case 3:
  311.         port = &Serial3;
  312.         break;
  313. #endif
  314.     case 0:
  315.     default:
  316.         port = &Serial;
  317.         break;
  318.     }
  319.  
  320.     port->begin(u32speed);
  321.     if (u8txenpin > 1)   // pin 0 & pin 1 are reserved for RX/TX
  322.     {
  323.         // return RS485 transceiver to transmit mode
  324.         pinMode(u8txenpin, OUTPUT);
  325.         digitalWrite(u8txenpin, LOW);
  326.     }
  327.  
  328.     while(port->read() >= 0);
  329.     u8lastRec = u8BufferSize = 0;
  330.     u16InCnt = u16OutCnt = u16errCnt = 0;
  331. }
  332.  
  333. /**
  334.  * @brief
  335.  * Initialize class object.
  336.  *
  337.  * Sets up the software serial port using specified baud rate and SoftwareSerial object.
  338.  * Call once class has been instantiated, typically within setup().
  339.  *
  340.  * @param speed   *softPort, pointer to SoftwareSerial class object
  341.  * @param speed   baud rate, in standard increments (300..115200)
  342.  * @ingroup setup
  343.  */
  344. void Modbus::begin(SoftwareSerial *sPort, long u32speed)
  345. {
  346.  
  347.     softPort=sPort;
  348.  
  349.     softPort->begin(u32speed);
  350.  
  351.     if (u8txenpin > 1)   // pin 0 & pin 1 are reserved for RX/TX
  352.     {
  353.         // return RS485 transceiver to transmit mode
  354.         pinMode(u8txenpin, OUTPUT);
  355.         digitalWrite(u8txenpin, LOW);
  356.     }
  357.  
  358.     while(softPort->read() >= 0);
  359.     u8lastRec = u8BufferSize = 0;
  360.     u16InCnt = u16OutCnt = u16errCnt = 0;
  361. }
  362.  
  363. /**
  364.  * @brief
  365.  * Initialize class object.
  366.  *
  367.  * Sets up the serial port using specified baud rate.
  368.  * Call once class has been instantiated, typically within setup().
  369.  *
  370.  * @see http://arduino.cc/en/Serial/Begin#.Uy4CJ6aKlHY
  371.  * @param speed   baud rate, in standard increments (300..115200)
  372.  * @param config  data frame settings (data length, parity and stop bits)
  373.  * @ingroup setup
  374.  */
  375. void Modbus::begin(long u32speed,uint8_t u8config)
  376. {
  377.  
  378.     switch( u8serno )
  379.     {
  380. #if defined(UBRR1H)
  381.     case 1:
  382.         port = &Serial1;
  383.         break;
  384. #endif
  385.  
  386. #if defined(UBRR2H)
  387.     case 2:
  388.         port = &Serial2;
  389.         break;
  390. #endif
  391.  
  392. #if defined(UBRR3H)
  393.     case 3:
  394.         port = &Serial3;
  395.         break;
  396. #endif
  397.     case 0:
  398.     default:
  399.         port = &Serial;
  400.         break;
  401.     }
  402.  
  403.     port->begin(u32speed, u8config);
  404.     if (u8txenpin > 1)   // pin 0 & pin 1 are reserved for RX/TX
  405.     {
  406.         // return RS485 transceiver to transmit mode
  407.         pinMode(u8txenpin, OUTPUT);
  408.         digitalWrite(u8txenpin, LOW);
  409.     }
  410.  
  411.     while(port->read() >= 0);
  412.     u8lastRec = u8BufferSize = 0;
  413.     u16InCnt = u16OutCnt = u16errCnt = 0;
  414. }
  415.  
  416. /**
  417.  * @brief
  418.  * Initialize default class object.
  419.  *
  420.  * Sets up the serial port using 19200 baud.
  421.  * Call once class has been instantiated, typically within setup().
  422.  *
  423.  * @overload Modbus::begin(uint16_t u16BaudRate)
  424.  * @ingroup setup
  425.  */
  426. void Modbus::begin()
  427. {
  428.     begin(115200);
  429. }
  430.  
  431. /**
  432.  * @brief
  433.  * Method to write a new slave ID address
  434.  *
  435.  * @param   u8id    new slave address between 1 and 247
  436.  * @ingroup setup
  437.  */
  438. void Modbus::setID( uint8_t u8id)
  439. {
  440.     if (( u8id != 0) && (u8id <= 247))
  441.     {
  442.         this->u8id = u8id;
  443.     }
  444. }
  445.  
  446. /**
  447.  * @brief
  448.  * Method to read current slave ID address
  449.  *
  450.  * @return u8id current slave address between 1 and 247
  451.  * @ingroup setup
  452.  */
  453. uint8_t Modbus::getID()
  454. {
  455.     return this->u8id;
  456. }
  457.  
  458. /**
  459.  * @brief
  460.  * Initialize time-out parameter
  461.  *
  462.  * Call once class has been instantiated, typically within setup().
  463.  * The time-out timer is reset each time that there is a successful communication
  464.  * between Master and Slave. It works for both.
  465.  *
  466.  * @param time-out value (ms)
  467.  * @ingroup setup
  468.  */
  469. void Modbus::setTimeOut( uint16_t u16timeOut)
  470. {
  471.     this->u16timeOut = u16timeOut;
  472. }
  473.  
  474. /**
  475.  * @brief
  476.  * Return communication Watchdog state.
  477.  * It could be usefull to reset outputs if the watchdog is fired.
  478.  *
  479.  * @return TRUE if millis() > u32timeOut
  480.  * @ingroup loop
  481.  */
  482. boolean Modbus::getTimeOutState()
  483. {
  484.     return (millis() > u32timeOut);
  485. }
  486.  
  487. /**
  488.  * @brief
  489.  * Get input messages counter value
  490.  * This can be useful to diagnose communication
  491.  *
  492.  * @return input messages counter
  493.  * @ingroup buffer
  494.  */
  495. uint16_t Modbus::getInCnt()
  496. {
  497.     return u16InCnt;
  498. }
  499.  
  500. /**
  501.  * @brief
  502.  * Get transmitted messages counter value
  503.  * This can be useful to diagnose communication
  504.  *
  505.  * @return transmitted messages counter
  506.  * @ingroup buffer
  507.  */
  508. uint16_t Modbus::getOutCnt()
  509. {
  510.     return u16OutCnt;
  511. }
  512.  
  513. /**
  514.  * @brief
  515.  * Get errors counter value
  516.  * This can be useful to diagnose communication
  517.  *
  518.  * @return errors counter
  519.  * @ingroup buffer
  520.  */
  521. uint16_t Modbus::getErrCnt()
  522. {
  523.     return u16errCnt;
  524. }
  525.  
  526. /**
  527.  * Get modbus master state
  528.  *
  529.  * @return = 0 IDLE, = 1 WAITING FOR ANSWER
  530.  * @ingroup buffer
  531.  */
  532. uint8_t Modbus::getState()
  533. {
  534.     return u8state;
  535. }
  536.  
  537. /**
  538.  * Get the last error in the protocol processor
  539.  *
  540.  * @returnreturn   NO_REPLY = 255      Time-out
  541.  * @return   EXC_FUNC_CODE = 1   Function code not available
  542.  * @return   EXC_ADDR_RANGE = 2  Address beyond available space for Modbus registers
  543.  * @return   EXC_REGS_QUANT = 3  Coils or registers number beyond the available space
  544.  * @ingroup buffer
  545.  */
  546. uint8_t Modbus::getLastError()
  547. {
  548.     return u8lastError;
  549. }
  550.  
  551. /**
  552.  * @brief
  553.  * *** Only Modbus Master ***
  554.  * Generate a query to an slave with a modbus_t telegram structure
  555.  * The Master must be in COM_IDLE mode. After it, its state would be COM_WAITING.
  556.  * This method has to be called only in loop() section.
  557.  *
  558.  * @see modbus_t
  559.  * @param modbus_t  modbus telegram structure (id, fct, ...)
  560.  * @ingroup loop
  561.  * @todo finish function 15
  562.  */
  563. int8_t Modbus::query( modbus_t telegram )
  564. {
  565.     uint8_t u8regsno, u8bytesno;
  566.     if (u8id!=0) return -2;
  567.     if (u8state != COM_IDLE) return -1;
  568.  
  569.     if ((telegram.u8id==0) || (telegram.u8id>247)) return -3;
  570.  
  571.     au16regs = telegram.au16reg;
  572.  
  573.     // telegram header
  574.     au8Buffer[ ID ]         = telegram.u8id;
  575.     au8Buffer[ FUNC ]       = telegram.u8fct;
  576.     au8Buffer[ ADD_HI ]     = highByte(telegram.u16RegAdd );
  577.     au8Buffer[ ADD_LO ]     = lowByte( telegram.u16RegAdd );
  578.  
  579.     switch( telegram.u8fct )
  580.     {
  581.     case MB_FC_READ_COILS:
  582.     case MB_FC_READ_DISCRETE_INPUT:
  583.     case MB_FC_READ_REGISTERS:
  584.     case MB_FC_READ_INPUT_REGISTER:
  585.         au8Buffer[ NB_HI ]      = highByte(telegram.u16CoilsNo );
  586.         au8Buffer[ NB_LO ]      = lowByte( telegram.u16CoilsNo );
  587.         u8BufferSize = 6;
  588.         break;
  589.     case MB_FC_WRITE_COIL:
  590.         au8Buffer[ NB_HI ]      = ((au16regs[0] > 0) ? 0xff : 0);
  591.         au8Buffer[ NB_LO ]      = 0;
  592.         u8BufferSize = 6;
  593.         break;
  594.     case MB_FC_WRITE_REGISTER:
  595.         au8Buffer[ NB_HI ]      = highByte(au16regs[0]);
  596.         au8Buffer[ NB_LO ]      = lowByte(au16regs[0]);
  597.         u8BufferSize = 6;
  598.         break;
  599.     case MB_FC_WRITE_MULTIPLE_COILS: // TODO: implement "sending coils"
  600.         u8regsno = telegram.u16CoilsNo / 16;
  601.         u8bytesno = u8regsno * 2;
  602.         if ((telegram.u16CoilsNo % 16) != 0)
  603.         {
  604.             u8bytesno++;
  605.             u8regsno++;
  606.         }
  607.  
  608.         au8Buffer[ NB_HI ]      = highByte(telegram.u16CoilsNo );
  609.         au8Buffer[ NB_LO ]      = lowByte( telegram.u16CoilsNo );
  610.         au8Buffer[ NB_LO+1 ]    = u8bytesno;
  611.         u8BufferSize = 7;
  612.  
  613.         u8regsno = u8bytesno = 0; // now auxiliary registers
  614.         for (uint16_t i = 0; i < telegram.u16CoilsNo; i++)
  615.         {
  616.  
  617.  
  618.         }
  619.         break;
  620.  
  621.     case MB_FC_WRITE_MULTIPLE_REGISTERS:
  622.         au8Buffer[ NB_HI ]      = highByte(telegram.u16CoilsNo );
  623.         au8Buffer[ NB_LO ]      = lowByte( telegram.u16CoilsNo );
  624.         au8Buffer[ NB_LO+1 ]    = (uint8_t) ( telegram.u16CoilsNo * 2 );
  625.         u8BufferSize = 7;
  626.  
  627.         for (uint16_t i=0; i< telegram.u16CoilsNo; i++)
  628.         {
  629.             au8Buffer[ u8BufferSize ] = highByte( au16regs[ i ] );
  630.             u8BufferSize++;
  631.             au8Buffer[ u8BufferSize ] = lowByte( au16regs[ i ] );
  632.             u8BufferSize++;
  633.         }
  634.         break;
  635.     }
  636.  
  637.     sendTxBuffer();
  638.     u8state = COM_WAITING;
  639.     return 0;
  640. }
  641.  
  642. /**
  643.  * @brief *** Only for Modbus Master ***
  644.  * This method checks if there is any incoming answer if pending.
  645.  * If there is no answer, it would change Master state to COM_IDLE.
  646.  * This method must be called only at loop section.
  647.  * Avoid any delay() function.
  648.  *
  649.  * Any incoming data would be redirected to au16regs pointer,
  650.  * as defined in its modbus_t query telegram.
  651.  *
  652.  * @params  nothing
  653.  * @return errors counter
  654.  * @ingroup loop
  655.  */
  656. int8_t Modbus::poll()
  657. {
  658.     // check if there is any incoming frame
  659.     uint8_t u8current;
  660.     if(u8serno<4)
  661.         u8current = port->available();
  662.     else
  663.         u8current = softPort->available();
  664.  
  665.     if (millis() - u32timeOut > (unsigned long) u16timeOut)
  666.     {
  667.         u8state = COM_IDLE;
  668.         u8lastError = NO_REPLY;
  669.         u16errCnt++;
  670.         return 0;
  671.     }
  672.  
  673.     if (u8current == 0) return 0;
  674.  
  675.     // check T35 after frame end or still no frame end
  676.     if (u8current != u8lastRec)
  677.     {
  678.         u8lastRec = u8current;
  679.         u32time = millis();
  680.         return 0;
  681.     }
  682.     if (millis() - u32time < T35) return 0;
  683.  
  684.     // transfer Serial buffer frame to auBuffer
  685.     u8lastRec = 0;
  686.     int8_t i8state = getRxBuffer();
  687.     if (i8state < 7)
  688.     {
  689.         u8state = COM_IDLE;
  690.         u16errCnt++;
  691.         return i8state;
  692.     }
  693.  
  694.     // validate message: id, CRC, FCT, exception
  695.     uint8_t u8exception = validateAnswer();
  696.     if (u8exception != 0)
  697.     {
  698.         u8state = COM_IDLE;
  699.         return u8exception;
  700.     }
  701.  
  702.     // process answer
  703.     switch( au8Buffer[ FUNC ] )
  704.     {
  705.     case MB_FC_READ_COILS:
  706.     case MB_FC_READ_DISCRETE_INPUT:
  707.         // call get_FC1 to transfer the incoming message to au16regs buffer
  708.         get_FC1( );
  709.         break;
  710.     case MB_FC_READ_INPUT_REGISTER:
  711.     case MB_FC_READ_REGISTERS :
  712.         // call get_FC3 to transfer the incoming message to au16regs buffer
  713.         get_FC3( );
  714.         break;
  715.     case MB_FC_WRITE_COIL:
  716.     case MB_FC_WRITE_REGISTER :
  717.     case MB_FC_WRITE_MULTIPLE_COILS:
  718.     case MB_FC_WRITE_MULTIPLE_REGISTERS :
  719.         // nothing to do
  720.         break;
  721.     default:
  722.         break;
  723.     }
  724.     u8state = COM_IDLE;
  725.     return u8BufferSize;
  726. }
  727.  
  728. /**
  729.  * @brief
  730.  * *** Only for Modbus Slave ***
  731.  * This method checks if there is any incoming query
  732.  * Afterwards, it would shoot a validation routine plus a register query
  733.  * Avoid any delay() function !!!!
  734.  * After a successful frame between the Master and the Slave, the time-out timer is reset.
  735.  *
  736.  * @param *regs  register table for communication exchange
  737.  * @param u8size  size of the register table
  738.  * @return 0 if no query, 1..4 if communication error, >4 if correct query processed
  739.  * @ingroup loop
  740.  */
  741. int8_t Modbus::poll( uint16_t *regs, uint8_t u8size )
  742. {
  743.  
  744.     au16regs = regs;
  745.     u8regsize = u8size;
  746.     uint8_t u8current;
  747.  
  748.  
  749.     // check if there is any incoming frame
  750.     if(u8serno<4)
  751.         u8current = port->available();
  752.     else
  753.         u8current = softPort->available();
  754.  
  755.     if (u8current == 0) return 0;
  756.  
  757.     // check T35 after frame end or still no frame end
  758.     if (u8current != u8lastRec)
  759.     {
  760.         u8lastRec = u8current;
  761.         u32time = millis();
  762.         return 0;
  763.     }
  764.     if (millis() - u32time < T35) return 0;
  765.  
  766.     u8lastRec = 0;
  767.     int8_t i8state = getRxBuffer();
  768.     u8lastError = i8state;
  769.     if (i8state < 7) return i8state;
  770.  
  771.     // check slave id
  772.     if (au8Buffer[ ID ] != u8id) return 0;
  773.  
  774.     // validate message: CRC, FCT, address and size
  775.     uint8_t u8exception = validateRequest();
  776.     if (u8exception > 0)
  777.     {
  778.         if (u8exception != NO_REPLY)
  779.         {
  780.             buildException( u8exception );
  781.             sendTxBuffer();
  782.         }
  783.         u8lastError = u8exception;
  784.         return u8exception;
  785.     }
  786.  
  787.     u32timeOut = millis();
  788.     u8lastError = 0;
  789.  
  790.     // process message
  791.     switch( au8Buffer[ FUNC ] )
  792.     {
  793.     case MB_FC_READ_COILS:
  794.     case MB_FC_READ_DISCRETE_INPUT:
  795.         return process_FC1( regs, u8size );
  796.         break;
  797.     case MB_FC_READ_INPUT_REGISTER:
  798.     case MB_FC_READ_REGISTERS :
  799.         return process_FC3( regs, u8size );
  800.         break;
  801.     case MB_FC_WRITE_COIL:
  802.         return process_FC5( regs, u8size );
  803.         break;
  804.     case MB_FC_WRITE_REGISTER :
  805.         return process_FC6( regs, u8size );
  806.         break;
  807.     case MB_FC_WRITE_MULTIPLE_COILS:
  808.         return process_FC15( regs, u8size );
  809.         break;
  810.     case MB_FC_WRITE_MULTIPLE_REGISTERS :
  811.         return process_FC16( regs, u8size );
  812.         break;
  813.     default:
  814.         break;
  815.     }
  816.     return i8state;
  817. }
  818.  
  819. /* _____PRIVATE FUNCTIONS_____________________________________________________ */
  820.  
  821. void Modbus::init(uint8_t u8id, uint8_t u8serno, uint8_t u8txenpin)
  822. {
  823.     this->u8id = u8id;
  824.     this->u8serno = (u8serno > 3) ? 0 : u8serno;
  825.     this->u8txenpin = u8txenpin;
  826.     this->u16timeOut = 1000;
  827. }
  828.  
  829. void Modbus::init(uint8_t u8id)
  830. {
  831.     this->u8id = u8id;
  832.     this->u8serno = 4;
  833.     this->u8txenpin = 0;
  834.     this->u16timeOut = 1000;
  835. }
  836.  
  837. /**
  838.  * @brief
  839.  * This method moves Serial buffer data to the Modbus au8Buffer.
  840.  *
  841.  * @return buffer size if OK, ERR_BUFF_OVERFLOW if u8BufferSize >= MAX_BUFFER
  842.  * @ingroup buffer
  843.  */
  844. int8_t Modbus::getRxBuffer()
  845. {
  846.     boolean bBuffOverflow = false;
  847.  
  848.     if (u8txenpin > 1) digitalWrite( u8txenpin, LOW );
  849.  
  850.     u8BufferSize = 0;
  851.     if(u8serno<4)
  852.         while ( port->available() )
  853.         {
  854.             au8Buffer[ u8BufferSize ] = port->read();
  855.             u8BufferSize ++;
  856.  
  857.             if (u8BufferSize >= MAX_BUFFER) bBuffOverflow = true;
  858.         }
  859.     else
  860.         while ( softPort->available() )
  861.         {
  862.             au8Buffer[ u8BufferSize ] = softPort->read();
  863.             u8BufferSize ++;
  864.  
  865.             if (u8BufferSize >= MAX_BUFFER) bBuffOverflow = true;
  866.         }
  867.     u16InCnt++;
  868.  
  869.     if (bBuffOverflow)
  870.     {
  871.         u16errCnt++;
  872.         return ERR_BUFF_OVERFLOW;
  873.     }
  874.     return u8BufferSize;
  875. }
  876.  
  877. /**
  878.  * @brief
  879.  * This method transmits au8Buffer to Serial line.
  880.  * Only if u8txenpin != 0, there is a flow handling in order to keep
  881.  * the RS485 transceiver in output state as long as the message is being sent.
  882.  * This is done with UCSRxA register.
  883.  * The CRC is appended to the buffer before starting to send it.
  884.  *
  885.  * @param nothing
  886.  * @return nothing
  887.  * @ingroup buffer
  888.  */
  889. void Modbus::sendTxBuffer()
  890. {
  891.     uint8_t i = 0;
  892.  
  893.     // append CRC to message
  894.     uint16_t u16crc = calcCRC( u8BufferSize );
  895.     au8Buffer[ u8BufferSize ] = u16crc >> 8;
  896.     u8BufferSize++;
  897.     au8Buffer[ u8BufferSize ] = u16crc & 0x00ff;
  898.     u8BufferSize++;
  899.  
  900.     // set RS485 transceiver to transmit mode
  901.     if (u8txenpin > 1)
  902.     {
  903.         switch( u8serno )
  904.         {
  905. #if defined(UBRR1H)
  906.         case 1:
  907.             UCSR1A=UCSR1A |(1 << TXC1);
  908.             break;
  909. #endif
  910.  
  911. #if defined(UBRR2H)
  912.         case 2:
  913.             UCSR2A=UCSR2A |(1 << TXC2);
  914.             break;
  915. #endif
  916.  
  917. #if defined(UBRR3H)
  918.         case 3:
  919.             UCSR3A=UCSR3A |(1 << TXC3);
  920.             break;
  921. #endif
  922.         case 0:
  923.         default:
  924.             UCSR0A=UCSR0A |(1 << TXC0);
  925.             break;
  926.         }
  927.         digitalWrite( u8txenpin, HIGH );
  928.     }
  929.  
  930.     // transfer buffer to serial line
  931.     if(u8serno<4)
  932.         port->write( au8Buffer, u8BufferSize );
  933.     else
  934.         softPort->write( au8Buffer, u8BufferSize );
  935.  
  936.     // keep RS485 transceiver in transmit mode as long as sending
  937.     if (u8txenpin > 1)
  938.     {
  939.         switch( u8serno )
  940.         {
  941. #if defined(UBRR1H)
  942.         case 1:
  943.             while (!(UCSR1A & (1 << TXC1)));
  944.             break;
  945. #endif
  946.  
  947. #if defined(UBRR2H)
  948.         case 2:
  949.             while (!(UCSR2A & (1 << TXC2)));
  950.             break;
  951. #endif
  952.  
  953. #if defined(UBRR3H)
  954.         case 3:
  955.             while (!(UCSR3A & (1 << TXC3)));
  956.             break;
  957. #endif
  958.         case 0:
  959.         default:
  960.             while (!(UCSR0A & (1 << TXC0)));
  961.             break;
  962.         }
  963.  
  964.         // return RS485 transceiver to receive mode
  965.         digitalWrite( u8txenpin, LOW );
  966.     }
  967.     if(u8serno<4)
  968.         while(port->read() >= 0);
  969.     else
  970.         while(softPort->read() >= 0);
  971.  
  972.     u8BufferSize = 0;
  973.  
  974.     // set time-out for master
  975.     u32timeOut = millis();
  976.  
  977.     // increase message counter
  978.     u16OutCnt++;
  979. }
  980.  
  981. /**
  982.  * @brief
  983.  * This method calculates CRC
  984.  *
  985.  * @return uint16_t calculated CRC value for the message
  986.  * @ingroup buffer
  987.  */
  988. uint16_t Modbus::calcCRC(uint8_t u8length)
  989. {
  990.     unsigned int temp, temp2, flag;
  991.     temp = 0xFFFF;
  992.     for (unsigned char i = 0; i < u8length; i++)
  993.     {
  994.         temp = temp ^ au8Buffer[i];
  995.         for (unsigned char j = 1; j <= 8; j++)
  996.         {
  997.             flag = temp & 0x0001;
  998.             temp >>=1;
  999.             if (flag)
  1000.                 temp ^= 0xA001;
  1001.         }
  1002.     }
  1003.     // Reverse byte order.
  1004.     temp2 = temp >> 8;
  1005.     temp = (temp << 8) | temp2;
  1006.     temp &= 0xFFFF;
  1007.     // the returned value is already swapped
  1008.     // crcLo byte is first & crcHi byte is last
  1009.     return temp;
  1010. }
  1011.  
  1012. /**
  1013.  * @brief
  1014.  * This method validates slave incoming messages
  1015.  *
  1016.  * @return 0 if OK, EXCEPTION if anything fails
  1017.  * @ingroup buffer
  1018.  */
  1019. uint8_t Modbus::validateRequest()
  1020. {
  1021.     // check message crc vs calculated crc
  1022.     uint16_t u16MsgCRC =
  1023.         ((au8Buffer[u8BufferSize - 2] << 8)
  1024.          | au8Buffer[u8BufferSize - 1]); // combine the crc Low & High bytes
  1025.     if ( calcCRC( u8BufferSize-2 ) != u16MsgCRC )
  1026.     {
  1027.         u16errCnt ++;
  1028.         return NO_REPLY;
  1029.     }
  1030.  
  1031.     // check fct code
  1032.     boolean isSupported = false;
  1033.     for (uint8_t i = 0; i< sizeof( fctsupported ); i++)
  1034.     {
  1035.         if (fctsupported[i] == au8Buffer[FUNC])
  1036.         {
  1037.             isSupported = 1;
  1038.             break;
  1039.         }
  1040.     }
  1041.     if (!isSupported)
  1042.     {
  1043.         u16errCnt ++;
  1044.         return EXC_FUNC_CODE;
  1045.     }
  1046.  
  1047.     // check start address & nb range
  1048.     uint16_t u16regs = 0;
  1049.     uint8_t u8regs;
  1050.     switch ( au8Buffer[ FUNC ] )
  1051.     {
  1052.     case MB_FC_READ_COILS:
  1053.     case MB_FC_READ_DISCRETE_INPUT:
  1054.     case MB_FC_WRITE_MULTIPLE_COILS:
  1055.         u16regs = word( au8Buffer[ ADD_HI ], au8Buffer[ ADD_LO ]) / 16;
  1056.         u16regs += word( au8Buffer[ NB_HI ], au8Buffer[ NB_LO ]) /16;
  1057.         u8regs = (uint8_t) u16regs;
  1058.         if (u8regs > u8regsize) return EXC_ADDR_RANGE;
  1059.         break;
  1060.     case MB_FC_WRITE_COIL:
  1061.         u16regs = word( au8Buffer[ ADD_HI ], au8Buffer[ ADD_LO ]) / 16;
  1062.         u8regs = (uint8_t) u16regs;
  1063.         if (u8regs > u8regsize) return EXC_ADDR_RANGE;
  1064.         break;
  1065.     case MB_FC_WRITE_REGISTER :
  1066.         u16regs = word( au8Buffer[ ADD_HI ], au8Buffer[ ADD_LO ]);
  1067.         u8regs = (uint8_t) u16regs;
  1068.         if (u8regs > u8regsize) return EXC_ADDR_RANGE;
  1069.         break;
  1070.     case MB_FC_READ_REGISTERS :
  1071.     case MB_FC_READ_INPUT_REGISTER :
  1072.     case MB_FC_WRITE_MULTIPLE_REGISTERS :
  1073.         u16regs = word( au8Buffer[ ADD_HI ], au8Buffer[ ADD_LO ]);
  1074.         u16regs += word( au8Buffer[ NB_HI ], au8Buffer[ NB_LO ]);
  1075.         u8regs = (uint8_t) u16regs;
  1076.         if (u8regs > u8regsize) return EXC_ADDR_RANGE;
  1077.         break;
  1078.     }
  1079.     return 0; // OK, no exception code thrown
  1080. }
  1081.  
  1082. /**
  1083.  * @brief
  1084.  * This method validates master incoming messages
  1085.  *
  1086.  * @return 0 if OK, EXCEPTION if anything fails
  1087.  * @ingroup buffer
  1088.  */
  1089. uint8_t Modbus::validateAnswer()
  1090. {
  1091.     // check message crc vs calculated crc
  1092.     uint16_t u16MsgCRC =
  1093.         ((au8Buffer[u8BufferSize - 2] << 8)
  1094.          | au8Buffer[u8BufferSize - 1]); // combine the crc Low & High bytes
  1095.     if ( calcCRC( u8BufferSize-2 ) != u16MsgCRC )
  1096.     {
  1097.         u16errCnt ++;
  1098.         return NO_REPLY;
  1099.     }
  1100.  
  1101.     // check exception
  1102.     if ((au8Buffer[ FUNC ] & 0x80) != 0)
  1103.     {
  1104.         u16errCnt ++;
  1105.         return ERR_EXCEPTION;
  1106.     }
  1107.  
  1108.     // check fct code
  1109.     boolean isSupported = false;
  1110.     for (uint8_t i = 0; i< sizeof( fctsupported ); i++)
  1111.     {
  1112.         if (fctsupported[i] == au8Buffer[FUNC])
  1113.         {
  1114.             isSupported = 1;
  1115.             break;
  1116.         }
  1117.     }
  1118.     if (!isSupported)
  1119.     {
  1120.         u16errCnt ++;
  1121.         return EXC_FUNC_CODE;
  1122.     }
  1123.  
  1124.     return 0; // OK, no exception code thrown
  1125. }
  1126.  
  1127. /**
  1128.  * @brief
  1129.  * This method builds an exception message
  1130.  *
  1131.  * @ingroup buffer
  1132.  */
  1133. void Modbus::buildException( uint8_t u8exception )
  1134. {
  1135.     uint8_t u8func = au8Buffer[ FUNC ];  // get the original FUNC code
  1136.  
  1137.     au8Buffer[ ID ]      = u8id;
  1138.     au8Buffer[ FUNC ]    = u8func + 0x80;
  1139.     au8Buffer[ 2 ]       = u8exception;
  1140.     u8BufferSize         = EXCEPTION_SIZE;
  1141. }
  1142.  
  1143. /**
  1144.  * This method processes functions 1 & 2 (for master)
  1145.  * This method puts the slave answer into master data buffer
  1146.  *
  1147.  * @ingroup register
  1148.  * TODO: finish its implementation
  1149.  */
  1150. void Modbus::get_FC1()
  1151. {
  1152.     uint8_t u8byte, i;
  1153.     u8byte = 0;
  1154.  
  1155.     //  for (i=0; i< au8Buffer[ 2 ] /2; i++) {
  1156.     //    au16regs[ i ] = word(
  1157.     //    au8Buffer[ u8byte ],
  1158.     //    au8Buffer[ u8byte +1 ]);
  1159.     //    u8byte += 2;
  1160.     //  }
  1161. }
  1162.  
  1163. /**
  1164.  * This method processes functions 3 & 4 (for master)
  1165.  * This method puts the slave answer into master data buffer
  1166.  *
  1167.  * @ingroup register
  1168.  */
  1169. void Modbus::get_FC3()
  1170. {
  1171.     uint8_t u8byte, i;
  1172.     u8byte = 3;
  1173.  
  1174.     for (i=0; i< au8Buffer[ 2 ] /2; i++)
  1175.     {
  1176.         au16regs[ i ] = word(
  1177.                             au8Buffer[ u8byte ],
  1178.                             au8Buffer[ u8byte +1 ]);
  1179.         u8byte += 2;
  1180.     }
  1181. }
  1182.  
  1183. /**
  1184.  * @brief
  1185.  * This method processes functions 1 & 2
  1186.  * This method reads a bit array and transfers it to the master
  1187.  *
  1188.  * @return u8BufferSize Response to master length
  1189.  * @ingroup discrete
  1190.  */
  1191. int8_t Modbus::process_FC1( uint16_t *regs, uint8_t u8size )
  1192. {
  1193.     uint8_t u8currentRegister, u8currentBit, u8bytesno, u8bitsno;
  1194.     uint8_t u8CopyBufferSize;
  1195.     uint16_t u16currentCoil, u16coil;
  1196.  
  1197.     // get the first and last coil from the message
  1198.     uint16_t u16StartCoil = word( au8Buffer[ ADD_HI ], au8Buffer[ ADD_LO ] );
  1199.     uint16_t u16Coilno = word( au8Buffer[ NB_HI ], au8Buffer[ NB_LO ] );
  1200.  
  1201.     // put the number of bytes in the outcoming message
  1202.     u8bytesno = (uint8_t) (u16Coilno / 8);
  1203.     if (u16Coilno % 8 != 0) u8bytesno ++;
  1204.     au8Buffer[ ADD_HI ]  = u8bytesno;
  1205.     u8BufferSize         = ADD_LO;
  1206.  
  1207.     // read each coil from the register map and put its value inside the outcoming message
  1208.     u8bitsno = 0;
  1209.  
  1210.     for (u16currentCoil = 0; u16currentCoil < u16Coilno; u16currentCoil++)
  1211.     {
  1212.         u16coil = u16StartCoil + u16currentCoil;
  1213.         u8currentRegister = (uint8_t) (u16coil / 16);
  1214.         u8currentBit = (uint8_t) (u16coil % 16);
  1215.  
  1216.         bitWrite(
  1217.             au8Buffer[ u8BufferSize ],
  1218.             u8bitsno,
  1219.             bitRead( regs[ u8currentRegister ], u8currentBit ) );
  1220.         u8bitsno ++;
  1221.  
  1222.         if (u8bitsno > 7)
  1223.         {
  1224.             u8bitsno = 0;
  1225.             u8BufferSize++;
  1226.         }
  1227.     }
  1228.  
  1229.     // send outcoming message
  1230.     if (u16Coilno % 8 != 0) u8BufferSize ++;
  1231.     u8CopyBufferSize = u8BufferSize +2;
  1232.     sendTxBuffer();
  1233.     return u8CopyBufferSize;
  1234. }
  1235.  
  1236. /**
  1237.  * @brief
  1238.  * This method processes functions 3 & 4
  1239.  * This method reads a word array and transfers it to the master
  1240.  *
  1241.  * @return u8BufferSize Response to master length
  1242.  * @ingroup register
  1243.  */
  1244. int8_t Modbus::process_FC3( uint16_t *regs, uint8_t u8size )
  1245. {
  1246.  
  1247.     uint8_t u8StartAdd = word( au8Buffer[ ADD_HI ], au8Buffer[ ADD_LO ] );
  1248.     uint8_t u8regsno = word( au8Buffer[ NB_HI ], au8Buffer[ NB_LO ] );
  1249.     uint8_t u8CopyBufferSize;
  1250.     uint8_t i;
  1251.  
  1252.     au8Buffer[ 2 ]       = u8regsno * 2;
  1253.     u8BufferSize         = 3;
  1254.  
  1255.     for (i = u8StartAdd; i < u8StartAdd + u8regsno; i++)
  1256.     {
  1257.         au8Buffer[ u8BufferSize ] = highByte(regs[i]);
  1258.         u8BufferSize++;
  1259.         au8Buffer[ u8BufferSize ] = lowByte(regs[i]);
  1260.         u8BufferSize++;
  1261.     }
  1262.     u8CopyBufferSize = u8BufferSize +2;
  1263.     sendTxBuffer();
  1264.  
  1265.     return u8CopyBufferSize;
  1266. }
  1267.  
  1268. /**
  1269.  * @brief
  1270.  * This method processes function 5
  1271.  * This method writes a value assigned by the master to a single bit
  1272.  *
  1273.  * @return u8BufferSize Response to master length
  1274.  * @ingroup discrete
  1275.  */
  1276. int8_t Modbus::process_FC5( uint16_t *regs, uint8_t u8size )
  1277. {
  1278.     uint8_t u8currentRegister, u8currentBit;
  1279.     uint8_t u8CopyBufferSize;
  1280.     uint16_t u16coil = word( au8Buffer[ ADD_HI ], au8Buffer[ ADD_LO ] );
  1281.  
  1282.     // point to the register and its bit
  1283.     u8currentRegister = (uint8_t) (u16coil / 16);
  1284.     u8currentBit = (uint8_t) (u16coil % 16);
  1285.  
  1286.     // write to coil
  1287.     bitWrite(
  1288.         regs[ u8currentRegister ],
  1289.         u8currentBit,
  1290.         au8Buffer[ NB_HI ] == 0xff );
  1291.  
  1292.  
  1293.     // send answer to master
  1294.     u8BufferSize = 6;
  1295.     u8CopyBufferSize = u8BufferSize +2;
  1296.     sendTxBuffer();
  1297.  
  1298.     return u8CopyBufferSize;
  1299. }
  1300.  
  1301. /**
  1302.  * @brief
  1303.  * This method processes function 6
  1304.  * This method writes a value assigned by the master to a single word
  1305.  *
  1306.  * @return u8BufferSize Response to master length
  1307.  * @ingroup register
  1308.  */
  1309. int8_t Modbus::process_FC6( uint16_t *regs, uint8_t u8size )
  1310. {
  1311.  
  1312.     uint8_t u8add = word( au8Buffer[ ADD_HI ], au8Buffer[ ADD_LO ] );
  1313.     uint8_t u8CopyBufferSize;
  1314.     uint16_t u16val = word( au8Buffer[ NB_HI ], au8Buffer[ NB_LO ] );
  1315.  
  1316.     regs[ u8add ] = u16val;
  1317.  
  1318.     // keep the same header
  1319.     u8BufferSize         = RESPONSE_SIZE;
  1320.  
  1321.     u8CopyBufferSize = u8BufferSize +2;
  1322.     sendTxBuffer();
  1323.  
  1324.     return u8CopyBufferSize;
  1325. }
  1326.  
  1327. /**
  1328.  * @brief
  1329.  * This method processes function 15
  1330.  * This method writes a bit array assigned by the master
  1331.  *
  1332.  * @return u8BufferSize Response to master length
  1333.  * @ingroup discrete
  1334.  */
  1335. int8_t Modbus::process_FC15( uint16_t *regs, uint8_t u8size )
  1336. {
  1337.     uint8_t u8currentRegister, u8currentBit, u8frameByte, u8bitsno;
  1338.     uint8_t u8CopyBufferSize;
  1339.     uint16_t u16currentCoil, u16coil;
  1340.     boolean bTemp;
  1341.  
  1342.     // get the first and last coil from the message
  1343.     uint16_t u16StartCoil = word( au8Buffer[ ADD_HI ], au8Buffer[ ADD_LO ] );
  1344.     uint16_t u16Coilno = word( au8Buffer[ NB_HI ], au8Buffer[ NB_LO ] );
  1345.  
  1346.  
  1347.     // read each coil from the register map and put its value inside the outcoming message
  1348.     u8bitsno = 0;
  1349.     u8frameByte = 7;
  1350.     for (u16currentCoil = 0; u16currentCoil < u16Coilno; u16currentCoil++)
  1351.     {
  1352.  
  1353.         u16coil = u16StartCoil + u16currentCoil;
  1354.         u8currentRegister = (uint8_t) (u16coil / 16);
  1355.         u8currentBit = (uint8_t) (u16coil % 16);
  1356.  
  1357.         bTemp = bitRead(
  1358.                     au8Buffer[ u8frameByte ],
  1359.                     u8bitsno );
  1360.  
  1361.         bitWrite(
  1362.             regs[ u8currentRegister ],
  1363.             u8currentBit,
  1364.             bTemp );
  1365.  
  1366.         u8bitsno ++;
  1367.  
  1368.         if (u8bitsno > 7)
  1369.         {
  1370.             u8bitsno = 0;
  1371.             u8frameByte++;
  1372.         }
  1373.     }
  1374.  
  1375.     // send outcoming message
  1376.     // it's just a copy of the incomping frame until 6th byte
  1377.     u8BufferSize         = 6;
  1378.     u8CopyBufferSize = u8BufferSize +2;
  1379.     sendTxBuffer();
  1380.     return u8CopyBufferSize;
  1381. }
  1382.  
  1383. /**
  1384.  * @brief
  1385.  * This method processes function 16
  1386.  * This method writes a word array assigned by the master
  1387.  *
  1388.  * @return u8BufferSize Response to master length
  1389.  * @ingroup register
  1390.  */
  1391. int8_t Modbus::process_FC16( uint16_t *regs, uint8_t u8size )
  1392. {
  1393.     uint8_t u8func = au8Buffer[ FUNC ];  // get the original FUNC code
  1394.     uint8_t u8StartAdd = au8Buffer[ ADD_HI ] << 8 | au8Buffer[ ADD_LO ];
  1395.     uint8_t u8regsno = au8Buffer[ NB_HI ] << 8 | au8Buffer[ NB_LO ];
  1396.     uint8_t u8CopyBufferSize;
  1397.     uint8_t i;
  1398.     uint16_t temp;
  1399.  
  1400.     // build header
  1401.     au8Buffer[ NB_HI ]   = 0;
  1402.     au8Buffer[ NB_LO ]   = u8regsno;
  1403.     u8BufferSize         = RESPONSE_SIZE;
  1404.  
  1405.     // write registers
  1406.     for (i = 0; i < u8regsno; i++)
  1407.     {
  1408.         temp = word(
  1409.                    au8Buffer[ (BYTE_CNT + 1) + i * 2 ],
  1410.                    au8Buffer[ (BYTE_CNT + 2) + i * 2 ]);
  1411.  
  1412.         regs[ u8StartAdd + i ] = temp;
  1413.     }
  1414.     u8CopyBufferSize = u8BufferSize +2;
  1415.     sendTxBuffer();
  1416.  
  1417.     return u8CopyBufferSize;
  1418. }
Advertisement
Add Comment
Please, Sign In to add comment
Public Pastes
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!