File content as of revision 173:acfb464a4aec:

/******************************************************************************
 *
 * File:                ModbusMaster.cpp
 * Desciption:          source for the ICE Modbus Master
 *
 *****************************************************************************/
#include "global.h"
#include <stdio.h>
#include "MTSLog.h"
#include "BLEDataHandler.h"
#include "ModbusMaster.h"
#include "MbedJSONValue.h"

DigitalOut dout1(PC_1);
DigitalOut dout2(PA_1);
DigitalIn flow_switch(PA_0);
DigitalIn dinp2(PC_13);

/*****************************************************************************
 * Function:             ModbusMaster
 * Description:          entry point for the Modbus Master
 *
 * @param                (IN) args (user-defined arguments)
 * @return               none
 *****************************************************************************/
std::map<std::string,VirtualCommand> VirtualCommandMap;

char ModbusMasterScratchBuf[1024];
void ModbusMaster(void const *args)
{
    logInfo("%s ModbusMaster has started...", __func__);
    bool status;
    RegisterType_t regType;
    bool move_up = true;

    modbus_init(MB_BAUD_RATE);
    DigitalOut flu_power(PA_11);
    flu_power = 0; // provide power to the modbus

    while( true ) {

        MbedJSONValue json_value;

        // configure modbus registers based in all files that start with "input"
        std::vector<mDot::mdot_file> file_list = GLOBAL_mdot->listUserFiles();
        for (std::vector<mDot::mdot_file>::iterator i = file_list.begin(); i != file_list.end(); ++i) {
            if( (strncmp( i->name, "vcmd", (strlen("input")-1)) == 0) ) {
                status = GLOBAL_mdot->readUserFile(i->name, ModbusMasterScratchBuf, 1024);
                if( status != true ) {
                    logInfo("(%d)read file failed, status=%d", __LINE__, status);
                } else {
                    logInfo("(%s:%d)loading File: %s", __func__, __LINE__, i->name );
                }

                parse( json_value, ModbusMasterScratchBuf );

                std::string id = json_value["id"].get<std::string>().c_str();
                VirtualCommandMap[id].constant = atof(json_value["constant"].get<std::string>().c_str());
                VirtualCommandMap[id].opl = json_value["opl"].get<std::string>().c_str();
                VirtualCommandMap[id].opr = json_value["opr"].get<std::string>().c_str();
                VirtualCommandMap[id].op = json_value["op"].get<std::string>().c_str();
                logInfo("Processd virtual command file: id=%s", id.c_str());
                continue;
            }

            regType = REG_TYPE_NONE;

            if( (strncmp( i->name, "input", (strlen("input")-1)) == 0) ) {
                regType = REG_TYPE_INPUT;
            } else if( (strncmp( i->name, "output", (strlen("output")-1)) == 0) ) {
                regType = REG_TYPE_OUTPUT;
            } else if( (strncmp( i->name, "vinput", (strlen("vinput")-1)) == 0) ) {
                regType = REG_TYPE_VINPUT;
            } else if( (strncmp( i->name, "voutput", (strlen("voutput")-1)) == 0) ) {
                regType = REG_TYPE_VOUTPUT;
            }

            if( regType != REG_TYPE_NONE ) {

                status = GLOBAL_mdot->readUserFile(i->name, ModbusMasterScratchBuf, 1024);
                if( status != true ) {
                    logInfo("(%d)read file failed, status=%d", __LINE__, status);
                } else {
                    logInfo("(%s:%d)loading File: %s", __func__, __LINE__, i->name );
                }

                parse( json_value, ModbusMasterScratchBuf );

                std::string id = json_value["id"].get<std::string>().c_str();
                ModbusRegisterMap[id].name = json_value["name"].get<std::string>().c_str();
                ModbusRegisterMap[id].units = json_value["units"].get<std::string>().c_str();
                ModbusRegisterMap[id].min = atof(json_value["min"].get<std::string>().c_str());
                ModbusRegisterMap[id].max = atof(json_value["max"].get<std::string>().c_str());
                ModbusRegisterMap[id].node = atoi(json_value["node"].get<std::string>().c_str());
                ModbusRegisterMap[id].reg = atoi(json_value["reg"].get<std::string>().c_str());
                ModbusRegisterMap[id].rtype = atoi(json_value["rtype"].get<std::string>().c_str());
                ModbusRegisterMap[id].type = atoi(json_value["type"].get<std::string>().c_str());
                ModbusRegisterMap[id].size = atoi(json_value["size"].get<std::string>().c_str());
                ModbusRegisterMap[id].order = atoi(json_value["order"].get<std::string>().c_str());
                ModbusRegisterMap[id].fmt = json_value["fmt"].get<std::string>().c_str();
                ModbusRegisterMap[id].rfreq = atoi(json_value["rfreq"].get<std::string>().c_str());
                ModbusRegisterMap[id].regType = regType;
                ModbusRegisterMap[id].simulated = false;
                ModbusRegisterMap[id].valid = true;
                if( (regType == REG_TYPE_VINPUT) || (regType == REG_TYPE_VOUTPUT) ) {
                    ModbusRegisterMap[id].vcmd = json_value["vcmd"].get<std::string>().c_str();
                }
            }
        }

        osSignalSet(mainThreadId, sig_output_continue);

        // read modbus registers that have been configured.
        while ( true ) {

            std::map<std::string, ModbusRegister>::iterator iter;
            for (iter = ModbusRegisterMap.begin(); iter != ModbusRegisterMap.end(); ++iter) {
                if( iter->second.simulated == true ) {
                    std::map<std::string, SimulateInput>::iterator simiter;
                    for (simiter = SimulateInputMap.begin(); simiter != SimulateInputMap.end(); ++simiter) {
//                        logInfo("simulating input=%s, min=%2.2f, max=%2.2f, start_value=%2.2f, up_step=%2.2f, down_step=%2.2f",iter->first.c_str(), SimulateInputMap[iter->first].min, SimulateInputMap[iter->first].max, SimulateInputMap[iter->first].start_value, SimulateInputMap[iter->first].up_step, SimulateInputMap[iter->first].down_step);
                        if( (SimulateInputMap[iter->first].min == 0) && (SimulateInputMap[iter->first].max == 0) ) {
                            ModbusRegisterMap[iter->first].float_value = SimulateInputMap[iter->first].start_value;
                        } else {
                            if( ModbusRegisterMap[iter->first].float_value >= SimulateInputMap[iter->first].max ) {
                                move_up = false;
                                ModbusRegisterMap[iter->first].float_value = ModbusRegisterMap[iter->first].float_value - SimulateInputMap[iter->first].down_step;
                            } else if( ModbusRegisterMap[iter->first].float_value <= SimulateInputMap[iter->first].min ) {
                                move_up = true;
                                ModbusRegisterMap[iter->first].float_value = ModbusRegisterMap[iter->first].float_value + SimulateInputMap[iter->first].up_step;
                            } else {
                                if( move_up == true ) {
                                    ModbusRegisterMap[iter->first].float_value = ModbusRegisterMap[iter->first].float_value + SimulateInputMap[iter->first].up_step;
                                } else {
                                    ModbusRegisterMap[iter->first].float_value = ModbusRegisterMap[iter->first].float_value - SimulateInputMap[iter->first].down_step;
                                }
                            }
                            logInfo("simulating input=%s, value=%2.2f",iter->first.c_str(), ModbusRegisterMap[iter->first].float_value);
                        }
                    }
                    continue;
                }

                if( iter->second.node != 0 ) {

                    logInfo("Processing Input: tag=%s, node=%d, reg=%d, size=%d, order=%d", iter->first.c_str(), iter->second.node, iter->second.reg, iter->second.size, iter->second.order );
                    SendModbusCommand(iter->second.node, iter->second.reg, iter->second.size);
                    Thread::wait(30);
                    switch( iter->second.type ) {
                        case TYPE_32BIT_FLOAT:
                            float float_value;
                            status = ReadModbus_32bit_float( &float_value, iter->second.order );
                            if( status == true ) {
                                ModbusRegisterMap[iter->first].float_value = float_value;
                                ModbusRegisterMap[iter->first].valid = true;
//                                logInfo("Modbus Tag:%s value=%2.2f", iter->first.c_str(), float_value );
                            } else {
                                ModbusRegisterMap[iter->first].valid = false;
//                                logInfo("Modbus Read Failed, tag=%s", iter->first.c_str() );
                            }
                            break;
                        case TYPE_32BIT_INT:
                            break;
                        case TYPE_32BIT_UINT:
                            break;
                        case TYPE_16BIT_INT:
                            break;
                        case TYPE_16BIT_UINT:
                            break;
                        default:
                            break;
                    }
                }

                if( (iter->second.node == 0) && (iter->second.regType == REG_TYPE_OUTPUT) ) {
//                    logInfo("processing PIN output=%s, reg=%d, value=%d",iter->first.c_str(), ModbusRegisterMap[iter->first].reg, (bool)ModbusRegisterMap[iter->first].float_value);
                    if( ModbusRegisterMap[iter->first].reg == 1 ) {
                        dout1 = (bool)ModbusRegisterMap[iter->first].float_value;
                    } else {
                        dout2 = (bool)ModbusRegisterMap[iter->first].float_value;
                    }
                }
                if( (iter->second.node == 0) && (iter->second.regType == REG_TYPE_INPUT) ) {
//                    logInfo("processing PIN input=%s, reg=%d, value=%d",iter->first.c_str(), ModbusRegisterMap[iter->first].reg, (bool)ModbusRegisterMap[iter->first].float_value);
                    if( ModbusRegisterMap[iter->first].reg == 1 ) {
                        // digital input
                        ModbusRegisterMap[iter->first].float_value = (float)flow_switch.read();
                    } else {
                        ModbusRegisterMap[iter->first].float_value = (float)dinp2.read();
                    }
                }
            }

            // now update all of the virtual registers
            for (iter = ModbusRegisterMap.begin(); iter != ModbusRegisterMap.end(); ++iter) {
                if( (ModbusRegisterMap[iter->first].regType != REG_TYPE_VINPUT) && (ModbusRegisterMap[iter->first].regType != REG_TYPE_VOUTPUT) ) {
                    continue;
                }
                logInfo("Processing Virtual Input: vcmd:%s, tag:%s, opl:%s, opr:%s, op:%s, constant:%.4f", iter->second.vcmd.c_str(), iter->first.c_str(),
                        VirtualCommandMap[iter->second.vcmd].opl.c_str(),
                        VirtualCommandMap[iter->second.vcmd].opr.c_str(),
                        VirtualCommandMap[iter->second.vcmd].op.c_str(),
                        VirtualCommandMap[iter->second.vcmd].constant
                       );
                switch( VirtualCommandMap[iter->second.vcmd].op.c_str()[0] ) {
                    case '=':
                        ModbusRegisterMap[iter->first].float_value = ModbusRegisterMap[VirtualCommandMap[iter->second.vcmd].opl].float_value;
                        logInfo("Setting tag=%s, equal to (value=%2.2f)", iter->first.c_str(),
                                VirtualCommandMap[iter->second.vcmd].opl.c_str(),
                                ModbusRegisterMap[VirtualCommandMap[iter->second.vcmd].opl].float_value
                               );
                        break;
                    case '*':
                        ModbusRegisterMap[iter->first].float_value =
                            ModbusRegisterMap[VirtualCommandMap[iter->second.vcmd].opl].float_value *
                            ModbusRegisterMap[VirtualCommandMap[iter->second.vcmd].opr].float_value;
                        logInfo("Setting tag=%s, equal to (%2.2f*%2.2f) = %2.2f", iter->first.c_str(),
                                VirtualCommandMap[iter->second.vcmd].opl.c_str(),
                                ModbusRegisterMap[VirtualCommandMap[iter->second.vcmd].opl].float_value,
                                ModbusRegisterMap[VirtualCommandMap[iter->second.vcmd].opr].float_value,
                                ModbusRegisterMap[iter->first].float_value
                               );
                    case '/':
                        if( ModbusRegisterMap[VirtualCommandMap[iter->second.vcmd].opr].float_value != 0 ) {
                            ModbusRegisterMap[iter->first].float_value =
                                ModbusRegisterMap[VirtualCommandMap[iter->second.vcmd].opl].float_value /
                                ModbusRegisterMap[VirtualCommandMap[iter->second.vcmd].opr].float_value;
                            logInfo("Setting tag=%s, equal to (%2.2f/%2.2f) = %2.2f", iter->first.c_str(),
                                    VirtualCommandMap[iter->second.vcmd].opl.c_str(),
                                    ModbusRegisterMap[VirtualCommandMap[iter->second.vcmd].opl].float_value,
                                    ModbusRegisterMap[VirtualCommandMap[iter->second.vcmd].opr].float_value,
                                    ModbusRegisterMap[iter->first].float_value
                                   );
                        } else {
                            logInfo("NOT DOING DIVIDE BY ZERO");
                        }
                        break;
                    case '+':
                    case '-':
                    default:
                        break;
                }
            }

            osEvent evt = ModbusMasterMailBox.get(50);
            if (evt.status == osEventMail) {
                Message_t *mail = (Message_t*)evt.value.p;
                logInfo("Mail Received: Action: %d, New Input File: %s", mail->action, mail->controlFile);
                ModbusMasterMailBox.free(mail);
                ModbusRegisterMap.clear();
                break;
            }
            Thread::wait(5000);
        }
    }
}


volatile char       modbus_buffer_char;
volatile bool       modbus_interrupt_complete = false;

uint8_t             modbus_input_buffer[SIZE_MB_BUFFER];// 1byte address + 1 byte function +1 byte number of regs + 12 bytes of data + 2 bytes for crc response frame from slave
volatile uint8_t    modbus_input_buffer_counter = 0;

//Frame crc calucation
uint16_t modbus_crc(uint8_t* buf, int len)
{
    uint16_t crc = 0xFFFF;

    for (int pos = 0; pos < len; pos++) {
        crc ^= (uint16_t)buf[pos];          // XOR byte into least sig. byte of crc

        for (int i = 8; i != 0; i--) {
            // Loop over each bit
            if ((crc & 0x0001) != 0) {
                // If the LSB is set
                crc >>= 1;                    // Shift right and XOR 0xA001
                crc ^= 0xA001;
            } else                          // Else LSB is not set
                crc >>= 1;                    // Just shift right
        }
    }
    // Note, this number has low and high bytes swapped, so use it accordingly (or swap bytes)
    return crc;
}

RawSerial modbus(PA_2, PA_3);
DigitalOut mod_de(PB_0);
DigitalOut mod_re(PB_1);

void modbus_init( uint16_t baudRate )
{
    modbus.baud(baudRate);
    modbus.attach(&modbus_recv, RawSerial::RxIrq);
}

//call back when character goes into RX buffer for RS485 modbus
void modbus_recv()
{

    if (modbus.readable()) {
        modbus_buffer_char = modbus.getc();
        if (modbus_input_buffer_counter == 0 && modbus_buffer_char == 0x00) {
            modbus_input_buffer_counter = 0;
        } else {
            modbus_input_buffer[modbus_input_buffer_counter] = modbus_buffer_char;
            modbus_input_buffer_counter++;
        }
    }

    if (modbus_input_buffer_counter > modbus_input_buffer[2] + 4) {
        modbus_interrupt_complete = true;
        modbus_input_buffer_counter = 0;
    }
}

// Read modbus master frame
void SendModbusCommand(uint8_t slave_address, uint16_t firstReg, uint16_t noRegs)
{
    uint8_t L1V[8] = {slave_address, 0x04, 0x00, 0x02, 0x00, 0x02, 0xD1, 0x16};

    L1V[2] = (firstReg >> 8) & 0xFF;
    L1V[3] = firstReg & 0xFF;
    L1V[4] = (noRegs >> 8) & 0xFF;
    L1V[5] = noRegs & 0xFF;
    L1V[6] = modbus_crc(L1V,6) & 0xFF;
    L1V[7] = (modbus_crc(L1V,6)>>8) & 0xFF;

    mod_de = 1;
    mod_re = 1;
    Thread::wait(1);
    for (uint8_t i = 0; i < 8; i++)
        modbus.putc(L1V[i]);

    Thread::wait(2);
    mod_de = 0;
    mod_re = 0;

}

bool mbInterruptComplete()
{
    if (modbus_interrupt_complete) {
        modbus_interrupt_complete = false;
        return true;
    } else {
        return false;
    }
}

bool ReadModbus_32bit_float( float *float_value, int order )
{
    MR_REGISTER_32_BIT_FLOAT value;

    if (mbInterruptComplete() != true ) {
        return false;
    }

    switch( order ) {
        case BigEndian:
            value.b.lo_lo = modbus_input_buffer[6];
            value.b.lo_hi = modbus_input_buffer[5];
            value.b.hi_lo = modbus_input_buffer[4];
            value.b.hi_hi = modbus_input_buffer[3];
            break;
        case BigEndianReverseWord:
            value.b.lo_lo = modbus_input_buffer[4];
            value.b.lo_hi = modbus_input_buffer[3];
            value.b.hi_lo = modbus_input_buffer[6];
            value.b.hi_hi = modbus_input_buffer[5];
            break;
        default:
            return false;
    }
    *float_value = value.f;
    return true;
}