Первый

Modbus.cpp

@@ -0,0 +1,678 @@
+/*
+    Modbus.cpp - Modbus Core Library Implementation
+    Copyright (C) 2014 Andr<64> Sarmento Barbosa
+                  2017-2020 Alexander Emelianov (a.m.emelianov@gmail.com)
+*/
+#include "Modbus.h"
+#include "word.h"
+
+#ifdef MB_GLOBAL_REGS
+std::vector<TRegister> _regs;
+std::vector<TCallback> _callbacks;
+#endif
+
+uint16_t Modbus::mbCallback(TRegister* reg, uint16_t val, TCallback::CallbackType t)
+{
+    uint16_t newVal = val;
+    std::vector<TCallback>::iterator it = _callbacks.begin();
+    do {
+        it = std::find_if(it, _callbacks.end(), [reg, t](TCallback& cb) { return cb.address == reg->address && cb.type == t; });
+        if (it != _callbacks.end()) {
+            newVal = it->cb(reg, newVal);
+            it++;
+        }
+    } while (it != _callbacks.end());
+    return newVal;
+}
+
+TRegister* Modbus::searchRegister(TAddress address)
+{
+    std::vector<TRegister>::iterator it = std::find_if(_regs.begin(), _regs.end(), [address](TRegister& addr) { return addr.address == address; });
+    if (it != _regs.end())
+        return &*it;
+    return nullptr;
+}
+
+bool Modbus::addReg(TAddress address, uint16_t value, uint16_t numregs)
+{
+#ifdef MB_MAX_REGS
+    if (_regs.size() + numregs > MB_MAX_REGS)
+        return false;
+#endif
+    for (uint16_t i = 0; i < numregs; i++) {
+        if (!searchRegister(address + i))
+            _regs.push_back({ address + i, value });
+    }
+    //std::sort(_regs.begin(), _regs.end());
+    return true;
+}
+
+bool Modbus::Reg(TAddress address, uint16_t value)
+{
+    TRegister* reg;
+    reg = searchRegister(address); //search for the register address
+    if (reg) { //if found then assign the register value to the new value.
+        if (cbEnabled) {
+            reg->value = mbCallback(reg, value, TCallback::ON_SET);
+        } else {
+            reg->value = value;
+        }
+        return true;
+    } else
+        return false;
+}
+
+uint16_t Modbus::Reg(TAddress address)
+{
+    TRegister* reg;
+    reg = searchRegister(address);
+    if (reg)
+        if (cbEnabled) {
+            return mbCallback(reg, reg->value, TCallback::ON_GET);
+        } else {
+            return reg->value;
+        }
+    else
+        return 0;
+}
+
+bool Modbus::removeReg(TAddress address, uint16_t numregs)
+{
+    TRegister* reg;
+    bool atLeastOne = false;
+    for (uint16_t i = 0; i < numregs; i++) {
+        reg = searchRegister(address + i);
+        if (reg) {
+            atLeastOne = true;
+            removeOnSet(address + i);
+            removeOnGet(address + i);
+            _regs.erase(std::remove(_regs.begin(), _regs.end(), *reg), _regs.end());
+        }
+    }
+    return atLeastOne;
+}
+
+void Modbus::slavePDU(uint8_t* frame)
+{
+    FunctionCode fcode = (FunctionCode)frame[0];
+    uint16_t field1 = (uint16_t)frame[1] << 8 | (uint16_t)frame[2];
+    uint16_t field2 = (uint16_t)frame[3] << 8 | (uint16_t)frame[4];
+    uint16_t bytecount_calc;
+    uint16_t k;
+    switch (fcode) {
+    case FC_WRITE_REG:
+        //field1 = reg, field2 = value
+        if (!Hreg(field1, field2)) { //Check Address and execute (reg exists?)
+            exceptionResponse(fcode, EX_ILLEGAL_ADDRESS);
+            break;
+        }
+        if (Hreg(field1) != field2) { //Check for failure
+            exceptionResponse(fcode, EX_SLAVE_FAILURE);
+            break;
+        }
+        _reply = REPLY_ECHO;
+        break;
+
+    case FC_READ_REGS:
+        //field1 = startreg, field2 = numregs, header len = 3
+        readWords(HREG(field1), field2, fcode);
+        break;
+
+    case FC_WRITE_REGS:
+        //field1 = startreg, field2 = numregs, frame[5] = data lenght, header len = 6
+        if (field2 < 0x0001 || field2 > 0x007B || frame[5] != 2 * field2) { //Check value
+            exceptionResponse(fcode, EX_ILLEGAL_VALUE);
+            break;
+        }
+        for (k = 0; k < field2; k++) { //Check Address (startreg...startreg + numregs)
+            if (!searchRegister(HREG(field1) + k)) {
+                exceptionResponse(fcode, EX_ILLEGAL_ADDRESS);
+                break;
+            }
+        }
+        if (k >= field2) {
+            setMultipleWords((uint16_t*)(frame + 6), HREG(field1), field2);
+            successResponce(HREG(field1), field2, fcode);
+            _reply = REPLY_NORMAL;
+        }
+        break;
+
+    case FC_READ_COILS:
+        //field1 = startreg, field2 = numregs
+        readBits(COIL(field1), field2, fcode);
+        break;
+
+    case FC_READ_INPUT_STAT:
+        //field1 = startreg, field2 = numregs
+        readBits(ISTS(field1), field2, fcode);
+        break;
+
+    case FC_READ_INPUT_REGS:
+        //field1 = startreg, field2 = numregs
+        readWords(IREG(field1), field2, fcode);
+        break;
+
+    case FC_WRITE_COIL:
+        //field1 = reg, field2 = status, header len = 3
+        if (field2 != 0xFF00 && field2 != 0x0000) { //Check value (status)
+            exceptionResponse(fcode, EX_ILLEGAL_VALUE);
+            break;
+        }
+        if (!Coil(field1, COIL_BOOL(field2))) { //Check Address and execute (reg exists?)
+            exceptionResponse(fcode, EX_ILLEGAL_ADDRESS);
+            break;
+        }
+        if (Coil(field1) != COIL_BOOL(field2)) { //Check for failure
+            exceptionResponse(fcode, EX_SLAVE_FAILURE);
+            break;
+        }
+        _reply = REPLY_ECHO;
+        break;
+
+    case FC_WRITE_COILS:
+        //field1 = startreg, field2 = numregs, frame[5] = bytecount, header len = 6
+        bytecount_calc = field2 / 8;
+        if (field2 % 8)
+            bytecount_calc++;
+        if (field2 < 0x0001 || field2 > 0x07B0 || frame[5] != bytecount_calc) { //Check registers range and data size maches
+            exceptionResponse(fcode, EX_ILLEGAL_VALUE);
+            break;
+        }
+        for (k = 0; k < field2; k++) { //Check Address (startreg...startreg + numregs)
+            if (!searchRegister(COIL(field1) + k)) {
+                exceptionResponse(fcode, EX_ILLEGAL_ADDRESS);
+                break;
+            }
+        }
+        if (k >= field2) {
+            setMultipleBits(frame + 6, COIL(field1), field2);
+            successResponce(COIL(field1), field2, fcode);
+            _reply = REPLY_NORMAL;
+        }
+        break;
+
+    default:
+        exceptionResponse(fcode, EX_ILLEGAL_FUNCTION);
+    }
+}
+
+void Modbus::successResponce(TAddress startreg, uint16_t numoutputs, FunctionCode fn)
+{
+    free(_frame);
+    _len = 5;
+    _frame = (uint8_t*)malloc(_len);
+    _frame[0] = fn;
+    _frame[1] = startreg.address >> 8;
+    _frame[2] = startreg.address & 0x00FF;
+    _frame[3] = numoutputs >> 8;
+    _frame[4] = numoutputs & 0x00FF;
+}
+
+void Modbus::exceptionResponse(FunctionCode fn, ResultCode excode)
+{
+    free(_frame);
+    _len = 2;
+    _frame = (uint8_t*)malloc(_len);
+    _frame[0] = fn + 0x80;
+    _frame[1] = excode;
+    _reply = REPLY_NORMAL;
+}
+
+void Modbus::getMultipleBits(uint8_t* frame, TAddress startreg, uint16_t numregs)
+{
+    uint8_t bitn = 0;
+    uint16_t i = 0;
+    while (numregs--) {
+        if (BIT_BOOL(Reg(startreg)))
+            bitSet(frame[i], bitn);
+        else
+            bitClear(frame[i], bitn);
+        bitn++; //increment the bit index
+        if (bitn == 8) {
+            i++;
+            bitn = 0;
+        }
+        startreg++; //increment the register
+    }
+}
+
+void Modbus::getMultipleWords(uint16_t* frame, TAddress startreg, uint16_t numregs)
+{
+    for (uint8_t i = 0; i < numregs; i++) {
+        frame[i] = __bswap_16(Reg(startreg + i));
+    }
+}
+
+void Modbus::readBits(TAddress startreg, uint16_t numregs, FunctionCode fn)
+{
+    if (numregs < 0x0001 || numregs > 0x07D0) { //Check value (numregs)
+        exceptionResponse(fn, EX_ILLEGAL_VALUE);
+        return;
+    }
+    //Check Address
+    //Check only startreg. Is this correct?
+    //When I check all registers in range I got errors in ScadaBR
+    //I think that ScadaBR request more than one in the single request
+    //when you have more then one datapoint configured from same type.
+    if (!searchRegister(startreg)) {
+        exceptionResponse(fn, EX_ILLEGAL_ADDRESS);
+        return;
+    }
+    free(_frame);
+    //Determine the message length = function type, byte count and
+    //for each group of 8 registers the message length increases by 1
+    _len = 2 + numregs / 8;
+    if (numregs % 8)
+        _len++; //Add 1 to the message length for the partial byte.
+    _frame = (uint8_t*)malloc(_len);
+    if (!_frame) {
+        exceptionResponse(fn, EX_SLAVE_FAILURE);
+        return;
+    }
+    _frame[0] = fn;
+    _frame[1] = _len - 2; //byte count (_len - function code and byte count)
+    _frame[_len - 1] = 0; //Clean last probably partial byte
+    getMultipleBits(_frame + 2, startreg, numregs);
+    _reply = REPLY_NORMAL;
+}
+
+void Modbus::readWords(TAddress startreg, uint16_t numregs, FunctionCode fn)
+{
+    //Check value (numregs)
+    if (numregs < 0x0001 || numregs > 0x007D) {
+        exceptionResponse(fn, EX_ILLEGAL_VALUE);
+        return;
+    }
+    if (!searchRegister(startreg)) { //Check Address
+        exceptionResponse(fn, EX_ILLEGAL_ADDRESS);
+        return;
+    }
+    free(_frame);
+    _len = 2 + numregs * 2; //calculate the query reply message length. 2 bytes per register + 2 bytes for header
+    _frame = (uint8_t*)malloc(_len);
+    if (!_frame) {
+        exceptionResponse(fn, EX_SLAVE_FAILURE);
+        return;
+    }
+    _frame[0] = fn;
+    _frame[1] = _len - 2; //byte count
+    getMultipleWords((uint16_t*)(_frame + 2), startreg, numregs);
+    _reply = REPLY_NORMAL;
+}
+
+void Modbus::setMultipleBits(uint8_t* frame, TAddress startreg, uint16_t numoutputs)
+{
+    uint8_t bitn = 0;
+    uint16_t i = 0;
+    while (numoutputs--) {
+        Reg(startreg, BIT_VAL(bitRead(frame[i], bitn)));
+        bitn++; //increment the bit index
+        if (bitn == 8) {
+            i++;
+            bitn = 0;
+        }
+        startreg++; //increment the register
+    }
+}
+
+void Modbus::setMultipleWords(uint16_t* frame, TAddress startreg, uint16_t numregs)
+{
+    for (uint8_t i = 0; i < numregs; i++) {
+        Reg(startreg + i, __bswap_16(frame[i]));
+    }
+}
+
+bool Modbus::onGet(TAddress address, cbModbus cb, uint16_t numregs)
+{
+    TRegister* reg;
+    bool atLeastOne = false;
+    if (!cb) {
+        return removeOnGet(address);
+    }
+    while (numregs > 0) {
+        reg = searchRegister(address);
+        if (reg) {
+            _callbacks.push_back({ TCallback::ON_GET, address, cb });
+            atLeastOne = true;
+        }
+        address++;
+        numregs--;
+    }
+    return atLeastOne;
+}
+bool Modbus::onSet(TAddress address, cbModbus cb, uint16_t numregs)
+{
+    TRegister* reg;
+    bool atLeastOne = false;
+    if (!cb) {
+        return removeOnGet(address);
+    }
+    while (numregs > 0) {
+        reg = searchRegister(address);
+        if (reg) {
+            _callbacks.push_back({ TCallback::ON_SET, address, cb });
+            atLeastOne = true;
+        }
+        address++;
+        numregs--;
+    }
+    return atLeastOne;
+}
+
+bool Modbus::removeOnSet(TAddress address, cbModbus cb, uint16_t numregs)
+{
+    while (numregs--) {
+        _callbacks.erase(remove_if(_callbacks.begin(), _callbacks.end(), [address, cb](TCallback entry) { return entry.type == TCallback::ON_SET && entry.address == address && (!cb || entry.cb == cb); }), _callbacks.end());
+        address++;
+    }
+    return false;
+}
+bool Modbus::removeOnGet(TAddress address, cbModbus cb, uint16_t numregs)
+{
+    while (numregs--) {
+        _callbacks.erase(remove_if(_callbacks.begin(), _callbacks.end(), [address, cb](TCallback entry) { return entry.type == TCallback::ON_GET && entry.address == address && (!cb || entry.cb == cb); }), _callbacks.end());
+        address++;
+    }
+    return false;
+}
+
+bool Modbus::readSlave(uint16_t address, uint16_t numregs, FunctionCode fn)
+{
+    free(_frame);
+    _len = 5;
+    _frame = (uint8_t*)malloc(_len);
+    _frame[0] = fn;
+    _frame[1] = address >> 8;
+    _frame[2] = address & 0x00FF;
+    _frame[3] = numregs >> 8;
+    _frame[4] = numregs & 0x00FF;
+    return true;
+}
+
+bool Modbus::writeSlaveBits(TAddress startreg, uint16_t to, uint16_t numregs, FunctionCode fn, bool* data)
+{
+    free(_frame);
+    _len = 6 + numregs / 8;
+    if (numregs % 8)
+        _len++; //Add 1 to the message length for the partial byte.
+    _frame = (uint8_t*)malloc(_len);
+    if (_frame) {
+        _frame[0] = fn;
+        _frame[1] = to >> 8;
+        _frame[2] = to & 0x00FF;
+        _frame[3] = numregs >> 8;
+        _frame[4] = numregs & 0x00FF;
+        _frame[5] = _len - 6;
+        _frame[_len - 1] = 0; //Clean last probably partial byte
+        if (data) {
+            boolToBits(_frame + 6, data, numregs);
+        } else {
+            getMultipleBits(_frame + 6, startreg, numregs);
+        }
+        _reply = REPLY_NORMAL;
+        return true;
+    }
+    _reply = REPLY_OFF;
+    return false;
+}
+
+bool Modbus::writeSlaveWords(TAddress startreg, uint16_t to, uint16_t numregs, FunctionCode fn, uint16_t* data)
+{
+    free(_frame);
+    _len = 6 + 2 * numregs;
+    _frame = (uint8_t*)malloc(_len);
+    if (_frame) {
+        _frame[0] = fn;
+        _frame[1] = to >> 8;
+        _frame[2] = to & 0x00FF;
+        _frame[3] = numregs >> 8;
+        _frame[4] = numregs & 0x00FF;
+        _frame[5] = _len - 6;
+        if (data) {
+            uint16_t* frame = (uint16_t*)(_frame + 6);
+            for (uint8_t i = 0; i < numregs; i++) {
+                frame[i] = __bswap_16(data[i]);
+            }
+        } else {
+            getMultipleWords((uint16_t*)(_frame + 6), startreg, numregs);
+        }
+        return true;
+    }
+    _reply = REPLY_OFF;
+    return false;
+}
+
+void Modbus::boolToBits(uint8_t* dst, bool* src, uint16_t numregs)
+{
+    uint8_t bitn = 0;
+    uint16_t i = 0;
+    uint16_t j = 0;
+    while (numregs--) {
+        if (src[j])
+            bitSet(dst[i], bitn);
+        else
+            bitClear(dst[i], bitn);
+        bitn++; //increment the bit index
+        if (bitn == 8) {
+            i++;
+            bitn = 0;
+        }
+        j++; //increment the register
+    }
+}
+
+void Modbus::bitsToBool(bool* dst, uint8_t* src, uint16_t numregs)
+{
+    uint8_t bitn = 0;
+    uint16_t i = 0;
+    uint16_t j = 0;
+    while (numregs--) {
+        dst[j] = bitRead(src[i], bitn);
+        bitn++; //increment the bit index
+        if (bitn == 8) {
+            i++;
+            bitn = 0;
+        }
+        j++; //increment the register
+    }
+}
+
+void Modbus::masterPDU(uint8_t* frame, uint8_t* sourceFrame, TAddress startreg, uint16_t* output)
+{
+    uint8_t fcode = frame[0];
+    _reply = EX_SUCCESS;
+    if ((fcode & 0x80) != 0) {
+        _reply = frame[1];
+        return;
+    }
+    uint16_t field2 = (uint16_t)sourceFrame[3] << 8 | (uint16_t)sourceFrame[4];
+    uint8_t bytecount_calc;
+    switch (fcode) {
+    case FC_READ_REGS:
+    case FC_READ_INPUT_REGS:
+        //field2 = numregs, frame[1] = data lenght, header len = 2
+        if (frame[1] != 2 * field2) { //Check if data size matches
+            _reply = EX_DATA_MISMACH;
+            break;
+        }
+        if (output) {
+            frame += 2;
+            while (field2) {
+                *((uint16_t*)output) = __bswap_16(*((uint16_t*)frame));
+                frame += 2;
+                output += 2;
+                field2--;
+            }
+        } else {
+            setMultipleWords((uint16_t*)(frame + 2), startreg, field2);
+        }
+        break;
+    case FC_READ_COILS:
+    case FC_READ_INPUT_STAT:
+        //field2 = numregs, frame[1] = data length, header len = 2
+        bytecount_calc = field2 / 8;
+        if (field2 % 8)
+            bytecount_calc++;
+        if (frame[1] != bytecount_calc) { // check if data size matches
+            _reply = EX_DATA_MISMACH;
+            break;
+        }
+        if (output) {
+            bitsToBool((bool*)output, frame + 2, field2);
+        } else {
+            setMultipleBits(frame + 2, startreg, field2);
+        }
+        break;
+    case FC_WRITE_REG:
+    case FC_WRITE_REGS:
+    case FC_WRITE_COIL:
+    case FC_WRITE_COILS:
+        break;
+    default:
+        _reply = EX_GENERAL_FAILURE;
+    }
+}
+
+void Modbus::cbEnable(bool state)
+{
+    cbEnabled = state;
+}
+void Modbus::cbDisable()
+{
+    cbEnable(false);
+}
+
+bool Modbus::addHreg(uint16_t offset, uint16_t value, uint16_t numregs)
+{
+    return addReg(HREG(offset), value, numregs);
+}
+bool Modbus::Hreg(uint16_t offset, uint16_t value)
+{
+    return Reg(HREG(offset), value);
+}
+uint16_t Modbus::Hreg(uint16_t offset)
+{
+    return Reg(HREG(offset));
+}
+uint16_t Modbus::removeHreg(uint16_t offset, uint16_t numregs)
+{
+    return removeReg(HREG(offset), numregs);
+}
+bool Modbus::addCoil(uint16_t offset, bool value, uint16_t numregs)
+{
+    return addReg(COIL(offset), COIL_VAL(value), numregs);
+}
+bool Modbus::addIsts(uint16_t offset, bool value, uint16_t numregs)
+{
+    return addReg(ISTS(offset), ISTS_VAL(value), numregs);
+}
+bool Modbus::addIreg(uint16_t offset, uint16_t value, uint16_t numregs)
+{
+    return addReg(IREG(offset), value, numregs);
+}
+bool Modbus::Coil(uint16_t offset, bool value)
+{
+    return Reg(COIL(offset), COIL_VAL(value));
+}
+bool Modbus::Ists(uint16_t offset, bool value)
+{
+    return Reg(ISTS(offset), ISTS_VAL(value));
+}
+bool Modbus::Ireg(uint16_t offset, uint16_t value)
+{
+    return Reg(IREG(offset), value);
+}
+bool Modbus::Coil(uint16_t offset)
+{
+    return COIL_BOOL(Reg(COIL(offset)));
+}
+bool Modbus::Ists(uint16_t offset)
+{
+    return ISTS_BOOL(Reg(ISTS(offset)));
+}
+uint16_t Modbus::Ireg(uint16_t offset)
+{
+    return Reg(IREG(offset));
+}
+bool Modbus::removeCoil(uint16_t offset, uint16_t numregs)
+{
+    return removeReg(COIL(offset), numregs);
+}
+bool Modbus::removeIsts(uint16_t offset, uint16_t numregs)
+{
+    return removeReg(ISTS(offset), numregs);
+}
+bool Modbus::removeIreg(uint16_t offset, uint16_t numregs)
+{
+    return removeReg(IREG(offset), numregs);
+}
+bool Modbus::onGetCoil(uint16_t offset, cbModbus cb, uint16_t numregs)
+{
+    return onGet(COIL(offset), cb, numregs);
+}
+bool Modbus::onSetCoil(uint16_t offset, cbModbus cb, uint16_t numregs)
+{
+    return onSet(COIL(offset), cb, numregs);
+}
+bool Modbus::onGetHreg(uint16_t offset, cbModbus cb, uint16_t numregs)
+{
+    return onGet(HREG(offset), cb, numregs);
+}
+bool Modbus::onSetHreg(uint16_t offset, cbModbus cb, uint16_t numregs)
+{
+    return onSet(HREG(offset), cb, numregs);
+}
+bool Modbus::onGetIsts(uint16_t offset, cbModbus cb, uint16_t numregs)
+{
+    return onGet(ISTS(offset), cb, numregs);
+}
+bool Modbus::onSetIsts(uint16_t offset, cbModbus cb, uint16_t numregs)
+{
+    return onSet(ISTS(offset), cb, numregs);
+}
+bool Modbus::onGetIreg(uint16_t offset, cbModbus cb, uint16_t numregs)
+{
+    return onGet(IREG(offset), cb, numregs);
+}
+bool Modbus::onSetIreg(uint16_t offset, cbModbus cb, uint16_t numregs)
+{
+    return onSet(IREG(offset), cb, numregs);
+}
+
+bool Modbus::removeOnGetCoil(uint16_t offset, cbModbus cb, uint16_t numregs)
+{
+    return removeOnGet(COIL(offset), cb, numregs);
+}
+bool Modbus::removeOnSetCoil(uint16_t offset, cbModbus cb, uint16_t numregs)
+{
+    return removeOnSet(COIL(offset), cb, numregs);
+}
+bool Modbus::removeOnGetHreg(uint16_t offset, cbModbus cb, uint16_t numregs)
+{
+    return removeOnGet(HREG(offset), cb, numregs);
+}
+bool Modbus::removeOnSetHreg(uint16_t offset, cbModbus cb, uint16_t numregs)
+{
+    return removeOnSet(HREG(offset), cb, numregs);
+}
+bool Modbus::removeOnGetIsts(uint16_t offset, cbModbus cb, uint16_t numregs)
+{
+    return removeOnGet(ISTS(offset), cb, numregs);
+}
+bool Modbus::removeOnSetIsts(uint16_t offset, cbModbus cb, uint16_t numregs)
+{
+    return removeOnSet(ISTS(offset), cb, numregs);
+}
+bool Modbus::removeOnGetIreg(uint16_t offset, cbModbus cb, uint16_t numregs)
+{
+    return removeOnGet(IREG(offset), cb, numregs);
+}
+bool Modbus::removeOnSetIreg(uint16_t offset, cbModbus cb, uint16_t numregs)
+{
+    return removeOnSet(IREG(offset), cb, numregs);
+}
+
+Modbus::~Modbus()
+{
+    free(_frame);
+}

Modbus.h

@@ -0,0 +1,261 @@
+/*
+    Modbus.h - Header for Modbus Core Library
+    Copyright (C) 2014 Andr<64> Sarmento Barbosa
+                  2017-2020 Alexander Emelianov (a.m.emelianov@gmail.com)
+*/
+#pragma once
+
+#include "mbed.h"
+#include <algorithm>
+#include <vector>
+#ifdef ARDUINO_ARCH_ESP32
+#include <byteswap.h>
+#endif
+
+#ifndef __bswap_16
+#define __bswap_16(num) (((uint16_t)num >> 8) | ((uint16_t)num << 8))
+#endif
+
+//#define MB_GLOBAL_REGS
+//#define MB_MAX_REGS     32
+#define MODBUS_MAX_FRAME 256
+#define COIL(n) \
+    (TAddress) { TAddress::COIL, n }
+#define ISTS(n) \
+    (TAddress) { TAddress::ISTS, n }
+#define IREG(n) \
+    (TAddress) { TAddress::IREG, n }
+#define HREG(n) \
+    (TAddress) { TAddress::HREG, n }
+#define BIT_VAL(v) (v ? 0xFF00 : 0x0000)
+#define BIT_BOOL(v) (v == 0xFF00)
+#define COIL_VAL(v) (v ? 0xFF00 : 0x0000)
+#define COIL_BOOL(v) (v == 0xFF00)
+#define ISTS_VAL(v) (v ? 0xFF00 : 0x0000)
+#define ISTS_BOOL(v) (v == 0xFF00)
+
+// For depricated (v1.xx) onSet/onGet format compatibility
+#define cbDefault nullptr
+
+struct TRegister;
+
+typedef uint16_t (*cbModbus)(TRegister* reg, uint16_t val); // Callback function Type
+
+struct TAddress {
+    enum RegType { COIL,
+        ISTS,
+        IREG,
+        HREG };
+    RegType type;
+    uint16_t address;
+    bool operator==(const TAddress& obj) const
+    { // TAddress == TAddress
+        return type == obj.type && address == obj.address;
+    }
+    bool operator!=(const TAddress& obj) const
+    { // TAddress != TAddress
+        return type != obj.type || address != obj.address;
+    }
+    TAddress& operator++()
+    { // ++TAddress
+        address++;
+        return *this;
+    }
+    TAddress operator++(int)
+    { // TAddress++
+        TAddress result(*this);
+        ++(*this);
+        return result;
+    }
+    TAddress& operator+=(const int& inc)
+    { // TAddress += integer
+        address += inc;
+        return *this;
+    }
+    const TAddress operator+(const int& inc) const
+    { // TAddress + integer
+        TAddress result(*this);
+        result.address += inc;
+        return result;
+    }
+    bool isCoil()
+    {
+        return type == COIL;
+    }
+    bool isIsts()
+    {
+        return type == ISTS;
+    }
+    bool isIreg()
+    {
+        return type == IREG;
+    }
+    bool isHreg()
+    {
+        return type == HREG;
+    }
+};
+
+struct TCallback {
+    enum CallbackType { ON_SET,
+        ON_GET };
+    CallbackType type;
+    TAddress address;
+    cbModbus cb;
+};
+
+struct TRegister {
+    TAddress address;
+    uint16_t value;
+    bool operator==(const TRegister& obj) const
+    {
+        return address == obj.address;
+    }
+};
+
+class Modbus {
+public:
+    //Function Codes
+    enum FunctionCode {
+        FC_READ_COILS = 0x01, // Read Coils (Output) Status
+        FC_READ_INPUT_STAT = 0x02, // Read Input Status (Discrete Inputs)
+        FC_READ_REGS = 0x03, // Read Holding Registers
+        FC_READ_INPUT_REGS = 0x04, // Read Input Registers
+        FC_WRITE_COIL = 0x05, // Write Single Coil (Output)
+        FC_WRITE_REG = 0x06, // Preset Single Register
+        FC_DIAGNOSTICS = 0x08, // Not implemented. Diagnostics (Serial Line only)
+        FC_WRITE_COILS = 0x0F, // Write Multiple Coils (Outputs)
+        FC_WRITE_REGS = 0x10, // Write block of contiguous registers
+        FC_READ_FILE_REC = 0x14, // Not implemented. Read File Record
+        FC_WRITE_FILE_REC = 0x15, // Not implemented. Write File Record
+        FC_MASKWRITE_REG = 0x16, // Not implemented. Mask Write Register
+        FC_READWRITE_REGS = 0x17 // Not implemented. Read/Write Multiple registers
+    };
+    //Exception Codes
+    //Custom result codes used internally and for callbacks but never used for Modbus responce
+    enum ResultCode {
+        EX_SUCCESS = 0x00, // Custom. No error
+        EX_ILLEGAL_FUNCTION = 0x01, // Function Code not Supported
+        EX_ILLEGAL_ADDRESS = 0x02, // Output Address not exists
+        EX_ILLEGAL_VALUE = 0x03, // Output Value not in Range
+        EX_SLAVE_FAILURE = 0x04, // Slave or Master Device Fails to process request
+        EX_ACKNOWLEDGE = 0x05, // Not used
+        EX_SLAVE_DEVICE_BUSY = 0x06, // Not used
+        EX_MEMORY_PARITY_ERROR = 0x08, // Not used
+        EX_PATH_UNAVAILABLE = 0x0A, // Not used
+        EX_DEVICE_FAILED_TO_RESPOND = 0x0B, // Not used
+        EX_GENERAL_FAILURE = 0xE1, // Custom. Unexpected master error
+        EX_DATA_MISMACH = 0xE2, // Custom. Inpud data size mismach
+        EX_UNEXPECTED_RESPONSE = 0xE3, // Custom. Returned result doesn't mach transaction
+        EX_TIMEOUT = 0xE4, // Custom. Operation not finished within reasonable time
+        EX_CONNECTION_LOST = 0xE5, // Custom. Connection with device lost
+        EX_CANCEL = 0xE6 // Custom. Transaction/request canceled
+    };
+    ~Modbus();
+    bool addHreg(uint16_t offset, uint16_t value = 0, uint16_t numregs = 1);
+    bool Hreg(uint16_t offset, uint16_t value);
+    uint16_t Hreg(uint16_t offset);
+    uint16_t removeHreg(uint16_t offset, uint16_t numregs = 1);
+    bool addCoil(uint16_t offset, bool value = false, uint16_t numregs = 1);
+    bool addIsts(uint16_t offset, bool value = false, uint16_t numregs = 1);
+    bool addIreg(uint16_t offset, uint16_t value = 0, uint16_t numregs = 1);
+    bool Coil(uint16_t offset, bool value);
+    bool Ists(uint16_t offset, bool value);
+    bool Ireg(uint16_t offset, uint16_t value);
+    bool Coil(uint16_t offset);
+    bool Ists(uint16_t offset);
+    uint16_t Ireg(uint16_t offset);
+    bool removeCoil(uint16_t offset, uint16_t numregs = 1);
+    bool removeIsts(uint16_t offset, uint16_t numregs = 1);
+    bool removeIreg(uint16_t offset, uint16_t numregs = 1);
+    /*
+        bool Hreg(uint16_t offset, uint16_t* value);
+        bool Coil(uint16_t offset, bool* value);
+        bool Ists(uint16_t offset, bool* value);
+        bool Ireg(uint16_t offset, uint16_t* value);
+        */
+    void cbEnable(bool state = true);
+    void cbDisable();
+
+    bool onGetCoil(uint16_t offset, cbModbus cb = nullptr, uint16_t numregs = 1);
+    bool onSetCoil(uint16_t offset, cbModbus cb = nullptr, uint16_t numregs = 1);
+    bool onGetHreg(uint16_t offset, cbModbus cb = nullptr, uint16_t numregs = 1);
+    bool onSetHreg(uint16_t offset, cbModbus cb = nullptr, uint16_t numregs = 1);
+    bool onGetIsts(uint16_t offset, cbModbus cb = nullptr, uint16_t numregs = 1);
+    bool onSetIsts(uint16_t offset, cbModbus cb = nullptr, uint16_t numregs = 1);
+    bool onGetIreg(uint16_t offset, cbModbus cb = nullptr, uint16_t numregs = 1);
+    bool onSetIreg(uint16_t offset, cbModbus cb = nullptr, uint16_t numregs = 1);
+
+    bool removeOnGetCoil(uint16_t offset, cbModbus cb = nullptr, uint16_t numregs = 1);
+    bool removeOnSetCoil(uint16_t offset, cbModbus cb = nullptr, uint16_t numregs = 1);
+    bool removeOnGetHreg(uint16_t offset, cbModbus cb = nullptr, uint16_t numregs = 1);
+    bool removeOnSetHreg(uint16_t offset, cbModbus cb = nullptr, uint16_t numregs = 1);
+    bool removeOnGetIsts(uint16_t offset, cbModbus cb = nullptr, uint16_t numregs = 1);
+    bool removeOnSetIsts(uint16_t offset, cbModbus cb = nullptr, uint16_t numregs = 1);
+    bool removeOnGetIreg(uint16_t offset, cbModbus cb = nullptr, uint16_t numregs = 1);
+    bool removeOnSetIreg(uint16_t offset, cbModbus cb = nullptr, uint16_t numregs = 1);
+
+private:
+    void readBits(TAddress startreg, uint16_t numregs, FunctionCode fn);
+    void readWords(TAddress startreg, uint16_t numregs, FunctionCode fn);
+
+    void setMultipleBits(uint8_t* frame, TAddress startreg, uint16_t numoutputs);
+    void setMultipleWords(uint16_t* frame, TAddress startreg, uint16_t numoutputs);
+
+    void getMultipleBits(uint8_t* frame, TAddress startreg, uint16_t numregs);
+    void getMultipleWords(uint16_t* frame, TAddress startreg, uint16_t numregs);
+
+    void bitsToBool(bool* dst, uint8_t* src, uint16_t numregs);
+    void boolToBits(uint8_t* dst, bool* src, uint16_t numregs);
+
+protected:
+    //Reply Types
+    enum ReplyCode {
+        REPLY_OFF = 0x01,
+        REPLY_ECHO = 0x02,
+        REPLY_NORMAL = 0x03,
+        REPLY_ERROR = 0x04,
+        REPLY_UNEXPECTED = 0x05
+    };
+#ifndef MB_GLOBAL_REGS
+    std::vector<TRegister> _regs;
+    std::vector<TCallback> _callbacks;
+#endif
+    uint8_t* _frame = nullptr;
+    uint16_t _len = 0;
+    uint8_t _reply = 0;
+    bool cbEnabled = true;
+    uint16_t mbCallback(TRegister* reg, uint16_t val, TCallback::CallbackType t);
+    TRegister* searchRegister(TAddress addr);
+    void exceptionResponse(FunctionCode fn, ResultCode excode); // Fills _frame with response
+    void successResponce(TAddress startreg, uint16_t numoutputs, FunctionCode fn); // Fills frame with response
+    void slavePDU(uint8_t* frame); //For Slave
+    void masterPDU(uint8_t* frame, uint8_t* sourceFrame, TAddress startreg, uint16_t* output = nullptr); //For Master
+    // frame - data received form slave
+    // sourceFrame - data have sent fo slave
+    // startreg - local register to start put data to
+    // output - if not null put data to the buffer insted local registers. output assumed to by array of uint16_t or boolean
+
+    bool readSlave(uint16_t address, uint16_t numregs, FunctionCode fn);
+    bool writeSlaveBits(TAddress startreg, uint16_t to, uint16_t numregs, FunctionCode fn, bool* data = nullptr);
+    bool writeSlaveWords(TAddress startreg, uint16_t to, uint16_t numregs, FunctionCode fn, uint16_t* data = nullptr);
+    // startreg - local register to get data from
+    // to - slave register to write data to
+    // numregs - number of registers
+    // fn - Modbus function
+    // data - if null use local registers. Otherwise use data from array to erite to slave
+
+    bool addReg(TAddress address, uint16_t value = 0, uint16_t numregs = 1);
+    bool Reg(TAddress address, uint16_t value);
+    uint16_t Reg(TAddress address);
+    bool removeReg(TAddress address, uint16_t numregs = 1);
+
+    bool onGet(TAddress address, cbModbus cb = nullptr, uint16_t numregs = 1);
+    bool onSet(TAddress address, cbModbus cb = nullptr, uint16_t numregs = 1);
+    bool removeOnSet(TAddress address, cbModbus cb = nullptr, uint16_t numregs = 1);
+    bool removeOnGet(TAddress address, cbModbus cb = nullptr, uint16_t numregs = 1);
+
+    virtual uint32_t eventSource() { return 0; }
+};
+
+typedef bool (*cbTransaction)(Modbus::ResultCode event, uint16_t transactionId, void* data); // Callback skeleton for requests

ModbusRTU.cpp

@@ -0,0 +1,452 @@
+/*
+    ModbusRTU Library for ESP8266/ESP32
+    Copyright (C) 2019-2020 Alexander Emelianov (a.m.emelianov@gmail.com)
+	https://github.com/emelianov/modbus-esp8266
+	This code is licensed under the BSD New License. See LICENSE.txt for more info.
+*/
+#include "ModbusRTU.h"
+#include "word.h"
+
+#define pgm_read_word(addr) (*(const unsigned short*)(addr))
+
+// Table of CRC values
+static const uint16_t _auchCRC[] = {
+    0x0000, 0xC1C0, 0x81C1, 0x4001, 0x01C3, 0xC003, 0x8002, 0x41C2, 0x01C6, 0xC006, 0x8007, 0x41C7, 0x0005, 0xC1C5, 0x81C4,
+    0x4004, 0x01CC, 0xC00C, 0x800D, 0x41CD, 0x000F, 0xC1CF, 0x81CE, 0x400E, 0x000A, 0xC1CA, 0x81CB, 0x400B, 0x01C9, 0xC009,
+    0x8008, 0x41C8, 0x01D8, 0xC018, 0x8019, 0x41D9, 0x001B, 0xC1DB, 0x81DA, 0x401A, 0x001E, 0xC1DE, 0x81DF, 0x401F, 0x01DD,
+    0xC01D, 0x801C, 0x41DC, 0x0014, 0xC1D4, 0x81D5, 0x4015, 0x01D7, 0xC017, 0x8016, 0x41D6, 0x01D2, 0xC012, 0x8013, 0x41D3,
+    0x0011, 0xC1D1, 0x81D0, 0x4010, 0x01F0, 0xC030, 0x8031, 0x41F1, 0x0033, 0xC1F3, 0x81F2, 0x4032, 0x0036, 0xC1F6, 0x81F7,
+    0x4037, 0x01F5, 0xC035, 0x8034, 0x41F4, 0x003C, 0xC1FC, 0x81FD, 0x403D, 0x01FF, 0xC03F, 0x803E, 0x41FE, 0x01FA, 0xC03A,
+    0x803B, 0x41FB, 0x0039, 0xC1F9, 0x81F8, 0x4038, 0x0028, 0xC1E8, 0x81E9, 0x4029, 0x01EB, 0xC02B, 0x802A, 0x41EA, 0x01EE,
+    0xC02E, 0x802F, 0x41EF, 0x002D, 0xC1ED, 0x81EC, 0x402C, 0x01E4, 0xC024, 0x8025, 0x41E5, 0x0027, 0xC1E7, 0x81E6, 0x4026,
+    0x0022, 0xC1E2, 0x81E3, 0x4023, 0x01E1, 0xC021, 0x8020, 0x41E0, 0x01A0, 0xC060, 0x8061, 0x41A1, 0x0063, 0xC1A3, 0x81A2,
+    0x4062, 0x0066, 0xC1A6, 0x81A7, 0x4067, 0x01A5, 0xC065, 0x8064, 0x41A4, 0x006C, 0xC1AC, 0x81AD, 0x406D, 0x01AF, 0xC06F,
+    0x806E, 0x41AE, 0x01AA, 0xC06A, 0x806B, 0x41AB, 0x0069, 0xC1A9, 0x81A8, 0x4068, 0x0078, 0xC1B8, 0x81B9, 0x4079, 0x01BB,
+    0xC07B, 0x807A, 0x41BA, 0x01BE, 0xC07E, 0x807F, 0x41BF, 0x007D, 0xC1BD, 0x81BC, 0x407C, 0x01B4, 0xC074, 0x8075, 0x41B5,
+    0x0077, 0xC1B7, 0x81B6, 0x4076, 0x0072, 0xC1B2, 0x81B3, 0x4073, 0x01B1, 0xC071, 0x8070, 0x41B0, 0x0050, 0xC190, 0x8191,
+    0x4051, 0x0193, 0xC053, 0x8052, 0x4192, 0x0196, 0xC056, 0x8057, 0x4197, 0x0055, 0xC195, 0x8194, 0x4054, 0x019C, 0xC05C,
+    0x805D, 0x419D, 0x005F, 0xC19F, 0x819E, 0x405E, 0x005A, 0xC19A, 0x819B, 0x405B, 0x0199, 0xC059, 0x8058, 0x4198, 0x0188,
+    0xC048, 0x8049, 0x4189, 0x004B, 0xC18B, 0x818A, 0x404A, 0x004E, 0xC18E, 0x818F, 0x404F, 0x018D, 0xC04D, 0x804C, 0x418C,
+    0x0044, 0xC184, 0x8185, 0x4045, 0x0187, 0xC047, 0x8046, 0x4186, 0x0182, 0xC042, 0x8043, 0x4183, 0x0041, 0xC181, 0x8180,
+    0x4040, 0x0000
+};
+
+uint16_t ModbusRTU::crc16(uint8_t address, uint8_t* frame, uint8_t pduLen)
+{
+    uint8_t i = 0xFF ^ address;
+    uint16_t val = pgm_read_word(_auchCRC + i);
+    uint8_t CRCHi = 0xFF ^ highByte(val); // Hi
+    uint8_t CRCLo = lowByte(val); //Low
+    while (pduLen--) {
+        i = CRCHi ^ *frame++;
+        val = pgm_read_word(_auchCRC + i);
+        CRCHi = CRCLo ^ highByte(val); // Hi
+        CRCLo = lowByte(val); //Low
+    }
+    return (CRCHi << 8) | CRCLo;
+}
+
+void ModbusRTU::setBaudrate(uint32_t baud)
+{
+    if (baud > 19200) {
+        _t = 50ms;
+    } else {
+        //        _t = (35000 / baud) + 1;
+        _t = 100ms;
+    }
+}
+
+bool ModbusRTU::begin(UnbufferedSerial* port)
+{
+    _port = port;
+    _t = 50ms;
+    _port->attach(callback(this, &ModbusRTU::SerialRXHandler), SerialBase::RxIrq);
+    return true;
+}
+
+bool ModbusRTU::begin(UnbufferedSerial* port, PinName txPin)
+{
+    uint32_t baud = 0;
+#if defined(ESP32) || defined(ESP8266)
+    // baudRate() only available with ESP32+ESP8266
+    baud = port->baudRate();
+#else
+    baud = 9600;
+#endif
+    setBaudrate(baud);
+#if defined(ESP8266)
+    maxRegs = port->setRxBufferSize(MODBUS_MAX_FRAME) / 2 - 3;
+#endif
+    _port = port;
+    if (txPin != NC) {
+        _txPin = new DigitalOut(txPin, 0);
+    }
+    _port->attach(callback(this, &ModbusRTU::SerialRXHandler), SerialBase::RxIrq);
+    return true;
+}
+
+void ModbusRTU::SerialRXHandler()
+{
+    char c;
+    if (_port->readable()) {
+        _port->read(&c, 1);
+        RX_RingBuff.push((uint8_t)(c & (uint8_t)0xFFU));
+    }
+}
+
+void ModbusRTU::SerialTXHandler()
+{
+    //Здесь проверка на TC а не на RX!!!!
+    //Изменено в serial_api.c
+    if (_port->writeable()) {
+        _port->attach(nullptr, SerialBase::TxIrq);
+        *_txPin = 0;
+    }
+}
+
+uint8_t ModbusRTU::Read(void)
+{
+    uint8_t cur = 0xff;
+    if (!RX_RingBuff.empty()) {
+        RX_RingBuff.pop((char&)cur);
+    }
+    return cur;
+}
+
+uint8_t ModbusRTU::isDataAvailable(void)
+{
+    if (RX_RingBuff.full()) {
+        RX_RingBuff.reset();
+    }
+    return !RX_RingBuff.empty();
+}
+
+#if defined(ESP8266)
+bool ModbusRTU::begin(SoftwareSerial* port, int16_t txPin)
+{
+    uint32_t baud = port->baudRate();
+    _port = port;
+    if (txPin >= 0)
+        port->setTransmitEnablePin(txPin);
+    setBaudrate(baud);
+    return true;
+}
+#endif
+
+bool ModbusRTU::rawSend(uint8_t slaveId, uint8_t* frame, uint8_t len)
+{
+    uint16_t newCrc = crc16(slaveId, frame, len);
+    char crc[2];
+    crc[0] = newCrc >> 8;
+    crc[1] = newCrc & 0xFF;
+    *_txPin = 1;
+#ifdef ESP32
+    portENTER_CRITICAL(&mux);
+#endif
+    _port->write(&slaveId, 1); //Send slaveId
+    _port->write(frame, len); // Send PDU
+    _port->write(crc, 2); //Send CRC
+#ifdef ESP32
+    portEXIT_CRITICAL(&mux);
+#endif
+    _port->attach(callback(this, &ModbusRTU::SerialTXHandler), SerialBase::TxIrq);
+    return true;
+}
+
+bool ModbusRTU::send(uint8_t slaveId, TAddress startreg, cbTransaction cb, void* data, bool waitResponse)
+{
+    if (_slaveId)
+        return false; // Break if waiting for previous request result
+    rawSend(slaveId, _frame, _len);
+    if (waitResponse) {
+        _slaveId = slaveId;
+        _timestamp = Kernel::Clock::now();
+        _cb = cb;
+        _data = data;
+        _sentFrame = _frame;
+        _sentReg = startreg;
+        _frame = nullptr;
+        _len = 0;
+    }
+    return true;
+}
+
+void ModbusRTU::task()
+{
+
+    if (RX_RingBuff.size() > _len) {
+        _len = RX_RingBuff.size();
+        t = Kernel::Clock::now();
+        return;
+    }
+    if (_len != 0 && Kernel::Clock::now() - t < _t) // Wait data whitespace if there is data
+        return;
+    if (isMaster)
+        cleanup();
+    if (_len == 0)
+        return;
+
+    uint8_t address;
+    address = Read();
+    _len--; // Decrease by slaveId byte
+    if (isMaster && _slaveId == 0) { // Check if slaveId is set
+        RX_RingBuff.reset(); // Skip packet if is not expected
+        _len = 0;
+        return;
+    }
+    if (address != MODBUSRTU_BROADCAST && address != _slaveId) { // SlaveId Check
+        RX_RingBuff.reset(); // Skip packet if SlaveId doesn't mach
+        _len = 0;
+        return;
+    }
+
+    free(_frame); //Just in case
+    _frame = (uint8_t*)malloc(_len);
+    if (!_frame) { // Fail to allocate buffer
+        RX_RingBuff.reset(); // Skip packet if can't allocate buffer
+        _len = 0;
+        return;
+    }
+    for (uint8_t i = 0; i < _len; i++) {
+        _frame[i] = Read(); // read data + crc
+#if defined(MODBUSRTU_DEBUG)
+        Serial.printf("%02X ", _frame[i]);
+#endif
+    }
+#if defined(MODBUSRTU_DEBUG)
+    Serial.println();
+#endif
+    uint16_t frameCrc = ((_frame[_len - 2] << 8) | _frame[_len - 1]); // Last two byts = crc
+    _len = _len - 2; // Decrease by CRC 2 bytes
+    if (frameCrc != crc16(address, _frame, _len)) { // CRC Check
+        free(_frame);
+        RX_RingBuff.reset();
+        _frame = nullptr;
+        _len = 0; // Cleanup if wrong crc
+        return;
+    }
+    if (isMaster) {
+        _reply = EX_SUCCESS;
+        if ((_frame[0] & 0x7F) == _sentFrame[0]) { // Check if function code the same as requested
+            // Procass incoming frame as master
+            masterPDU(_frame, _sentFrame, _sentReg, (uint16_t*)_data);
+            if (_cb) {
+                _cb((ResultCode)_reply, 0, nullptr);
+            }
+            free(_sentFrame);
+            _sentFrame = nullptr;
+            _data = nullptr;
+            _slaveId = 0;
+        }
+        _reply = Modbus::REPLY_OFF; // No reply if master
+    } else {
+        slavePDU(_frame);
+        if (address == MODBUSRTU_BROADCAST)
+            _reply = Modbus::REPLY_OFF; // No reply for Broadcasts
+    }
+    if (_reply != Modbus::REPLY_OFF)
+        rawSend(_slaveId, _frame, _len);
+    // Cleanup
+    free(_frame);
+    RX_RingBuff.reset();
+    _frame = nullptr;
+    _len = 0;
+}
+
+bool ModbusRTU::cleanup()
+{
+    // Remove timeouted request and forced event
+    if (_slaveId && (Kernel::Clock::now() - _timestamp > MODBUSRTU_TIMEOUT)) {
+        if (_cb)
+            _cb(Modbus::EX_TIMEOUT, 0, nullptr);
+        free(_sentFrame);
+        RX_RingBuff.reset();
+        _sentFrame = nullptr;
+        _data = nullptr;
+        _slaveId = 0;
+        return true;
+    }
+    return false;
+}
+
+uint16_t ModbusRTU::writeHreg(uint8_t slaveId, uint16_t offset, uint16_t value, cbTransaction cb)
+{
+    readSlave(offset, value, FC_WRITE_REG);
+    return send(slaveId, HREG(offset), cb, nullptr, cb);
+}
+
+uint16_t ModbusRTU::writeCoil(uint8_t slaveId, uint16_t offset, bool value, cbTransaction cb)
+{
+    readSlave(offset, COIL_VAL(value), FC_WRITE_COIL);
+    return send(slaveId, COIL(offset), cb, nullptr, cb);
+}
+
+uint16_t ModbusRTU::readCoil(uint8_t slaveId, uint16_t offset, bool* value, uint16_t numregs, cbTransaction cb)
+{
+    if (numregs < 0x0001 || numregs > maxRegs << 4)
+        return false;
+    readSlave(offset, numregs, FC_READ_COILS);
+    return send(slaveId, COIL(offset), cb, value);
+}
+
+uint16_t ModbusRTU::writeCoil(uint8_t slaveId, uint16_t offset, bool* value, uint16_t numregs, cbTransaction cb)
+{
+    if (numregs < 0x0001 || numregs > 0x07D0)
+        return false;
+    writeSlaveBits(COIL(offset), offset, numregs, FC_WRITE_COILS, value);
+    return send(slaveId, COIL(offset), cb, nullptr, cb);
+}
+
+uint16_t ModbusRTU::writeHreg(uint8_t slaveId, uint16_t offset, uint16_t* value, uint16_t numregs, cbTransaction cb)
+{
+    if (numregs < 0x0001 || numregs > 0x007D)
+        return false;
+    writeSlaveWords(HREG(offset), offset, numregs, FC_WRITE_REGS, value);
+    return send(slaveId, HREG(offset), cb, nullptr, cb);
+}
+
+uint16_t ModbusRTU::readHreg(uint8_t slaveId, uint16_t offset, uint16_t* value, uint16_t numregs, cbTransaction cb)
+{
+    if (numregs < 0x0001 || numregs > maxRegs)
+        return false;
+    readSlave(offset, numregs, FC_READ_REGS);
+    return send(slaveId, HREG(offset), cb, value);
+}
+
+uint16_t ModbusRTU::readIsts(uint8_t slaveId, uint16_t offset, bool* value, uint16_t numregs, cbTransaction cb)
+{
+    if (numregs < 0x0001 || numregs > maxRegs << 4)
+        return false;
+    readSlave(offset, numregs, FC_READ_INPUT_STAT);
+    return send(slaveId, ISTS(offset), cb, value);
+}
+
+uint16_t ModbusRTU::readIreg(uint8_t slaveId, uint16_t offset, uint16_t* value, uint16_t numregs, cbTransaction cb)
+{
+    if (numregs < 0x0001 || numregs > maxRegs)
+        return false;
+    readSlave(offset, numregs, FC_READ_INPUT_REGS);
+    return send(slaveId, IREG(offset), cb, value);
+}
+
+uint16_t ModbusRTU::pushCoil(uint8_t slaveId, uint16_t to, uint16_t from, uint16_t numregs, cbTransaction cb)
+{
+    if (numregs < 0x0001 || numregs > 0x07D0)
+        return false;
+    if (!searchRegister(COIL(from)))
+        return false;
+    if (numregs == 1) {
+        readSlave(to, COIL_VAL(Coil(from)), FC_WRITE_COIL);
+    } else {
+        writeSlaveBits(COIL(from), to, numregs, FC_WRITE_COILS);
+    }
+    return send(slaveId, COIL(from), cb);
+}
+
+uint16_t ModbusRTU::pullCoil(uint8_t slaveId, uint16_t from, uint16_t to, uint16_t numregs, cbTransaction cb)
+{
+    if (numregs < 0x0001 || numregs > maxRegs << 4)
+        return false;
+#ifdef MODBUSRTU_ADD_REG
+    addCoil(to, numregs);
+#endif
+    readSlave(from, numregs, FC_READ_COILS);
+    return send(slaveId, COIL(to), cb);
+}
+
+uint16_t ModbusRTU::pullIsts(uint8_t slaveId, uint16_t from, uint16_t to, uint16_t numregs, cbTransaction cb)
+{
+    if (numregs < 0x0001 || numregs > maxRegs << 4)
+        return false;
+#ifdef MODBUSRTU_ADD_REG
+    addIsts(to, numregs);
+#endif
+    readSlave(from, numregs, FC_READ_INPUT_STAT);
+    return send(slaveId, ISTS(to), cb);
+}
+
+uint16_t ModbusRTU::pushHreg(uint8_t slaveId, uint16_t to, uint16_t from, uint16_t numregs, cbTransaction cb)
+{
+    if (numregs < 0x0001 || numregs > 0x007D)
+        return false;
+    if (!searchRegister(HREG(from)))
+        return false;
+    if (numregs == 1) {
+        readSlave(to, Hreg(from), FC_WRITE_REG);
+    } else {
+        writeSlaveWords(HREG(from), to, numregs, FC_WRITE_REGS);
+    }
+    return send(slaveId, HREG(from), cb);
+}
+
+uint16_t ModbusRTU::pullHreg(uint8_t slaveId, uint16_t from, uint16_t to, uint16_t numregs, cbTransaction cb)
+{
+    if (numregs < 0x0001 || numregs > maxRegs)
+        return false;
+#ifdef MODBUSRTU_ADD_REG
+    addHreg(to, numregs);
+#endif
+    readSlave(from, numregs, FC_READ_REGS);
+    return send(slaveId, HREG(to), cb);
+}
+
+uint16_t ModbusRTU::pullIreg(uint8_t slaveId, uint16_t from, uint16_t to, uint16_t numregs, cbTransaction cb)
+{
+    if (numregs < 0x0001 || numregs > maxRegs)
+        return false;
+#ifdef MODBUSRTU_ADD_REG
+    addIreg(to, numregs);
+#endif
+    readSlave(from, numregs, FC_READ_INPUT_REGS);
+    return send(slaveId, IREG(to), cb);
+}
+
+uint16_t ModbusRTU::pushIregToHreg(uint8_t slaveId, uint16_t to, uint16_t from, uint16_t numregs, cbTransaction cb)
+{
+    if (numregs < 0x0001 || numregs > 0x007D)
+        return false;
+    if (!searchRegister(IREG(from)))
+        return false;
+    if (numregs == 1) {
+        readSlave(to, Ireg(from), FC_WRITE_REG);
+    } else {
+        writeSlaveWords(IREG(from), to, numregs, FC_WRITE_REGS);
+    }
+    return send(slaveId, IREG(from), cb);
+}
+
+uint16_t ModbusRTU::pushIstsToCoil(uint8_t slaveId, uint16_t to, uint16_t from, uint16_t numregs, cbTransaction cb)
+{
+    if (numregs < 0x0001 || numregs > maxRegs << 4)
+        return false;
+    if (!searchRegister(ISTS(from)))
+        return false;
+    if (numregs == 1) {
+        readSlave(to, ISTS_VAL(Ists(from)), FC_WRITE_COIL);
+    } else {
+        writeSlaveBits(ISTS(from), to, numregs, FC_WRITE_COILS);
+    }
+    return send(slaveId, ISTS(from), cb);
+}
+
+uint16_t ModbusRTU::pullHregToIreg(uint8_t slaveId, uint16_t from, uint16_t to, uint16_t numregs, cbTransaction cb)
+{
+    if (numregs < 0x0001 || numregs > maxRegs)
+        return false;
+#ifdef MODBUSRTU_ADD_REG
+    addIreg(to, numregs);
+#endif
+    readSlave(from, numregs, FC_READ_REGS);
+    return send(slaveId, IREG(to), cb);
+}
+
+uint16_t ModbusRTU::pullCoilToIsts(uint8_t slaveId, uint16_t from, uint16_t to, uint16_t numregs, cbTransaction cb)
+{
+    if (numregs < 0x0001 || numregs > maxRegs << 4)
+        return false;
+#ifdef MODBUSRTU_ADD_REG
+    addIsts(to, numregs);
+#endif
+    readSlave(from, numregs, FC_READ_COILS);
+    return send(slaveId, ISTS(to), cb);
+}

ModbusRTU.h

@@ -0,0 +1,90 @@
+/*
+    ModbusRTU Library for ESP8266/ESP32
+    Copyright (C) 2019-2020 Alexander Emelianov (a.m.emelianov@gmail.com)
+	https://github.com/emelianov/modbus-esp8266
+	This code is licensed under the BSD New License. See LICENSE.txt for more info.
+*/
+#pragma once
+#include "mbed.h"
+#include <Modbus.h>
+
+#if defined(ESP8266)
+#include <SoftwareSerial.h>
+#endif
+
+//#define MODBUSRTU_DEBUG
+#define MODBUSRTU_BROADCAST 0
+#define MB_RESERVE 248
+#define MB_SERIAL_BUFFER 128
+#define MB_MAX_TIME 10
+//#define MODBUSRTU_TIMEOUT 1s
+#define MODBUSRTU_ADD_REG
+//#define MB_STATIC_FRAME 1
+
+class ModbusRTU : public Modbus {
+protected:
+    CircularBuffer<char, 200> RX_RingBuff;
+    UnbufferedSerial* _port;
+    DigitalOut* _txPin;
+    void SerialRXHandler();
+    void SerialTXHandler();
+    uint8_t isDataAvailable(void);
+    uint8_t Read(void);
+    //    unsigned int
+    Kernel::Clock::duration MODBUSRTU_TIMEOUT = 1s;
+    Kernel::Clock::duration _t; // inter-frame delay in mS
+    Kernel::Clock::time_point t; // time sience last data byte arrived
+    bool isMaster = false;
+    uint8_t _slaveId;
+    Kernel::Clock::time_point _timestamp;
+    cbTransaction _cb = nullptr;
+    void* _data = nullptr;
+    uint8_t* _sentFrame = nullptr;
+    TAddress _sentReg = COIL(0);
+    uint16_t maxRegs = 0x007D;
+#ifdef ESP32
+    portMUX_TYPE mux = portMUX_INITIALIZER_UNLOCKED;
+#endif
+    bool send(uint8_t slaveId, TAddress startreg, cbTransaction cb, void* data = nullptr, bool waitResponse = true);
+    // Prepare and send ModbusRTU frame. _frame buffer and _len should be filled with Modbus data
+    // slaveId - slave id
+    // startreg - first local register to save returned data to (miningless for write to slave operations)
+    // cb - transaction callback function
+    // data - if not null use buffer to save returned data instead of local registers
+    bool rawSend(uint8_t slaveId, uint8_t* frame, uint8_t len);
+    bool cleanup(); // Free clients if not connected and remove timedout transactions and transaction with forced events
+    uint16_t crc16(uint8_t address, uint8_t* frame, uint8_t pdulen);
+
+public:
+    void setBaudrate(uint32_t baud = -1);
+#if defined(ESP8266)
+    bool begin(BufferedSerial* port, int16_t txPin = -1);
+#endif
+    bool begin(UnbufferedSerial* port, PinName txPin = NC);
+    bool begin(UnbufferedSerial* port);
+    void task();
+    void master() { isMaster = true; };
+    void slave(uint8_t slaveId) { _slaveId = slaveId; };
+    uint8_t slave() { return _slaveId; }
+    uint32_t eventSource() override { return _slaveId; }
+    uint16_t writeHreg(uint8_t slaveId, uint16_t offset, uint16_t value, cbTransaction cb = nullptr);
+    uint16_t writeCoil(uint8_t slaveId, uint16_t offset, bool value, cbTransaction cb = nullptr);
+    uint16_t readCoil(uint8_t slaveId, uint16_t offset, bool* value, uint16_t numregs = 1, cbTransaction cb = nullptr);
+    uint16_t writeCoil(uint8_t slaveId, uint16_t offset, bool* value, uint16_t numregs = 1, cbTransaction cb = nullptr);
+    uint16_t writeHreg(uint8_t slaveId, uint16_t offset, uint16_t* value, uint16_t numregs = 1, cbTransaction cb = nullptr);
+    uint16_t readIsts(uint8_t slaveId, uint16_t offset, bool* value, uint16_t numregs = 1, cbTransaction cb = nullptr);
+    uint16_t readHreg(uint8_t slaveId, uint16_t offset, uint16_t* value, uint16_t numregs = 1, cbTransaction cb = nullptr);
+    uint16_t readIreg(uint8_t slaveId, uint16_t offset, uint16_t* value, uint16_t numregs = 1, cbTransaction cb = nullptr);
+
+    uint16_t pushCoil(uint8_t slaveId, uint16_t to, uint16_t from, uint16_t numregs = 1, cbTransaction cb = nullptr);
+    uint16_t pullCoil(uint8_t slaveId, uint16_t from, uint16_t to, uint16_t numregs = 1, cbTransaction cb = nullptr);
+    uint16_t pullIsts(uint8_t slaveId, uint16_t from, uint16_t to, uint16_t numregs = 1, cbTransaction cb = nullptr);
+    uint16_t pushHreg(uint8_t slaveId, uint16_t to, uint16_t from, uint16_t numregs = 1, cbTransaction cb = nullptr);
+    uint16_t pullHreg(uint8_t slaveId, uint16_t from, uint16_t to, uint16_t numregs = 1, cbTransaction cb = nullptr);
+    uint16_t pullIreg(uint8_t slaveId, uint16_t from, uint16_t to, uint16_t numregs = 1, cbTransaction cb = nullptr);
+
+    uint16_t pullHregToIreg(uint8_t slaveId, uint16_t offset, uint16_t startreg, uint16_t numregs = 1, cbTransaction cb = nullptr);
+    uint16_t pullCoilToIsts(uint8_t slaveId, uint16_t offset, uint16_t startreg, uint16_t numregs = 1, cbTransaction cb = nullptr);
+    uint16_t pushIstsToCoil(uint8_t slaveId, uint16_t to, uint16_t from, uint16_t numregs = 1, cbTransaction cb = nullptr);
+    uint16_t pushIregToHreg(uint8_t slaveId, uint16_t to, uint16_t from, uint16_t numregs = 1, cbTransaction cb = nullptr);
+};

word.h

@@ -0,0 +1,95 @@
+/**
+@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 <http://www.gnu.org/licenses/>.
+
+  Written by Doc Walker (Rx)
+  Copyright © 2009-2015 Doc Walker <4-20ma at wvfans dot net>
+
+*/
+
+#ifndef _UTIL_WORD_H_
+#define _UTIL_WORD_H_
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+#include "mbed.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);
+  }
+
+  /*模拟ardunio函数*************************************************/
+  static inline uint8_t lowByte(uint16_t ww)
+  {
+    return (uint8_t)((ww)&0x00FF);
+  }
+
+  static inline uint8_t highByte(uint16_t ww)
+  {
+    return (uint8_t)((ww) >> 8);
+  }
+
+  static inline uint16_t word(uint8_t H_Byte, uint8_t L_Byte)
+  {
+    uint16_t word;
+    word = (uint16_t)(H_Byte << 8);
+    word = word + L_Byte;
+    return word;
+  }
+
+#define bitSet(value, bit) ((value) |= (1UL << (bit)))
+#define bitClear(value, bit) ((value) &= ~(1UL << (bit)))
+
+#define bitWrite(value, bit, bitvalue) (bitvalue ? bitSet(value, bit) : bitClear(value, bit))
+#define bitRead(value, bit) (((value) >> (bit)) & 0x01)
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* _UTIL_WORD_H_ */