File content as of revision 214:52ef35bc44ec:

/******************************************************************************
 *
 * 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 "ModbusMasterApi.h"
#include "mod.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;

std::map< std::string, std::map<std::string, VirtualCommand> > VirtualCommandMap;

void LoadModbusConfigFile( char *fileName );
void UpdateSimulatedInput( std::map<std::string, ModbusRegister>::iterator &modMap );
void ReadModbusRegister( std::map<std::string, ModbusRegister>::iterator &modMap );
void ExecuteRegisterCommand(  std::map<std::string, ModbusRegister>::iterator &modMap  );
void UpdateOutputRegister( std::map<std::string, ModbusRegister>::iterator &modMap );

char ModbusMasterScratchBuf[MAX_FILE_SIZE];
void ModbusMaster(void const *args)
{
    logInfo("%s ModbusMaster has started...", __func__);
    bool SignaledMain = false;
    std::map<std::string, ModbusRegister>::iterator modMap;

    mod_init();
    DigitalOut mod_power(PA_11);
    mod_power = 0; // provide power to the modbus

    std::vector<mDot::mdot_file>::iterator file;
    std::vector<mDot::mdot_file> file_list;
    file_list = GLOBAL_mdot->listUserFiles();
    for(file = file_list.begin(); file != file_list.end(); ++file) {
        LoadModbusConfigFile( file->name );
    }

    while ( true ) {

        for (modMap = ModbusRegisterMap.begin(); modMap != ModbusRegisterMap.end(); ++modMap) {
            if( modMap->second.simulated == true ) {
                UpdateSimulatedInput( modMap );
                continue;
            } else if( modMap->second.node != 0 ) {
                ReadModbusRegister( modMap );
            } else if( (modMap->second.node == 0) && (modMap->second.regType == REG_TYPE_INPUT) ) {
//                logInfo("processing PIN input=%s, reg=%d, value=%d",modMap->first.c_str(), ModbusRegisterMap[modMap->first].reg, (bool)ModbusRegisterMap[modMap->first].float_value);
                if( ModbusRegisterMap[modMap->first].reg == 1 ) {
                    // digital input
                    ModbusRegisterMap[modMap->first].float_value = (float)flow_switch.read();
                } else {
                    ModbusRegisterMap[modMap->first].float_value = (float)dinp2.read();
                }
            }
        }

        // now that all of the modbus registers are updated we can execute the register commands.
        for (modMap = ModbusRegisterMap.begin(); modMap != ModbusRegisterMap.end(); ++modMap) {
            ExecuteRegisterCommand( modMap );
        }

        // now that all of the inputs and virtuals have been updated, go through the outputs.
        for (modMap = ModbusRegisterMap.begin(); modMap != ModbusRegisterMap.end(); ++modMap) {
            if( modMap->second.regType == REG_TYPE_OUTPUT ) {
                UpdateOutputRegister( modMap );
            }
        }

        osEvent evt = ModbusMasterMailBox.get(50);
        if (evt.status == osEventMail) {
            Message_t *mail = (Message_t*)evt.value.p;
            printf("Mail Received: Action: %d, New Input File: %s\r\n", mail->action, mail->controlFile);
            if( mail->action == ACTION_EXEC_CMD ) {
                ModbusMasterExecCmd( mail->controlFile );
            } else {
                LoadModbusConfigFile( mail->controlFile );
            }
            ModbusMasterMailBox.free(mail);
        }
        if( SignaledMain == false ) {
            SignaledMain = true;
            osSignalSet(mainThreadId, sig_output_continue);
        }
        Thread::wait(5000);
    }
}

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

    switch( order ) {
        case BigEndian:
            value.b.lo_lo = rd_buf[3];
            value.b.lo_hi = rd_buf[2];
            value.b.hi_lo = rd_buf[1];
            value.b.hi_hi = rd_buf[0];
            break;
        case BigEndianReverseWord:
            value.b.lo_lo = rd_buf[1];
            value.b.lo_hi = rd_buf[0];
            value.b.hi_lo = rd_buf[3];
            value.b.hi_hi = rd_buf[2];
            break;
        default:
            printf("%s:%d: order not supported\r\n",__func__,__LINE__);
            return false;
    }
//    printf("0x%x 0x%x 0x%x 0x%x (%2.4f)\r\n", value.b.hi_hi, value.b.hi_lo, value.b.lo_hi, value.b.lo_lo, value.f);
    *float_value = value.f;
    return true;
}

bool WriteModbus_32bit_float( float float_value, int order, unsigned char *xmt_buf )
{
    MR_REGISTER_32_BIT_FLOAT value;

    value.f = float_value;

    switch( order ) {
        case BigEndian:
            xmt_buf[3] = value.b.lo_lo;
            xmt_buf[2] = value.b.lo_hi;
            xmt_buf[1] = value.b.hi_lo;
            xmt_buf[0] = value.b.hi_hi;
            break;
        case BigEndianReverseWord:
            xmt_buf[1] = value.b.lo_lo;
            xmt_buf[0] = value.b.lo_hi;
            xmt_buf[3] = value.b.hi_lo;
            xmt_buf[2] = value.b.hi_hi;
            break;
        default:
            printf("%s:%d: order not supported\r\n",__func__,__LINE__);
            return false;
    }
    printf("%s:%d: 0x%x 0x%x 0x%x 0x%x (%2.4f)\r\n",__func__,__LINE__, value.b.hi_hi, value.b.hi_lo, value.b.lo_hi, value.b.lo_lo, value.f);
    return true;
}

bool ReadModbus_32bit_int( int32_t *int32_value, int order, unsigned char *rd_buf )
{
    MR_REGISTER_32BIT_INT value;

    switch( order ) {
        case BigEndian:
            value.b.lo_lo = rd_buf[3];
            value.b.lo_hi = rd_buf[2];
            value.b.hi_lo = rd_buf[1];
            value.b.hi_hi = rd_buf[0];
            break;
        case BigEndianReverseWord:
            value.b.lo_lo = rd_buf[1];
            value.b.lo_hi = rd_buf[0];
            value.b.hi_lo = rd_buf[3];
            value.b.hi_hi = rd_buf[2];
            break;
        default:
            printf("%s:%d: order not supported\r\n",__func__,__LINE__);
            return false;
    }
//    printf("0x%x 0x%x 0x%x 0x%x (%d)\r\n", value.b.hi_hi, value.b.hi_lo, value.b.lo_hi, value.b.lo_lo, value.i);
    *int32_value = value.i;
    return true;
}

bool WriteModbus_32bit_int( int32_t int32_value, int order, unsigned char *xmt_buf )
{
    MR_REGISTER_32BIT_INT value;

    value.i = int32_value;

    switch( order ) {
        case BigEndian:
            xmt_buf[3] = value.b.lo_lo;
            xmt_buf[2] = value.b.lo_hi;
            xmt_buf[1] = value.b.hi_lo;
            xmt_buf[0] = value.b.hi_hi;
            break;
        case BigEndianReverseWord:
            xmt_buf[1] = value.b.lo_lo;
            xmt_buf[0] = value.b.lo_hi;
            xmt_buf[3] = value.b.hi_lo;
            xmt_buf[2] = value.b.hi_hi;
            break;
        default:
            printf("%s:%d: order not supported\r\n",__func__,__LINE__);
            return false;
    }
    printf("%s:%d: 0x%x 0x%x 0x%x 0x%x (%d)\r\n", __func__, __LINE__, value.b.hi_hi, value.b.hi_lo, value.b.lo_hi, value.b.lo_lo, value.i);
    return true;
}

bool WriteModbus_16bit_int( int16_t int16_value, int order, unsigned char *xmt_buf )
{
    MR_REGISTER_16BIT_INT value;

    value.w = int16_value;

    switch( order ) {
        case BigEndian:
        case BigEndianReverseWord:
            xmt_buf[1] = value.b.lo;
            xmt_buf[0] = value.b.hi;
            break;
        default:
            printf("%s:%d: order not supported\r\n",__func__,__LINE__);
            return false;
    }
    printf("%s:%d: 0x%x 0x%x (%d)\r\n", __func__,__LINE__, value.b.hi, value.b.lo, value.w);
    return true;
}

void ModbusMasterExecCmd( char *cmd )
{
    MbedJSONValue json_cmd;
    bool status;
    int ret;
    unsigned char rd_buf[16];

//    printf("%s:%d: command=%s\r\n", __func__, __LINE__, cmd );

    parse( json_cmd, cmd );

    std::string id = json_cmd["id"].get<std::string>().c_str();
    int node = atoi(json_cmd["node"].get<std::string>().c_str());
    int func = atoi(json_cmd["func"].get<std::string>().c_str());
    int sreg = atoi(json_cmd["sreg"].get<std::string>().c_str());
    int nreg = atoi(json_cmd["nreg"].get<std::string>().c_str());
    int dtype = atoi(json_cmd["dtype"].get<std::string>().c_str());
    int order = atoi(json_cmd["order"].get<std::string>().c_str());
    float value = atof(json_cmd["value"].get<std::string>().c_str());

    switch( func ) {
        case MOD_FUNC_GET_HREG: // read holding register
        case MOD_FUNC_GET_IREG: // read input register
            ret = mod_read(node, func, sreg, nreg, rd_buf);
            switch( dtype ) {
                case TYPE_32BIT_FLOAT: {
                    float float_value;
                    if( ret != MOD_ERROR_NONE ) {
                        printf("CMD: %s:%d: %s failed, errflag=%d\r\n", __func__, __LINE__,  id.c_str(), ret );
                        break;
                    }
                    status = ReadModbus_32bit_float( &float_value, order, rd_buf );
                    if( status == true ) {
                        printf("CMD: %s:%d: %s value=%2.4f\r\n", __func__, __LINE__,  id.c_str(), float_value );
                    } else {
                        printf("CMD: %s:%d: %s failed\r\n", __func__, __LINE__, id.c_str() );
                    }
                    break;
                }
                case TYPE_32BIT_INT:
                case TYPE_32BIT_UINT: {
                    int32_t int32_value;
                    if( ret != MOD_ERROR_NONE ) {
                        printf("CMD: %s:%d: %s failed, errflag=%d\r\n", __func__, __LINE__,  id.c_str(), ret );
                        break;
                    }
                    status = ReadModbus_32bit_int( &int32_value, order, rd_buf );
                    if( status == true ) {
                        printf("CMD: %s:%d: %s value=%d\r\n", __func__, __LINE__,  id.c_str(), int32_value );
                    } else {
                        printf("CMD: %s:%d: %s failed\r\n", __func__, __LINE__, id.c_str() );
                    }
                    break;
                }
                case TYPE_16BIT_INT:
                case TYPE_16BIT_UINT:
                    break;
                default:
                    break;
            }
            break;
        case MOD_FUNC_SET_HREG:  // write holding register
        case MOD_FUNC_SET_HREGS: // write multiple registers (only supports 2 right now)
        case MOD_FUNC_SET_COIL:  { // write coil
            unsigned char xmt_buf[10];
            switch( dtype ) {
                case TYPE_32BIT_FLOAT: {
                    status = WriteModbus_32bit_float( value, order, xmt_buf );
                    if( status != true ) {
                        printf("CMD: %s:%d: %s failed\r\n", __func__, __LINE__, id.c_str() );
                        return;
                    }
                    printf("%s:%d: 0x%x 0x%x 0x%x 0x%x\r\n", __func__,__LINE__, xmt_buf[0], xmt_buf[1], xmt_buf[2], xmt_buf[3]);
                    break;
                }
                case TYPE_32BIT_INT:
                case TYPE_32BIT_UINT: {
                    status = WriteModbus_32bit_int( (int32_t)value, order, xmt_buf );
                    if( status != true ) {
                        printf("CMD: %s:%d:  %s failed\r\n", __func__, __LINE__, id.c_str() );
                        return;
                    }
                    break;
                }
                case TYPE_16BIT_INT:
                case TYPE_16BIT_UINT:
                    status = WriteModbus_16bit_int( (int16_t)value, order, xmt_buf );
                    if( status != true ) {
                        printf("CMD: %s:%d: %s failed\r\n", __func__, __LINE__, id.c_str() );
                        return;
                    }
                    printf("%s:%d: 0x%x 0x%x\r\n", __func__,__LINE__, xmt_buf[0], xmt_buf[1]);
                    break;
                default:
                    printf("CMD: %s:%d: %s NOT IMPLEMENTED\r\n", __func__, __LINE__, id.c_str() );
                    return;
            }
            printf("%s:%d: 0x%x 0x%x 0x%x 0x%x\r\n", __func__,__LINE__, xmt_buf[0], xmt_buf[1], xmt_buf[2], xmt_buf[3]);
            ret = mod_write(node, func, sreg, nreg, xmt_buf);
            if( ret != MOD_ERROR_NONE ) {
                printf("CMD: %s:%d: %s failed, errflag=%d\r\n", __func__, __LINE__, id.c_str(), ret );
            } else {
                printf("CMD: %s:%d: %s wrote to modbus func=%d reg=%d value=%2.4f, errflag=%d\r\n", __func__, __LINE__, id.c_str(), func, sreg, value, ret );
            }
            break;
        }
        default:
            printf("CMD: %s:%d: %s failed, errflag=%d\r\n", __func__, __LINE__, id.c_str(), ret );
            break;
    }
}

void LoadModbusConfigFile( char *fileName )
{
    bool status;
    RegisterType_t regType;

    if( (strncmp( fileName, "vcmd", (strlen("vcmd")-1)) == 0) ) {
        status = GLOBAL_mdot->readUserFile(fileName, ModbusMasterScratchBuf, MAX_FILE_SIZE);
        if( status != true ) {
            printf("(%d)read file failed, status=%d\r\n", __LINE__, status);
            return;
        } else {
            printf("(%s:%d)loading File: %s\r\n", __func__, __LINE__, fileName );
        }
        MbedJSONValue json_value;
        parse( json_value, ModbusMasterScratchBuf );

        std::string tag = json_value["tag"].get<std::string>().c_str();
        std::string id = json_value["id"].get<std::string>().c_str();
        VirtualCommandMap[tag][id].c = atof(json_value["c"].get<std::string>().c_str());
        VirtualCommandMap[tag][id].tag = json_value["tag"].get<std::string>().c_str();
        VirtualCommandMap[tag][id].opl = json_value["opl"].get<std::string>().c_str();
        VirtualCommandMap[tag][id].opr = json_value["opr"].get<std::string>().c_str();
        VirtualCommandMap[tag][id].op = json_value["op"].get<std::string>().c_str();
        printf("Processd virtual command file: id=%s, tag=%s\r\n", id.c_str(), tag.c_str());
        return;
    }

    regType = REG_TYPE_NONE;

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

    if( regType != REG_TYPE_NONE ) {
        MbedJSONValue json_value;
        status = GLOBAL_mdot->readUserFile(fileName, ModbusMasterScratchBuf, MAX_FILE_SIZE);
        if( status != true ) {
            logInfo("(%d)read file failed, status=%d", __LINE__, status);
            return;
        } else {
            logInfo("(%s:%d)loading File: %s", __func__, __LINE__, fileName );
        }

        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].errflag = 0;
        if( (regType == REG_TYPE_VINPUT) || (regType == REG_TYPE_VOUTPUT) ) {
            ModbusRegisterMap[id].vcmd = json_value["vcmd"].get<std::string>().c_str();
        }
    }
    return;
}

void UpdateSimulatedInput( std::map<std::string, ModbusRegister>::iterator &modMap )
{
    ModbusRegisterMap[modMap->first].errflag = 0;
//    printf("\r\nsimulating input=%s, min=%2.4f, max=%2.4f, start_value=%2.4f, up_step=%2.4f, down_step=%2.4f moving_up=%d\r\n",modMap->first.c_str(), SimulateInputMap[modMap->first].min, SimulateInputMap[modMap->first].max, SimulateInputMap[modMap->first].start_value, SimulateInputMap[modMap->first].up_step, SimulateInputMap[modMap->first].down_step, SimulateInputMap[modMap->first].moving_up);
    if( (SimulateInputMap[modMap->first].min == 0) && (SimulateInputMap[modMap->first].max == 0) ) {
        ModbusRegisterMap[modMap->first].float_value = SimulateInputMap[modMap->first].start_value;
//    printf("simulating input=%s, value=%2.4f\r\n",modMap->first.c_str(), ModbusRegisterMap[modMap->first].float_value);
    } else {
        if( ModbusRegisterMap[modMap->first].float_value >= SimulateInputMap[modMap->first].max ) {
            SimulateInputMap[modMap->first].moving_up = false;
//            printf("simulating down input=%s, value=%2.4f - %2.4f\r\n",modMap->first.c_str(), ModbusRegisterMap[modMap->first].float_value, SimulateInputMap[modMap->first].down_step);
            ModbusRegisterMap[modMap->first].float_value = ModbusRegisterMap[modMap->first].float_value - SimulateInputMap[modMap->first].down_step;
        } else if( ModbusRegisterMap[modMap->first].float_value <= SimulateInputMap[modMap->first].min ) {
            SimulateInputMap[modMap->first].moving_up = true;
//            printf("simulating up input=%s, value=%2.4f + %2.4f\r\n",modMap->first.c_str(), ModbusRegisterMap[modMap->first].float_value, SimulateInputMap[modMap->first].up_step);
            ModbusRegisterMap[modMap->first].float_value = ModbusRegisterMap[modMap->first].float_value + SimulateInputMap[modMap->first].up_step;
        } else {
            if( SimulateInputMap[modMap->first].moving_up == true ) {
//                printf("continue simulate up input=%s, value=%2.4f + %2.4f\r\n",modMap->first.c_str(), ModbusRegisterMap[modMap->first].float_value, SimulateInputMap[modMap->first].up_step);
                ModbusRegisterMap[modMap->first].float_value = ModbusRegisterMap[modMap->first].float_value + SimulateInputMap[modMap->first].up_step;
            } else {
//                printf("continue simulate down input=%s, value=%2.4f - %2.4f\r\n",modMap->first.c_str(), ModbusRegisterMap[modMap->first].float_value, SimulateInputMap[modMap->first].down_step);
                ModbusRegisterMap[modMap->first].float_value = ModbusRegisterMap[modMap->first].float_value - SimulateInputMap[modMap->first].down_step;
            }
        }
//        printf("simulating input=%s, value=%2.4f\r\n\r\n",modMap->first.c_str(), ModbusRegisterMap[modMap->first].float_value);
    }
}

void ReadModbusRegister( std::map<std::string, ModbusRegister>::iterator &modMap )
{
    bool status;
    unsigned char rd_buf[16];
    memset( rd_buf, 0, 16 );
    logInfo("Processing Input: tag=%s, node=%d, reg=%d, size=%d, order=%d", modMap->first.c_str(), modMap->second.node, modMap->second.reg, modMap->second.size, modMap->second.order );
    int ret = mod_read(modMap->second.node, modMap->second.rtype, modMap->second.reg, modMap->second.size, rd_buf);
    switch( modMap->second.type ) {
        case TYPE_32BIT_FLOAT:
            float float_value;
            if( ret != MOD_ERROR_NONE ) {
                ModbusRegisterMap[modMap->first].errflag = ret;
                break;
            }
            status = ReadModbus_32bit_float( &float_value, modMap->second.order, rd_buf );
            if( status == true ) {
                ModbusRegisterMap[modMap->first].float_value = float_value;
                ModbusRegisterMap[modMap->first].errflag = 0;
//                                logInfo("Modbus Tag:%s value=%2.4f", modMap->first.c_str(), float_value );
            } else {
                ModbusRegisterMap[modMap->first].errflag = 1000;
//                                logInfo("Modbus Read Failed, tag=%s", modMap->first.c_str() );
            }
            break;
        case TYPE_32BIT_INT:
            int32_t int32_value;
            if( ret != MOD_ERROR_NONE ) {
                ModbusRegisterMap[modMap->first].errflag = ret;
                break;
            }
            status = ReadModbus_32bit_int( &int32_value, modMap->second.order, rd_buf );
            if( status == true ) {
                ModbusRegisterMap[modMap->first].float_value = int32_value;
                ModbusRegisterMap[modMap->first].errflag = 0;
                logInfo("Modbus Tag:%s value=%2.4f", modMap->first.c_str(), ModbusRegisterMap[modMap->first].float_value );
            } else {
                ModbusRegisterMap[modMap->first].errflag = 1000;
                logInfo("Modbus Read Failed, tag=%s", modMap->first.c_str() );
            }
            break;
        case TYPE_32BIT_UINT:
            break;
        case TYPE_16BIT_INT:
            break;
        case TYPE_16BIT_UINT:
            break;
        default:
            break;
    }
}

void ExecuteRegisterCommand( std::map<std::string, ModbusRegister>::iterator &modMap  )
{
    std::map<std::string, VirtualCommand>::iterator cmdMap;
    for (cmdMap = VirtualCommandMap[modMap->first].begin(); cmdMap != VirtualCommandMap[modMap->first].end(); ++cmdMap) {

//        printf("Executing Register Command: key:%s, id:%s, tag:%s, opl:%s, opr:%s, op:%s, constant:%.4f\r\n", modMap->first.c_str(), cmdMap->first.c_str(),
//               cmdMap->second.tag.c_str(),  cmdMap->second.opl.c_str(), cmdMap->second.opr.c_str(), cmdMap->second.op.c_str(), cmdMap->second.c );

        switch( cmdMap->second.op.c_str()[0] ) {
            case '=':
                if( cmdMap->second.opr.size() == 0 ) {
                    ModbusRegisterMap[cmdMap->second.tag].float_value = cmdMap->second.c;
                } else {
                    ModbusRegisterMap[cmdMap->second.tag].float_value = ModbusRegisterMap[cmdMap->second.opl].float_value;
                }
//                printf("Setting tag=%s, equal to (value=%2.4f)\r\n", cmdMap->second.tag.c_str(), ModbusRegisterMap[cmdMap->second.tag].float_value );
                break;
            case '*': {
                float opl = ModbusRegisterMap[cmdMap->second.opl].float_value;
                float opr = ModbusRegisterMap[cmdMap->second.opr].float_value;
                if( cmdMap->second.opr.size() == 0 ) {
                    ModbusRegisterMap[cmdMap->second.tag].float_value = ModbusRegisterMap[cmdMap->second.opl].float_value * cmdMap->second.c;
//                    printf("Setting tag=%s, equal to (%2.4f*%2.4f) = %2.4f\r\n", cmdMap->second.tag.c_str(), opl, cmdMap->second.c, ModbusRegisterMap[cmdMap->second.tag].float_value );
                } else {
                    ModbusRegisterMap[cmdMap->second.tag].float_value = ModbusRegisterMap[cmdMap->second.opl].float_value * ModbusRegisterMap[cmdMap->second.opr].float_value;
//                    printf("Setting tag=%s, equal to (%2.4f*%2.4f) = %2.4f\r\n", cmdMap->second.tag.c_str(), opl, opr, ModbusRegisterMap[cmdMap->second.tag].float_value );
                }
                break;
            }
            case '/': {
                float opl = ModbusRegisterMap[cmdMap->second.opl].float_value;
                float opr = ModbusRegisterMap[cmdMap->second.opr].float_value;
                if( cmdMap->second.opr.size() == 0 ) {
                    // constant operation
                    ModbusRegisterMap[cmdMap->second.tag].float_value = ModbusRegisterMap[cmdMap->second.opl].float_value / cmdMap->second.c;
//                    printf("Setting tag=%s, equal to (%2.4f/%2.4f) = %2.4f\r\n", cmdMap->second.tag.c_str(), opl, cmdMap->second.c, ModbusRegisterMap[cmdMap->second.tag].float_value );
                } else if( ModbusRegisterMap[cmdMap->second.opr].float_value != 0 ) {
                    ModbusRegisterMap[cmdMap->second.tag].float_value = ModbusRegisterMap[cmdMap->second.opl].float_value / ModbusRegisterMap[cmdMap->second.opr].float_value;
//                    printf("Setting tag=%s, equal to (%2.4f/%2.4f) = %2.4f\r\n", cmdMap->second.tag.c_str(), opl, opr, ModbusRegisterMap[cmdMap->second.tag].float_value );
                } else {
                    printf("NOT DOING DIVIDE BY ZERO\r\n");
                }
                break;
            }
            case '+':
            case '-':
            default:
                break;
        }
    }
}

void UpdateOutputRegister( std::map<std::string, ModbusRegister>::iterator &modMap )
{
    bool status;
    int ret;

    if( modMap->second.node == 0)  {
        if( ModbusRegisterMap[modMap->first].reg == 1 ) {
            dout1 = (bool)((int)ModbusRegisterMap[modMap->first].float_value&0x1);
        } else {
            dout2 = (bool)((int)ModbusRegisterMap[modMap->first].float_value&0x1);
        }
    } else {
        unsigned char xmt_buf[10];

        switch( ModbusRegisterMap[modMap->first].type ) {
            case TYPE_32BIT_FLOAT: {
                status = WriteModbus_32bit_float( ModbusRegisterMap[modMap->first].float_value, ModbusRegisterMap[modMap->first].order, xmt_buf );
                if( status != true ) {
                    printf("CMD: %s:%d: failed\r\n", __func__, __LINE__);
                    return;
                }
                printf("%s:%d: 0x%x 0x%x 0x%x 0x%x\r\n", __func__,__LINE__, xmt_buf[0], xmt_buf[1], xmt_buf[2], xmt_buf[3]);
                break;
            }
            case TYPE_32BIT_INT:
            case TYPE_32BIT_UINT: {
                status = WriteModbus_32bit_int( ((int32_t)ModbusRegisterMap[modMap->first].float_value&0x1), ModbusRegisterMap[modMap->first].order, xmt_buf );
                if( status != true ) {
                    printf("CMD: %s:%d: failed\r\n", __func__, __LINE__ );
                    return;
                }
                break;
            }
            case TYPE_16BIT_INT:
            case TYPE_16BIT_UINT:
                status = WriteModbus_16bit_int( ((int16_t)ModbusRegisterMap[modMap->first].float_value&0x1), ModbusRegisterMap[modMap->first].order, xmt_buf );
                if( status != true ) {
                    printf("CMD: %s:%d: failed\r\n", __func__, __LINE__ );
                    return;
                }
                printf("%s:%d: 0x%x 0x%x\r\n", __func__,__LINE__, xmt_buf[0], xmt_buf[1]);
                break;
            default:
                printf("CMD: %s:%d: NOT IMPLEMENTED\r\n", __func__, __LINE__ );
                return;
        }
        printf("%s:%d: 0x%x 0x%x 0x%x 0x%x\r\n", __func__,__LINE__, xmt_buf[0], xmt_buf[1], xmt_buf[2], xmt_buf[3]);
        ret = mod_write(ModbusRegisterMap[modMap->first].node, ModbusRegisterMap[modMap->first].rtype, ModbusRegisterMap[modMap->first].reg, ModbusRegisterMap[modMap->first].size, xmt_buf);
        if( ret != MOD_ERROR_NONE ) {
            printf("CMD: %s:%d: failed, errflag=%d\r\n", __func__, __LINE__, ret );
        } else {
            printf("CMD: %s:%d: wrote to modbus func=%d reg=%d value=%2.4f, errflag=%d\r\n", __func__, __LINE__, ModbusRegisterMap[modMap->first].rtype, ModbusRegisterMap[modMap->first].reg, ModbusRegisterMap[modMap->first].float_value, ret );
        }
    }
}