ModbusBridge.cpp

Go to the documentation of this file.

#include "config.h"
 
#ifdef MODBUS_BRIDGE
 
#include <ArduinoLog.h>
#include "./Addon.h"
#include "ModbusRtu.h"
#include "ModbusBridge.h"
#include "config.h"
#include <SPI.h>
#include <Ethernet.h>
#include "enums.h"
#include "debug_utils.h"
 
#include "./OmronVFD.h"
#include "./OmronPID.h"
 
#define HAS_DEBUG_MODBUS_BRIDGE_PRINT_QUEUE
 
#define RS485Serial 3
#define MasterModbusAdd 0
Modbus master(MasterModbusAdd, RS485Serial);
modbus_t ModbusQuery[1];
uint16_t ModbusSlaveRegisters[8];
millis_t WaitingTime;
int _state = IDLE;
bool __debugQuery = false;
bool debugReset = false;
bool debugErrors = false;
 
void ModbusBridge::setDebugSend(bool debug)
{
    master.debugSend = debug;
}
Modbus *ModbusBridge::modbus()
{
    return &master;
}
short ModbusBridge::setup()
{
    master.begin(MODBUS_RS485_BAUDRATE, MODBUS_RS485_PORT);
    master.setTimeOut(MODBUS_RS485_TIMEOUT);
    WaitingTime = millis() + nextWaitingTime;
    _state = IDLE;
    startTS = millis();
 
    // defaults
    for (uchar i = 0; i < MAX_QUERY_BUFFER; i++)
    {
        queries[i].reset();
        queries[i].id = i;
    }
    for (uchar i = 0; i < MODBUS_TCP_MAX_REGISTERS; i++)
    {
        mb->R[i] = MODBUS_TCP_DEFAULT_REGISTER_VALUE;
        mb->C[i] = MODBUS_TCP_DEFAULT_REGISTER_VALUE;
    }
    return E_OK;
}
Query *ModbusBridge::nextByPrio(uchar state, int prio)
{
    Query *oldest;
    for (int i = 0; i < MAX_QUERY_BUFFER; i++)
    {
        if (!oldest)
        {
            oldest = &queries[i];
        }
        if (queries[i].state == state && queries[i].prio == prio)
        {
            oldest = &queries[i];
        }
    }
    return oldest;
}
Query *ModbusBridge::nextQueryByState2(uchar state, int owner)
{
    Query *ret = NULL;
    for (int i = 0; i < MAX_QUERY_BUFFER; i++)
    {
        if (queries[i].state == state && queries[i].owner == owner)
        {
            if (!ret)
            {
                ret = &queries[i];
            }
 
            if (queries[i].prio > ret->prio)
            {
                ret = &queries[i];
            }
        }
    }
    return ret;
}
bool ModbusBridge::skipRead(int slave, int fn, int addr, int num, int prio){
    Query *same = nextSame(QUEUED, slave, addr, fn, num);
    if (same && millis() - same->ts < MODBUS_SAME_REQUEST_INTERVAL)
    {
        return true;
    }
 
    if (numByState(DONE) < MODBUS_QUEUE_MIN_FREE)
    {
        return true;
    }
 
    if (numSame(QUEUED, slave, addr, fn, num) >= 1)
    {
        return true;
    }
 
    if (numSame(PROCESSING, slave, addr, fn, num) >= 1)
    {
        return true;
    }
    return false;
}
void ModbusBridge::clearQueue()
{
    millis_t t = millis();
    for (int i = 0; i < MAX_QUERY_BUFFER; i++)
    {
        if ((queries[i].state == QUEUED ||
             queries[i].state == PROCESSING) &&
            t - queries[i].ts > MODBUS_MAX_LIFESPAN)
        {
            if (debugReset)
            {
                queries[i].print();
                print();
            }
            queries[i].reset();
            continue;
        }
    }
}
Query *ModbusBridge::nextQueryByState(uchar state, int owner)
{
    if (owner > 0)
    {
        Query *q = nextQueryByOwner(state, owner);
        if (q != NULL)
        {
            return q;
        }
    }
 
    millis_t t = millis();
    Query *oldest = NULL;
    for (int i = 0; i < MAX_QUERY_BUFFER; i++)
    {
        if (queries[i].state == state)
        {
            if (queries[i].prio != MB_QUERY_TYPE_CMD && t - queries[i].ts > 1000)
            {
                return &queries[i];
            }
 
            if (!oldest)
            {
                oldest = &queries[i];
            }
 
            if (queries[i].ts > oldest->ts)
            {
                oldest = &queries[i];
            }
        }
    }
    return oldest;
}
Query *ModbusBridge::nextQueryByOwner(uchar state, int owner)
{
    millis_t t = millis();
    Query *oldest = NULL;
    for (int i = 0; i < MAX_QUERY_BUFFER; i++)
    {
        if (queries[i].state == state && queries[i].owner == owner)
        {
 
            if (queries[i].ts == 0)
            {
                queries[i].ts = t;
                continue;
            }
 
            if (queries[i].prio == MB_QUERY_TYPE_CMD && t - queries[i].ts > 300)
            {
                return &queries[i];
            }
 
            if (!oldest)
            {
                oldest = &queries[i];
            }
 
            if (queries[i].ts > oldest->ts)
            {
                oldest = &queries[i];
            }
        }
    }
    return oldest;
}
Query *ModbusBridge::nextSame(uchar state, short slave, int addr, short fn, int value)
{
    Query *oldest = NULL;
    for (int i = 0; i < MAX_QUERY_BUFFER; i++)
    {
        if (queries[i].state == state)
        {
            Query *q = &queries[i];
            if (q->addr == addr && q->fn == fn && q->value == value && q->slave == slave)
            {
                if (!oldest)
                {
                    oldest = &queries[i];
                }
                if (queries[i].ts > oldest->ts)
                {
                    oldest = &queries[i];
                }
            }
        }
    }
    return oldest;
}
int ModbusBridge::numSame(uchar state, short slave, int addr, short fn, int value)
{
    int num = 0;
    for (int i = 0; i < MAX_QUERY_BUFFER; i++)
    {
        if (queries[i].state == state)
        {
            Query *q = &queries[i];
            if (q->addr == addr && q->fn == fn && q->value == value && q->slave == slave)
            {
                num++;
            }
        }
    }
    return num;
}
int ModbusBridge::numSameOwner(uchar state, short slave, int addr, short fn, int value, int owner)
{
    int num = 0;
    for (int i = 0; i < MAX_QUERY_BUFFER; i++)
    {
        if (queries[i].state == state && queries[i].owner == owner)
        {
            Query *q = &queries[i];
            if (q->addr == addr && q->fn == fn && q->value == value && q->slave == slave)
            {
                num++;
            }
        }
    }
    return num;
}
int ModbusBridge::numByState(int state)
{
    int num = 0;
    for (int i = 0; i < MAX_QUERY_BUFFER; i++)
    {
        if (queries[i].state == state)
        {
            num++;
        }
    }
    return num;
}
void ModbusBridge::print()
{
#ifdef HAS_DEBUG_MODBUS_BRIDGE_PRINT_QUEUE
    Serial.print("----- Queries : --- ");
    Serial.print("Proccessing : ");
    Serial.print(numByState(PROCESSING));
    Serial.print(" | QUEUED : ");
    Serial.print(numByState(QUEUED));
    Serial.print(" | DONE: ");
    Serial.print(numByState(DONE));
    Serial.print(" | ADDR: ");
    Serial.print(cAddr);
    Serial.print(" | FN: ");
    Serial.print(cFN);
    Serial.print(" | NOW : ");
    Serial.print(millis());
    Serial.print("-----\n");
    for (int i = 0; i < MAX_QUERY_BUFFER; i++)
    {
        if (queries[i].state == DONE)
        {
            continue;
        }
        Serial.print(" - ");
        Serial.print(queries[i].id);
        Serial.print(". \t ");
        queries[i].print();
        Serial.print("\n");
    }
#endif
}
short ModbusBridge::qstate()
{
    return _state;
    // return modbus->getState();
}
short ModbusBridge::loop()
{
    clearQueue();
    loopPoll();
    mb->Run();
    return E_OK;
}
short ModbusBridge::query(
    int slave,
    short function,
    long start,
    int coils,
    Component *_owner,
    ComponentFnPtr _responseFn)
{
    if (qstate() != IDLE)
    {
        return WAITING;
    }
 
    cAddr = 0;
    cSlave = slave;
    cFN = function;
    cAddr = start;
    cNB = coils;
    rOwner = _owner;
    responseFn = _responseFn;
#ifdef HAS_DEBUG_MODBUS_BRIDGE_PRINT_QUERY
    if (__debugQuery)
    {
        Serial.print("\n --------------Modbus QUERY --------- SLAVE :  ");
        Serial.print(slave);
        Serial.print(" | FN : ");
        Serial.print(cFN);
        Serial.print(" | NB : ");
        Serial.print(coils);
        Serial.print(" | Address : ");
        Serial.print(cAddr, HEX);
        Serial.print(" | STATE : ");
        Serial.print(_state);
        Serial.print(" | OWNER : ");
        Serial.println(rOwner->id);
        Serial.println(" \n ");
    }
#endif
    _state = WAITING;
 
    return E_OK;
}
 
short ModbusBridge::query(int slave, short function, long start, int coils, int _ownerId)
{
    if (qstate() != IDLE)
    {
        return WAITING;
    }
 
    cAddr = 0;
    cSlave = slave;
    cFN = function;
    cAddr = start;
    cNB = coils;
    ownerId = _ownerId;
#ifdef HAS_DEBUG_MODBUS_BRIDGE_PRINT_QUERY
    if (__debugQuery)
    {
        Serial.print("\n --------------Modbus QUERY --------- SLAVE :  ");
        Serial.print(slave);
        Serial.print(" | FN : ");
        Serial.print(cFN);
        Serial.print(" | NB : ");
        Serial.print(coils);
        Serial.print(" | Address : ");
        Serial.print(cAddr, HEX);
        Serial.print(" | STATE : ");
        Serial.print(_state);
        Serial.print(" | OWNER : ");
        Serial.println(ownerId);
        Serial.println(" \n ");
    }
#endif
    _state = WAITING;
    return E_OK;
}
short ModbusBridge::loopPoll()
{
    switch (_state)
    {
 
    case IDLE:
    {
        return E_OK;
    }
 
    case WAITING:
    {
        if (millis() > WaitingTime)
        {
            _state = QUERY;
        }
        break;
    }
    case QUERY:
    {
        ModbusQuery[0].u8id = cSlave;                  // slave address
        ModbusQuery[0].u8fct = cFN;                    // function code (this one is registers read)
        ModbusQuery[0].u16RegAdd = cAddr;              // start address in slave
        ModbusQuery[0].u16CoilsNo = cNB;               // number of elements (coils or registers) to read
        ModbusQuery[0].au16reg = ModbusSlaveRegisters; // pointer to a memory array in the CONTROLLINO
        master.query(ModbusQuery[0]);                  // send query (only once)
        _state = RESPONSE;                             // set to RESPONSE
        break;
    }
    case RESPONSE:
    {
        master.poll();
        Component *c = NULL;
 
        if (owner && ownerId)
        {
            c = app->byId(ownerId);
        }
 
        if (master.getState() == COM_IDLE)
        {
            _state = IDLE;
            int errors = master.getErrCnt();
            if (errors)
            {
                if (debugErrors)
                {
                    Log.verboseln("Modbus Error : %d | Slave=%d | FN=%d", master.getLastError(), cSlave, cFN);
                }
                if (c)
                {
                    c->onError(id, master.getLastError());
                }
                master.clearError();
                ownerId = 0;
                return E_OK;
            }
 
            long onMessageError = 0;
            if (c)
            {
                onMessageError = c->onRawResponse(master.rxSize, master.rxBuffer);
                c->onResponse(onMessageError);
            }
            WaitingTime = millis() + nextWaitingTime;
 
            ownerId = 0;
        }
        break;
    }
    }
    return E_OK;
}
short ModbusBridge::loop_test()
{
    switch (_state)
    {
 
    case IDLE:
    {
        return E_OK;
    }
 
    case WAITING:
    {
        if (millis() > WaitingTime)
        {
            _state++; // set to query state
        }
        break;
    }
    case QUERY:
    {
        ModbusQuery[0].u8id = cSlave;                  // slave address
        ModbusQuery[0].u8fct = cFN;                    // function code (this one is registers read)
        ModbusQuery[0].u16RegAdd = cAddr;              // start address in slave
        ModbusQuery[0].u16CoilsNo = cNB;               // number of elements (coils or registers) to read
        ModbusQuery[0].au16reg = ModbusSlaveRegisters; // pointer to a memory array in the CONTROLLINO
        master.query(ModbusQuery[0]);                  // send query (only once)
        _state++;                                      // set to RESPONSE
        break;
    }
    case RESPONSE:
    {
        master.poll();
        if (master.getState() == COM_IDLE)
        {
            int errors = master.getErrCnt();
            if (errors)
            {
                if (rOwner && onError != NULL)
                {
                    (rOwner->*onError)(master.getLastError(), 1);
                    Log.verboseln("MB-Error: %d | Slave=%d | FN=%d", master.getLastError(), cSlave, cFN);
                    master.clearError();
                }
                else
                {
                    Log.errorln("MB-Error::%d", master.getLastError());
                }
                _state = IDLE;
                return E_OK;
            }
 
            long onMessageError = 0;
            if (rOwner && onMessage)
            {
                onMessageError = (rOwner->*onMessage)(master.rxSize, master.rxBuffer);
            }
            (rOwner->*responseFn)(onMessageError, 1);
            WaitingTime = millis() + nextWaitingTime;
            _state = IDLE;
        }
        break;
    }
    }
    return E_OK;
}
short ModbusBridge::debug()
{
    return E_OK;
}
short ModbusBridge::info()
{
    return E_OK;
}
 
#endif