//Read OCR Result
int EnergyCam_GetResultOCRInt( uint32_t* pInt, uint16_t* pFrac) {
uint32_t readRegCnt;
const uint32_t regCnt = 3;
uint16_t inputRegs[regCnt];
if(NULL == pInt) return MODBUSERROR;
if(NULL == pFrac) return MODBUSERROR;
readRegCnt = modbus_read_input_registers(m_ctx, MODBUS_GET_INTERNAL_ADDR_FROM_OFFICIAL(MODBUS_SLAVE_INPUTREG_MEMMAP_RESULTOCRINTHIGH), regCnt, &inputRegs[0]);
if (readRegCnt != -1) {
uint32_t tmp = 0;
tmp = ((uint32_t)inputRegs[0]) << 16;
tmp |= inputRegs[1];
*pInt = tmp;
*pFrac = inputRegs[2];
return MODBUSOK;
} else {
fprintf(stderr,"EnergyCam_GetResultOCRInt failed \r\n");
return MODBUSERROR;
}
return MODBUSERROR;
}
//read Buildnumber of Firmware
int EnergyCam_GetAppFirmwareBuildNumber(uint32_t* pBuildNumber) {
uint32_t readRegCnt;
const uint32_t regCnt = 2;
uint16_t inputRegs[regCnt];
if(NULL == pBuildNumber)
return MODBUSERROR;
readRegCnt = modbus_read_input_registers(m_ctx, MODBUS_GET_INTERNAL_ADDR_FROM_OFFICIAL(MODBUS_COMMON_INPUTREG_MEMMAP_APPBUILDNUMBER1), regCnt, &inputRegs[0]);
if (readRegCnt != -1) {
uint32_t tmp = 0;
tmp = ((uint32_t)inputRegs[0]) << 16;
tmp |= inputRegs[1];
*pBuildNumber = tmp;
return MODBUSOK;
}
else {
fprintf(stderr,"EnergyCam_GetAppFirmwareBuildNumber failed \r\n");
return MODBUSERROR;
}
return MODBUSERROR;
}
MPSPickPlace1::MachineState MPSPickPlace1::getState() {
uint16_t rec[1] = {0};
int rc = modbus_read_input_registers(mb, 3, 1, rec);
if (rc == -1) {
machineState = DISCONNECTED;
}
if(rec[0] == 1) {
machineState = AVAILABLE;
return AVAILABLE;
}
else if(rec[0] == 2) {
machineState = PROCESSING;
return PROCESSING;
}
else if(rec[0] == 3) {
machineState = PROCESSED;
return PROCESSED;
}
else if(rec[0] == 4) {
machineState = DELIVER;
return DELIVER;
}
else if(rec[0] == 5) {
machineState = DELIVERED;
return DELIVERED;
}
else if(rec[0] == 6) {
machineState = RETRIEVED;
return RETRIEVED;
}
else {
machineState = IDLE;
return IDLE;
}
}
static int _ctx_read_regs(lua_State *L, bool input)
{
ctx_t *ctx = ctx_check(L, 1);
int addr = luaL_checknumber(L, 2);
int count = luaL_checknumber(L, 3);
int rcount = 0;
int rc;
if (count > MODBUS_MAX_READ_REGISTERS) {
return luaL_argerror(L, 3, "requested too many registers");
}
// better malloc as much space as we need to return data here...
uint16_t *buf = malloc(count * sizeof(uint16_t));
assert(buf);
if (input) {
rc = modbus_read_input_registers(ctx->modbus, addr, count, buf);
} else {
rc = modbus_read_registers(ctx->modbus, addr, count, buf);
}
if (rc == count) {
lua_newtable(L);
/* nota bene, lua style offsets! */
for (int i = 1; i <= rc; i++) {
lua_pushnumber(L, i);
lua_pushnumber(L, buf[i-1]);
lua_settable(L, -3);
}
rcount = 1;
} else {
rcount = libmodbus_rc_to_nil_error(L, rc, count);
}
free(buf);
return rcount;
}
int main( int argc, char **argv )
{
int a, rc;
int ec = 0;
options_t options;
modbus_t *ctx;
uint8_t *tab_bit;
uint16_t *tab_reg;
if(argc > 1)
{
options_init(&options);
for (a = 1; a < argc; a++)
{
options_execute(&options, argv[a], strlen(argv[a])+1);
rc = options_finish(&options);
DEBUG("argument: '%s' end state: %d\n",argv[a], rc);
if (rc == -1)
{
options_err_disp(&options,argv[a]);
ec = 1;
goto exit;
}
}
if(rc == 0)
{
fprintf(stderr, "Missing action argument\n");
ec = 1;
goto exit;
}
if(options.action == _UNDEF)
goto exit;
// Options are valid
options_dump(&options);
ctx = modbus_init_con(&options);
modbus_set_debug(ctx, options.debug);
modbus_set_response_timeout(ctx, &options.timeout);
if (modbus_connect(ctx) == -1) {
fprintf(stderr, "Connection failed: %s\n",
modbus_strerror(errno));
ec = 1;
goto destroy;
}
switch(options.action)
{
case _RC:
case _RD:
tab_bit = (uint8_t *) malloc(options.count * sizeof(uint8_t));
DBG_ASSERT(tab_bit != NULL, "Unable to allocate tab_bit!");
memset(tab_bit, 0, options.count * sizeof(uint8_t));
switch(options.action)
{
case _RC:
rc = modbus_read_bits(ctx, options.address,
options.count, tab_bit);
break;
case _RD:
rc = modbus_read_input_bits(ctx, options.address,
options.count, tab_bit);
break;
}
if (rc == -1) {
fprintf(stderr, "%s\n", modbus_strerror(errno));
ec = 1;
} else {
display_8bit(&options, tab_bit);
}
free(tab_bit);
break;
case _RH:
case _RI:
tab_reg = (uint16_t *) malloc(options.count * sizeof(uint16_t));
DBG_ASSERT(tab_reg != NULL, "Unable to allocate tab_reg!");
switch(options.action)
{
case _RH:
rc = modbus_read_registers(ctx, options.address,
options.count, tab_reg);
break;
case _RI:
rc = modbus_read_input_registers(ctx, options.address,
options.count, tab_reg);
break;
}
if (rc == -1) {
fprintf(stderr, "%s\n", modbus_strerror(errno));
ec = 1;
} else {
display_16bit(&options, tab_reg);
}
free(tab_reg);
break;
case _WC:
if(options.count == 1)
rc = modbus_write_bit(ctx, options.address,
options.coil_values[0]);
else {
rc = modbus_write_bits(ctx, options.address,
options.count, options.coil_values);
}
if (rc == -1) {
fprintf(stderr, "%s\n", modbus_strerror(errno));
ec = 1;
} else {
printf("Success\n");
}
break;
case _WH:
if(options.count == 1)
rc = modbus_write_register(ctx, options.address,
options.reg_values[0]);
else {
rc = modbus_write_registers(ctx, options.address,
options.count, options.reg_values);
}
if (rc == -1) {
fprintf(stderr, "%s\n", modbus_strerror(errno));
ec = 1;
} else {
printf("Success\n");
}
break;
default:
DBG_ASSERT(0,"Unhandled action enum constant!");
}
} else {
options_help();
ec = 1;
goto exit;
}
close:
modbus_close(ctx);
destroy:
modbus_free(ctx);
exit:
return ec;
}
int main(int argc, char **argv)
{
int c;
int ok;
int debug = 0;
BackendParams *backend = 0;
int slaveAddr = 1;
int startAddr = 100;
int startReferenceAt0 = 0;
int readWriteNo = 1;
int fType = FuncNone;
int timeout_ms = 1000;
int hasDevice = 0;
int isWriteFunction = 0;
enum WriteDataType {
DataInt,
Data8Array,
Data16Array
} wDataType = DataInt;
union Data {
int dataInt;
uint8_t *data8;
uint16_t *data16;
} data;
while (1) {
int option_index = 0;
static struct option long_options[] = {
{DebugOpt, no_argument, 0, 0},
{0, 0, 0, 0}
};
c = getopt_long(argc, argv, "a:b:d:c:m:r:s:t:p:o:0",
long_options, &option_index);
if (c == -1) {
break;
}
switch (c) {
case 0:
if (0 == strcmp(long_options[option_index].name, DebugOpt)) {
debug = 1;
}
break;
case 'a': {
slaveAddr = getInt(optarg, &ok);
if (0 == ok) {
printf("Slave address (%s) is not integer!\n\n", optarg);
printUsage(argv[0]);
exit(EXIT_FAILURE);
}
}
break;
case 'c': {
readWriteNo = getInt(optarg, &ok);
if (0 == ok) {
printf("# elements to read/write (%s) is not integer!\n\n", optarg);
printUsage(argv[0]);
exit(EXIT_FAILURE);
}
}
break;
case 'm':
if (0 == strcmp(optarg, TcpOptVal)) {
backend = createTcpBackend((TcpBackend*)malloc(sizeof(TcpBackend)));
}
else if (0 == strcmp(optarg, RtuOptVal))
backend = createRtuBackend((RtuBackend*)malloc(sizeof(RtuBackend)));
else {
printf("Unrecognized connection type %s\n\n", optarg);
printUsage(argv[0]);
exit(EXIT_FAILURE);
}
break;
case 'r': {
startAddr = getInt(optarg, &ok);
if (0 == ok) {
printf("Start address (%s) is not integer!\n\n", optarg);
printUsage(argv[0]);
exit(EXIT_FAILURE);
}
}
break;
case 't': {
fType = getInt(optarg, &ok);
if (0 == ok) {
printf("Function type (%s) is not integer!\n\n", optarg);
printUsage(argv[0]);
exit(EXIT_FAILURE);
}
}
break;
case 'o': {
timeout_ms = getInt(optarg, &ok);
if (0 == ok) {
printf("Timeout (%s) is not integer!\n\n", optarg);
printUsage(argv[0]);
exit(EXIT_FAILURE);
}
printf("Timeout set to %d\r\n", timeout_ms);
}
break;
case '0':
startReferenceAt0 = 1;
break;
//tcp/rtu params
case 'p':
case 'b':
case 'd':
case 's':
if (0 == backend) {
printf("Connection type (-m switch) has to be set before its params are provided!\n");
printUsage(argv[0]);
exit(EXIT_FAILURE);
}
else {
if (0 == backend->setParam(backend, c, optarg)) {
printUsage(argv[0]);
exit(EXIT_FAILURE);
}
}
break;
case '?':
break;
default:
printf("?? getopt returned character code 0%o ??\n", c);
}
}
if (0 == backend) {
printf("No connection type was specified!\n");
printUsage(argv[0]);
exit(EXIT_FAILURE);
}
if (1 == startReferenceAt0) {
startAddr--;
}
//choose write data type
switch (fType) {
case(ReadCoils):
wDataType = Data8Array;
break;
case(ReadDiscreteInput):
wDataType = DataInt;
break;
case(ReadHoldingRegisters):
case(ReadInputRegisters):
wDataType = Data16Array;
break;
case(WriteSingleCoil):
case(WriteSingleRegister):
wDataType = DataInt;
isWriteFunction = 1;
break;
case(WriteMultipleCoils):
wDataType = Data8Array;
isWriteFunction = 1;
break;
case(WriteMultipleRegisters):
wDataType = Data16Array;
isWriteFunction = 1;
break;
default:
printf("No correct function type chosen");
printUsage(argv[0]);
exit(EXIT_FAILURE);
}
if (isWriteFunction) {
int dataNo = argc - optind - 1;
/*if (-1 != readWriteNo && dataNo != readWriteNo) {
printf("Write count specified, not equal to data values count!");
printUsage(argv[0]);
exit(EXIT_FAILURE);
}
else*/ readWriteNo = dataNo;
}
//allocate buffer for data
switch (wDataType) {
case (DataInt):
//no need to alloc anything
break;
case (Data8Array):
data.data8 = malloc(readWriteNo * sizeof(uint8_t));
break;
case (Data16Array):
data.data16 = malloc(readWriteNo * sizeof(uint16_t));
break;
default:
printf("Data alloc error!\n");
exit(EXIT_FAILURE);
}
int wDataIdx = 0;
if (1 == debug && 1 == isWriteFunction)
printf("Data to write: ");
if (optind < argc) {
while (optind < argc) {
if (0 == hasDevice) {
if (0 != backend) {
if (Rtu == backend->type) {
RtuBackend *rtuP = (RtuBackend*)backend;
strcpy(rtuP->devName, argv[optind]);
hasDevice = 1;
}
else if (Tcp == backend->type) {
TcpBackend *tcpP = (TcpBackend*)backend;
strcpy(tcpP->ip, argv[optind]);
hasDevice = 1;
}
}
}
else {//setting write data buffer
switch (wDataType) {
case (DataInt):
data.dataInt = getInt(argv[optind], 0);
if (debug)
printf("0x%x", data.dataInt);
break;
case (Data8Array): {
data.data8[wDataIdx] = getInt(argv[optind], 0);
if (debug)
printf("0x%02x ", data.data8[wDataIdx]);
}
break;
case (Data16Array): {
data.data16[wDataIdx] = getInt(argv[optind], 0);
if (debug)
printf("0x%04x ", data.data16[wDataIdx]);
}
break;
}
wDataIdx++;
}
optind++;
}
}
if (1 == debug && 1 == isWriteFunction)
printf("\n");
//create modbus context, and preapare it
modbus_t *ctx = backend->createCtxt(backend);
modbus_set_debug(ctx, debug);
modbus_set_slave(ctx, slaveAddr);
//issue the request
int ret = -1;
if (modbus_connect(ctx) == -1) {
fprintf(stderr, "Connection failed: %s\n",
modbus_strerror(errno));
modbus_free(ctx);
return -1;
} else {
switch (fType) {
case(ReadCoils):
ret = modbus_read_bits(ctx, startAddr, readWriteNo, data.data8);
break;
case(ReadDiscreteInput):
printf("ReadDiscreteInput: not implemented yet!\n");
wDataType = DataInt;
break;
case(ReadHoldingRegisters):
ret = modbus_read_registers(ctx, startAddr, readWriteNo, data.data16);
break;
case(ReadInputRegisters):
ret = modbus_read_input_registers(ctx, startAddr, readWriteNo, data.data16);
break;
case(WriteSingleCoil):
ret = modbus_write_bit(ctx, startAddr, data.dataInt);
break;
case(WriteSingleRegister):
ret = modbus_write_register(ctx, startAddr, data.dataInt);
break;
case(WriteMultipleCoils):
ret = modbus_write_bits(ctx, startAddr, readWriteNo, data.data8);
break;
case(WriteMultipleRegisters):
ret = modbus_write_registers(ctx, startAddr, readWriteNo, data.data16);
break;
default:
printf("No correct function type chosen");
printUsage(argv[0]);
exit(EXIT_FAILURE);
}
}
if (ret == readWriteNo) {//success
if (isWriteFunction)
printf("SUCCESS: written %d elements!\n", readWriteNo);
else {
printf("SUCCESS: read %d of elements:\n\tData: ", readWriteNo);
int i = 0;
if (DataInt == wDataType) {
printf("0x%04x\n", data.dataInt);
}
else {
const char Format8[] = "0x%02x ";
const char Format16[] = "0x%04x ";
const char *format = ((Data8Array == wDataType) ? Format8 : Format16);
for (; i < readWriteNo; ++i) {
printf(format, (Data8Array == wDataType) ? data.data8[i] : data.data16[i]);
}
printf("\n");
}
}
}
else {
printf("ERROR occured!\n");
modbus_strerror(errno);
}
//cleanup
modbus_close(ctx);
modbus_free(ctx);
backend->del(backend);
switch (wDataType) {
case (DataInt):
//nothing to be done
break;
case (Data8Array):
free(data.data8);
break;
case (Data16Array):
free(data.data16);
break;
}
exit(EXIT_SUCCESS);
}
/******************************************************************************
* *
* Function: zbx_modbus_read_registers *
* *
* Purpose: a main entry point for processing of an item *
* *
* Parameters: request - structure that contains item key and parameters *
* request->key - item key without parameters *
* request->nparam - number of parameters *
* request->timeout - processing should not take longer than *
* this number of seconds *
* request->params[N-1] - pointers to item key parameters *
* *
* result - structure that will contain result *
* *
* Return value: SYSINFO_RET_FAIL - function failed, item will be marked *
* as not supported by zabbix *
* SYSINFO_RET_OK - success *
* *
* Comment: get_rparam(request, N-1) can be used to get a pointer to the Nth *
* parameter starting from 0 (first parameter). Make sure it exists *
* by checking value of request->nparam. *
* *
******************************************************************************/
int zbx_modbus_read_registers(AGENT_REQUEST *request, AGENT_RESULT *result)
{
char *param1, *param2,*param3,*param4,*param5,*param6,*param7;
if (request->nparam <4) //check if mandatory params are provided
{
SET_MSG_RESULT(result, strdup("Invalid number of parameters."));
return SYSINFO_RET_FAIL;
}
param1 = get_rparam(request, 0);
if(param_is_empty(param1)) {
SET_MSG_RESULT(result, strdup("No connection address provided."));
return SYSINFO_RET_FAIL;
}
param2 = get_rparam(request, 1);
if(param_is_empty(param2)) {
SET_MSG_RESULT(result, strdup("No slave id provided."));
return SYSINFO_RET_FAIL;
}
param3 = get_rparam(request, 2);
if(param_is_empty(param3)) {
SET_MSG_RESULT(result, strdup("No register to read provided."));
return SYSINFO_RET_FAIL;
}
param4 = get_rparam(request, 3);
if(param_is_empty(param4)) {
SET_MSG_RESULT(result, strdup("No Modbus function provided! Please provide either 1,2,3,4."));
return SYSINFO_RET_FAIL;
}
modbus_t *ctx;
int lock_required;
create_modbus_context(param1,&ctx,&lock_required);
if (ctx == NULL) {
SET_MSG_RESULT(result, strdup("Unable to create the libmodbus context"));
modbus_free(ctx);
return SYSINFO_RET_FAIL;
}
//<slave_id> set slave id
char *endptr;
errno = 0;
int slave_id = strtol(param2,&endptr, 0);
if (errno!=0 || *endptr != '\0') {
SET_MSG_RESULT(result, strdup("Check slaveid parameter"));
modbus_free(ctx);
return SYSINFO_RET_FAIL;
}
modbus_set_slave(ctx, slave_id);
//<reg> set register to start from
errno = 0;
int reg_start = strtol(param3,&endptr, 0);
if (errno!=0 || *endptr != '\0') {
SET_MSG_RESULT(result, strdup("Check register to read"));
modbus_free(ctx);
return SYSINFO_RET_FAIL;
}
//set function to use
errno = 0;
int function = strtol(param4,&endptr, 0);
if (errno!=0 || *endptr != '\0') {
SET_MSG_RESULT(result, strdup("Check function (1,2,3,4) used"));
modbus_free(ctx);
return SYSINFO_RET_FAIL;
}
char datatype;
int end = MODBUS_16BIT_LE; //<endianness> endianness LE(0) BE(1) default LE
if (request->nparam > 4) { //optional params provided
param5 = get_rparam(request, 4); //datatype
if(!validate_datatype_param(param5)) {
SET_MSG_RESULT(result, strdup("Check datatype provided."));
modbus_free(ctx);
return SYSINFO_RET_FAIL;
}
datatype = *param5; // set datatype
param6 = get_rparam(request, 5); //16 endiannes
if(param6) {
//endianness to use
errno = 0;
end = strtol(param6,&endptr, 0);
if ( (end != MODBUS_16BIT_LE && end != MODBUS_16BIT_BE) ||
(errno!=0 || *endptr != '\0') ) {
SET_MSG_RESULT(result, strdup("Check endiannes used"));
modbus_free(ctx);
return SYSINFO_RET_FAIL;
}
}
param7 = get_rparam(request, 6); //PDU
if(param7) {//PDU <first reg> check
//int first_reg=atoi(param7);
errno = 0;
int first_reg = strtol(param7,&endptr, 0);
if ( (first_reg != MODBUS_PROTOCOL_ADDRESS_1 && first_reg != MODBUS_PDU_ADDRESS_0) ||
(errno!=0 || *endptr != '\0') ) {
SET_MSG_RESULT(result, strdup("Check addressing scheme(PDU,PROTOCOL) used"));
modbus_free(ctx);
return SYSINFO_RET_FAIL;
}
if (first_reg == MODBUS_PROTOCOL_ADDRESS_1){
reg_start=reg_start-1;
}
}
}
else {//no datatype set, place defaults
if (function==MODBUS_READ_COIL_1 || function == MODBUS_READ_DINPUTS_2) {
datatype = MODBUS_BIT;//default
}
if (function==MODBUS_READ_H_REGISTERS_3 || function == MODBUS_READ_I_REGISTERS_4) {
datatype = MODBUS_INTEGER ;//default
}
}
/* 3.0.3
struct timeval response_timeout ;
response_timeout.tv_sec = 0;
response_timeout.tv_usec = 0;
modbus_set_response_timeout(ctx, &response_timeout);
*/
//modbus_set_response_timeout(ctx, 10, 0);
//read part
uint16_t tab_reg[64];//temp vars
uint8_t tab_reg_bits[64];
int regs_to_read = 1;
if (datatype == MODBUS_FLOAT || datatype == MODBUS_LONG) { regs_to_read=2;}
if (lock_required == 1 ) LOCK_SERIAL_PORT;
if (modbus_connect(ctx) == -1) {
SET_MSG_RESULT(result, strdup(modbus_strerror(errno)));
modbus_free(ctx);
if (lock_required == 1 ) UNLOCK_SERIAL_PORT;
return SYSINFO_RET_FAIL;
}
int rc;//modbus return_code
switch (function) {
case MODBUS_READ_COIL_1:
rc = modbus_read_bits(ctx, reg_start, regs_to_read, tab_reg_bits);
break;
case MODBUS_READ_DINPUTS_2:
rc = modbus_read_input_bits(ctx, reg_start, regs_to_read, tab_reg_bits);
break;
case MODBUS_READ_H_REGISTERS_3:
rc = modbus_read_registers(ctx, reg_start, regs_to_read, tab_reg);
break;
case MODBUS_READ_I_REGISTERS_4:
rc = modbus_read_input_registers(ctx, reg_start, regs_to_read, tab_reg);
break;
default :
SET_MSG_RESULT(result, strdup("Check function (1,2,3,4) used"));
//close connection
modbus_close(ctx);
if (lock_required == 1 ) UNLOCK_SERIAL_PORT;
modbus_free(ctx);
return SYSINFO_RET_FAIL;
break;
}
//close connection
modbus_close(ctx);
if (lock_required == 1 ) UNLOCK_SERIAL_PORT;
modbus_free(ctx);
if (rc == -1) {
SET_MSG_RESULT(result, strdup(modbus_strerror(errno)));
return SYSINFO_RET_FAIL;
}
//post-parsing
uint16_t temp_arr[2]; //output based on datatype
switch(datatype){
case MODBUS_BIT:
SET_UI64_RESULT(result, tab_reg_bits[0]);
break;
case MODBUS_INTEGER:
SET_UI64_RESULT(result, tab_reg[0]);
break;
case MODBUS_FLOAT:
if (end == MODBUS_16BIT_LE) {
temp_arr[0] = tab_reg[0];
temp_arr[1] = tab_reg[1];
}
if (end == MODBUS_16BIT_BE) {
temp_arr[0] = tab_reg[1];
temp_arr[1] = tab_reg[0];
}
SET_DBL_RESULT(result, modbus_get_float(temp_arr));
break;
case MODBUS_LONG:
//MODBUS_GET_INT32_FROM_INT16 is doing BIG_ENDIAN for register pair, so inverse registers (sort of hack)
if (end == MODBUS_16BIT_LE) {
temp_arr[0] = tab_reg[1];
temp_arr[1] = tab_reg[0];
}
if (end == MODBUS_16BIT_BE) {
temp_arr[0] = tab_reg[0];
temp_arr[1] = tab_reg[1];
}
SET_UI64_RESULT(result, MODBUS_GET_INT32_FROM_INT16(temp_arr, 0));
break;
default :
SET_MSG_RESULT(result, strdup("Check datatype provided."));
modbus_free(ctx);
return SYSINFO_RET_FAIL;
break;
}
return SYSINFO_RET_OK;
}
vector<uint16_t> RbCtrlIface::readMultiReg(uint16_t startAddr, uint16_t nReg)
{
// >>>>> Simulation?
if(mSimulActive)
{
vector<uint16_t> readRegReply;
//mBoardMutex.lock();
{
// >>>>> Reply buffer resize if needed
if( nReg > mReplyBufSize )
{
mReplyBufSize *= 2;
delete [] mReplyBuffer;
mReplyBuffer = new uint16_t[mReplyBufSize];
}
// <<<<< Reply buffer resize if needed
readRegReply.resize( nReg/*+2*/ );
/*readRegReply[0] = (uint16_t)startAddr;
readRegReply[1] = (uint16_t)nReg;*/
for( int i=0; i<nReg; i++ )
{
readRegReply[/*2+*/i] = (i+1)*1000;
}
//qWarning() << PREFIX << "ModBus replies are simulated!";
ros::Duration(0.001).sleep(); // sleep for a msec
}
//mBoardMutex.unlock();
return readRegReply;
}
// <<<<< Simulation?
vector<uint16_t> readRegReply;
if( !mBoardConnected )
{
ROS_ERROR_STREAM( "RoboController is not connected!" );
return readRegReply;
}
//mBoardMutex.lock();
{
// >>>>> Reply buffer resize if needed
if( nReg > mReplyBufSize )
{
mReplyBufSize *= 2;
delete [] mReplyBuffer;
mReplyBuffer = new uint16_t[mReplyBufSize];
}
// <<<<< Reply buffer resize if needed
int res = modbus_read_input_registers( mModbus, startAddr, nReg, mReplyBuffer );
if(res!=nReg)
{
ROS_ERROR_STREAM( "modbus_read_input_registers error -> " << modbus_strerror( errno )
<< "[First regAddress: " << startAddr << "- #reg: " << nReg << "]" );
//mBoardMutex.unlock();
return readRegReply;
}
readRegReply.resize( nReg/*+2*/ );
/*readRegReply[0] = (uint16_t)startAddr;
readRegReply[1] = (uint16_t)nReg;*/
memcpy( (uint16_t*)(readRegReply.data())/*+2*/, mReplyBuffer, nReg*sizeof(uint16_t) );
}
//mBoardMutex.unlock();
return readRegReply;
}
uint8_t *sdl_read_data(int *size,int register_num)
{
int retry=0;
int rc,read_num,i,j;
int created_time;
float parameter;
long long_value;
uint16_t *SDL_Paramenters;
read_num = 2*register_num;
modbus_t *sdl;
uint8_t * rsp = malloc(MODBUS_RTU_MAX_ADU_LENGTH);
sdl = modbus_new_tcp("169.254.114.25",502);
struct timeval old_response_timeout;
struct timeval response_timeout;
/* Save original timeout */
modbus_get_response_timeout(sdl, &old_response_timeout);
/* Define a new and too short timeout! */
response_timeout.tv_sec = 2;
response_timeout.tv_usec = 0;
modbus_set_response_timeout(sdl, &response_timeout);
modbus_set_debug(sdl, TRUE);
modbus_set_error_recovery(sdl,
MODBUS_ERROR_RECOVERY_LINK |
MODBUS_ERROR_RECOVERY_PROTOCOL);
modbus_set_slave(sdl,1);
if (modbus_connect(sdl) == -1)
{
return NULL;
}
try_more:
// The number 6 is the extra bytes that we need to store the data such as checksum
SDL_Paramenters = (uint16_t*)malloc(read_num*sizeof(uint16_t));
rc = modbus_read_input_registers(sdl,UT_REGISTERS_RD_DATA_BUFF_ADDRESS,read_num,SDL_Paramenters);
//*size = modbus_receive_confirmation(sdl, rsp);
if (rc != read_num)
{
if(retry < 3)
{
retry++;
sleep(2);
goto try_more;
}
else return -100000;
}
i = 0;
for(j=0;j<rc;j++)
{
rsp[i+1] = SDL_Paramenters[j] & 0x00ff;
rsp[i] = SDL_Paramenters[j]>>8;
i = i+2;
//printf("%x %x\n",rsp[i+1],rsp[i]);
}
*size = 2*rc;
return rsp;
}
/* Read values from modbus */
uint8_t read_modbus(modbus_t *ctx, uint8_t readOrWrite, uint16_t modbusRegister, GenDataType_t typeOfValue, MoBuRegType_t typeOfReg, uint8_t desiredBit, uint16_t* valueArray)
{
uint8_t noOfRegisters = 0;
uint16_t modbusValueArray[2];
uint16_t modbusTmpValue; //used for bit values read from registers
memset(modbusValueArray, 0x00, (sizeof(uint16_t) * 2));
if (readOrWrite > 0) {
log_entry(APP_NAME, "invalid parameter in read_modbus: Only reading values supported");
printf("invalid parameter in read_modbus: Only reading values supported");
return -1;
}
switch (typeOfValue)
{
case bitCoil:
case sint16: // read one address
case uint16: //read one address
noOfRegisters = 1;
break;
case sint32: //read 2 registers
case uint32:
case float32:
noOfRegisters = 2;
break;
default:
log_entry(APP_NAME, "MODBUSD; unknown dataType");
printf("MODBUSD; unknown dataType: %d", typeOfValue);
}
switch (typeOfReg)
{
case coil:
if (0 == readOrWrite) { //read
if (1 != modbus_read_bits(ctx, modbusRegister, 1, (uint8_t *) modbusValueArray)) {
log_entry(APP_NAME, "MODBUSD: could not read coils");
printf("MODBUSD: could not read coils: %d, %s", modbusRegister, strerror(errno));
return -1;
}
} else {
/* first read the whole unit16 value */
if (1 == modbus_read_bits(ctx, modbusRegister, 1, (uint8_t *) modbusValueArray)) {
if (valueArray[0] == 0) {
modbusTmpValue = 0;
} else if (valueArray[0] == 1) {
//setting bit to 1
modbusTmpValue = 1;
} else {
//toggle the bit
if(0 == modbusValueArray[0]){
modbusTmpValue = 1;
}
else {
modbusTmpValue = 0;
}
}
if (1 != modbus_write_bit(ctx, modbusRegister, modbusTmpValue)) {
return -1;
}
}
}
break;
case discrete_input:
if (0 == readOrWrite) { //read
if (1 != modbus_read_input_bits(ctx, modbusRegister, noOfRegisters, (uint8_t *) &(modbusValueArray[0]))) {
log_entry(APP_NAME, "MODBUSD: could not read input");
printf("MODBUSD: could not read input: %d, %s", modbusRegister, strerror(errno));
return -1;
}
} else {
log_entry(APP_NAME, "MODBUSD: illegal operation -> unable to write on discrete input");
printf("MODBUSD: illegal operation -> unable to write on discrete input");
}
break;
case holding_register:
if (0 == readOrWrite) { //read
if (1 != modbus_read_registers(ctx, modbusRegister, noOfRegisters, &(modbusValueArray[0]))) {
return -1;
}
} else {
if (1 != modbus_write_registers(ctx, modbusRegister, noOfRegisters, &(valueArray[0]))) {
return -1;
}
}
break;
case input_register:
if (0 == readOrWrite) { //read
if (1 != modbus_read_input_registers(ctx, modbusRegister, noOfRegisters, &(modbusValueArray[0]))) {
log_entry(APP_NAME, "MODBUSD: could not read input registers");
printf("MODBUSD: could not read input registers: %d, %s", modbusRegister, strerror(errno));
return -1;
}
} else {
log_entry(APP_NAME, "unable to write on input register");
printf("unable to write on input register");
}
break;
case holding_bit:
if (0 == readOrWrite) { //read
if (1 == modbus_read_registers(ctx, modbusRegister, noOfRegisters, &modbusTmpValue)) {
modbusValueArray[0] = modbusTmpValue & (1 << desiredBit);
} else {
return -1;
}
} else {
/* first read the whole unit16 value */
if (1 == modbus_read_registers(ctx, modbusRegister, noOfRegisters, &modbusTmpValue)) {
if (valueArray[0] == 0) {
//clearing a bit at postion desired bit
modbusTmpValue &= ~(1 << desiredBit);
} else if (valueArray[0] == 1) {
//setting bit to 1
modbusTmpValue |= 1 << desiredBit;
} else {
//toggle the bit
modbusTmpValue ^= 1 << desiredBit;
}
if (1 != modbus_write_registers(ctx, modbusRegister, noOfRegisters, &modbusTmpValue)) {
return -1;
}
} else {
return -1;
}
}
break;
case input_bit:
if (0 == readOrWrite) { //read
if (1 == modbus_read_input_registers(ctx, modbusRegister, noOfRegisters, &modbusTmpValue)) {
modbusValueArray[0] = modbusTmpValue & (1 << desiredBit);
} else {
return -1;
}
} else {
log_entry(APP_NAME, "MODBUSD; unable to write single bit for input register");
printf("MODBUSD; unable to write single bit for input register");
return -1;
}
break;
default:
log_entry(APP_NAME, "MODBUSD; unknown register type");
printf("MODBUSD; unknown register type %d", typeOfReg);
}
if (0 == readOrWrite) { //read
valueArray[0] = modbusValueArray[0];
valueArray[1] = modbusValueArray[1];
}
return 1;
}
int main(int argc, char **argv) {
uint8_t *tab_bit;
uint16_t *tab_reg;
/* Allocate and initialize the memory to store the status */
tab_bit = (uint8_t *)malloc(MODBUS_MAX_READ_BITS * sizeof(uint8_t));
memset(tab_bit, 0, MODBUS_MAX_READ_BITS * sizeof(uint8_t));
/* Allocate and initialize the memory to store the registers */
tab_reg = (uint16_t *)malloc(MODBUS_MAX_READ_REGISTERS * sizeof(uint16_t));
memset(tab_reg, 0, MODBUS_MAX_READ_REGISTERS * sizeof(uint16_t));
/* Defaults */
char host[1024] = "127.0.0.1";
size_t port = 502;
enum mb_operations operation = read_registers;
size_t base_addr = 0; // read from address 0 by deafult
size_t n_addrs = 10; // read forst 10 addressxes by default.
size_t n_times = 1; // one times by default.
size_t interval = 0; // no waiting by default.
bool quiet = false; // Do not suppress output by default.
size_t n_data = 0; // The number of data items from the command line.
char **data_items; // Array of data items from the command line.
int c;
int rc;
while (1)
{
c = getopt(argc, argv, "h:p:o:b:a:n:i:q");
if (c == -1)
break;
switch (c)
{
case 'h':
printf("Connect to host: '%s'\n", optarg);
strncpy(host, optarg, 1024); // Made host str 1024. Should define a constant.
break;
case 'p':
printf("Connect on port: '%s'\n", optarg);
port = strtoul(optarg, (void *)0, 10);
break;
case 'o':
printf("Option o with value '%s'\n", optarg);
if (!strcmp("read_bits", optarg)) operation = read_bits;
if (!strcmp("read_input_bits", optarg)) operation = read_input_bits;
if (!strcmp("read_registers", optarg)) operation = read_registers;
if (!strcmp("read_input_registers", optarg))operation = read_input_registers;
if (!strcmp("write_bit", optarg)) operation = write_bit;
if (!strcmp("write_register", optarg)) operation = write_register;
if (!strcmp("write_bits", optarg)) operation = write_bits;
if (!strcmp("write_registers",optarg)) operation = write_registers;
break;
case 'b':
printf("Base address set to: '%s'\n", optarg);
base_addr = strtoul(optarg, (void *)0, 10);;
break;
case 'a':
printf("Number of addresses to read: '%s'\n", optarg);
n_addrs = strtoul(optarg, (void *)0, 10);
if (n_addrs > MODBUS_MAX_READ_REGISTERS) {
printf("Number of addresses to read adjusted to maximum: '%i'\n", MODBUS_MAX_READ_REGISTERS);
n_addrs = MODBUS_MAX_READ_REGISTERS;
}
break;
case 'n':
printf("Repeat this many times: '%s'\n", optarg);
n_times = strtoul(optarg, (void *)0, 10);
break;
case 'i':
printf("Repeat at this interval: '%s'\n", optarg);
interval = strtoul(optarg, (void *)0, 10);
break;
case 'q':
printf("option q: supressing output.\n");
quiet = true;
break;
case '?':
printf("option ?: There is not doc only source codes.\n");
break;
default:
printf("?? getopt returned character code 0%o ??\n", c);
}
}
if (optind < argc) {
n_data = argc - optind;
data_items = (char **)malloc(n_data * sizeof(char *));
printf("There are %i non-option ARGV-elements: ", (argc-optind));
size_t n = 0;
while (optind < argc) {
printf("%s ", argv[optind]);
data_items[n] = argv[optind];
++optind;
++n;
}
printf("\n");
}
/* Get show on the road. */
modbus_t *ctx;
ctx = modbus_new_tcp(host, port);
if (modbus_connect(ctx) == -1) {
fprintf(stderr, "Connection failed: %s\n",
modbus_strerror(errno));
modbus_free(ctx);
return -1;
}
for (size_t i = 0; i<n_times; i++) {
switch (operation)
{
case read_bits:
printf("READ BITS:\n");
rc = modbus_read_bits(ctx, base_addr, n_addrs, tab_bit);
if (rc == -1) {
fprintf(stderr, "%s\n", modbus_strerror(errno));
return(EXIT_FAILURE);
}
if (!quiet) {
/* What did we read?*/
for (uint8_t *reg_ptr = tab_bit; reg_ptr < tab_bit + n_addrs; reg_ptr++) {
printf("%i ", *reg_ptr);
}
printf("\n");
}
break;
case read_input_bits:
printf("READ INPUT BITS:\n");
rc = modbus_read_input_bits(ctx, base_addr, n_addrs, tab_bit);
if (rc == -1) {
fprintf(stderr, "%s\n", modbus_strerror(errno));
return(EXIT_FAILURE);
}
if (!quiet) {
/* What did we read?*/
for (uint8_t *reg_ptr = tab_bit; reg_ptr < tab_bit + n_addrs; reg_ptr++) {
printf("%i ", *reg_ptr);
}
printf("\n");
}
break;
case read_registers:
printf("READ REGISTERS:\n");
rc = modbus_read_registers(ctx, base_addr, n_addrs, tab_reg);
if (rc == -1) {
fprintf(stderr, "%s\n", modbus_strerror(errno));
return(EXIT_FAILURE);
}
if (!quiet) {
/* What did we read?*/
for (uint16_t *reg_ptr = tab_reg; reg_ptr < tab_reg + n_addrs; reg_ptr++) {
printf("%i ", *reg_ptr);
}
printf("\n");
}
break;
case read_input_registers:
printf("READ INPUT REGISTERS\n\n");
rc = modbus_read_input_registers(ctx, base_addr, n_addrs, tab_reg);
if (rc == -1) {
fprintf(stderr, "%s\n", modbus_strerror(errno));
return(EXIT_FAILURE);
}
if (!quiet) {
/* What did we read?*/
for (uint16_t *reg_ptr = tab_reg; reg_ptr < tab_reg + n_addrs; reg_ptr++) {
printf("%i ", *reg_ptr);
}
printf("\n");
}
break;
case write_bit:
printf("WRITE BIT\n");
n_addrs = 1;
if (n_addrs > n_data) {
fprintf(stderr, "Not enough data items on command line to write to requested address.\n");
return(EXIT_FAILURE);
}
for (uint8_t *reg_ptr = tab_bit; reg_ptr < tab_bit + n_addrs; reg_ptr++) {
*reg_ptr = strtoul(*data_items++, (void *)0, 10);
}
rc = modbus_write_bit(ctx, base_addr, tab_bit);
if (rc == -1) {
fprintf(stderr, "%s\n", modbus_strerror(errno));
return(EXIT_FAILURE);
}
break;
case write_register:
printf("WRITE REGISTER\n");
// There is only one register. Almost everything else stays the same as write_registers.
n_addrs = 1;
if (n_addrs > n_data) {
fprintf(stderr, "Not enough data items on command line to write to requested address.\n");
return(EXIT_FAILURE);
}
for (uint16_t *reg_ptr = tab_reg; reg_ptr < tab_reg + n_addrs; reg_ptr++) {
*reg_ptr = strtoul(*data_items++, (void *)0, 10);
}
rc = modbus_write_register(ctx, base_addr, tab_reg);
if (rc == -1) {
fprintf(stderr, "%s\n", modbus_strerror(errno));
return(EXIT_FAILURE);
}
break;
case write_bits:
printf("WRITE BITS\n");
if (n_addrs > n_data) {
fprintf(stderr, "Not enough data items on command line to write to requested addresses.\n");
return(EXIT_FAILURE);
}
for (uint8_t *reg_ptr = tab_bit; reg_ptr < tab_bit + n_addrs; reg_ptr++) {
*reg_ptr = strtoul(*data_items++, (void *)0, 10);
}
rc = modbus_write_bits(ctx, base_addr, n_addrs, tab_bit);
if (rc == -1) {
fprintf(stderr, "%s\n", modbus_strerror(errno));
return(EXIT_FAILURE);
}
break;
case write_registers:
printf("WRITE REGISTERS\n");
if (n_addrs > n_data) {
fprintf(stderr, "Not enough data items on command line to write to requested addresses.\n");
return(EXIT_FAILURE);
}
for (uint16_t *reg_ptr = tab_reg; reg_ptr < tab_reg + n_addrs; reg_ptr++) {
*reg_ptr = strtoul(*data_items++, (void *)0, 10);
}
rc = modbus_write_registers(ctx, base_addr, n_addrs, tab_reg);
if (rc == -1) {
fprintf(stderr, "%s\n", modbus_strerror(errno));
return(EXIT_FAILURE);
}
break;
default:
break;
}
sleep(interval);
}
/* Free the memory */
free(tab_bit);
free(tab_reg);
//free(data_items);
/* Close the connection */
modbus_close(ctx);
modbus_free(ctx);
exit(EXIT_SUCCESS);
}
static void update_dynamic_overlay(char *s)
{
if (strlen(s) > OVERLAY_STR_SIZE) {
g_message("Overlay string to large %d > %d", strlen(s),
OVERLAY_STR_SIZE);
return;
}
/* Ignore return value if group is already created */
sc_create_group("DYNAMIC_TEXT_IS1", 512, 0);
struct sc_param sc_par = { .name="DYNAMIC_TEXT",
.size=OVERLAY_BUF_SIZE,
.data=s,
.type=SC_STRING};
/* NULL-terminated list of pointers to param structs. we need just one */
struct sc_param *arr[2] = {&sc_par, 0};
sc_set_group("DYNAMIC_TEXT_IS1", arr, SC_CREATE);
}
static float lily_read_humidity_data(struct modbus **modbus)
{
g_assert(modbus);
g_assert(*modbus);
struct modbus *m = *modbus;
/* Read input register starting at 0 and just the first register */
modbus_read_input_registers(m, 10, 2);
/* Wait for device to process data */
usleep(50000);
static unsigned int n_reads = 0;
static unsigned int n_failures = 0;
uint16_t reg1;
uint16_t reg2;
size_t nregs;
uint16_t *regs = modbus_parse_input_registers(m, &nregs);
if (regs) {
reg1 = regs[0];
g_message("[%d, %d] Got Reg1 0x%04x",
n_reads % 10, n_failures % 5, reg1);
int bit1 = (reg1 & 0x01) != 0;
int bit2 = (reg1 & (0x01 << 1)) != 0;
int bit3 = (reg1 & (0x01 << 2)) != 0;
int bit4 = (reg1 & (0x01 << 3)) != 0;
/* Finally update the dynamic overlay with the humidity data */
char str[OVERLAY_BUF_SIZE];
g_snprintf(str, sizeof(str), "[%d] REG1-bits: %d%d%d%d", (++n_reads) % 10, bit1, bit2, bit3, bit4);
if (nregs > 1) {
reg2 = regs[1];
g_message("[%d] Got Reg2 0x%04x", n_reads % 10,
reg2);
bit1 = (reg2 & 0x01) != 0;
bit2 = (reg2 & (0x01 << 1)) != 0;
bit3 = (reg2 & (0x01 << 2)) != 0;
bit4 = (reg2 & (0x01 << 3)) != 0;
int bit5 = (reg2 & (0x01 << 4)) != 0;
int bit6 = (reg2 & (0x01 << 5)) != 0;
int bit7 = (reg2 & (0x01 << 6)) != 0;
int bit8 = (reg2 & (0x01 << 7)) != 0;
snprintf(&str[strlen(str)], sizeof(str), " REG2-bits: %d%d%d%d%d%d%d%d",
bit1, bit2, bit3, bit4, bit5, bit6, bit7, bit8);
}
update_dynamic_overlay(str);
g_free(regs);
} else {
n_failures++;
/* Re-init serial port in case something went wrong */
if (n_failures && n_failures % 5) {
lily_init_modbus(modbus);
}
}
return 0;
}
static int mb_read_data (mb_host_t *host, mb_slave_t *slave, /* {{{ */
mb_data_t *data)
{
uint16_t values[2];
int values_num;
const data_set_t *ds;
int status;
if ((host == NULL) || (slave == NULL) || (data == NULL))
return (EINVAL);
ds = plugin_get_ds (data->type);
if (ds == NULL)
{
ERROR ("Modbus plugin: Type \"%s\" is not defined.", data->type);
return (-1);
}
if (ds->ds_num != 1)
{
ERROR ("Modbus plugin: The type \"%s\" has %i data sources. "
"I can only handle data sets with only one data source.",
data->type, ds->ds_num);
return (-1);
}
if ((ds->ds[0].type != DS_TYPE_GAUGE)
&& (data->register_type != REG_TYPE_INT32)
&& (data->register_type != REG_TYPE_UINT32))
{
NOTICE ("Modbus plugin: The data source of type \"%s\" is %s, not gauge. "
"This will most likely result in problems, because the register type "
"is not UINT32.", data->type, DS_TYPE_TO_STRING (ds->ds[0].type));
}
memset (values, 0, sizeof (values));
if ((data->register_type == REG_TYPE_INT32)
|| (data->register_type == REG_TYPE_UINT32)
|| (data->register_type == REG_TYPE_FLOAT))
values_num = 2;
else
values_num = 1;
status = 0;
if (host->connection == NULL)
{
status = EBADF;
}
else if (host->conntype == MBCONN_TCP)
{
struct sockaddr sockaddr;
socklen_t saddrlen = sizeof (sockaddr);
status = getpeername (modbus_get_socket (host->connection),
&sockaddr, &saddrlen);
if (status != 0)
status = errno;
}
if ((status == EBADF) || (status == ENOTSOCK) || (status == ENOTCONN))
{
status = mb_init_connection (host);
if (status != 0)
{
ERROR ("Modbus plugin: mb_init_connection (%s/%s) failed. ",
host->host, host->node);
host->is_connected = 0;
host->connection = NULL;
return (-1);
}
}
else if (status != 0)
{
#if LEGACY_LIBMODBUS
modbus_close (&host->connection);
#else
modbus_close (host->connection);
modbus_free (host->connection);
#endif
}
#if LEGACY_LIBMODBUS
/* Version 2.0.3: Pass the connection struct as a pointer and pass the slave
* id to each call of "read_holding_registers". */
# define modbus_read_registers(ctx, addr, nb, dest) \
read_holding_registers (&(ctx), slave->id, (addr), (nb), (dest))
#else /* if !LEGACY_LIBMODBUS */
/* Version 2.9.2: Set the slave id once before querying the registers. */
status = modbus_set_slave (host->connection, slave->id);
if (status != 0)
{
ERROR ("Modbus plugin: modbus_set_slave (%i) failed with status %i.",
slave->id, status);
return (-1);
}
#endif
if (data->modbus_register_type == MREG_INPUT){
status = modbus_read_input_registers (host->connection,
/* start_addr = */ data->register_base,
/* num_registers = */ values_num, /* buffer = */ values);
}
else{
status = modbus_read_registers (host->connection,
/* start_addr = */ data->register_base,
/* num_registers = */ values_num, /* buffer = */ values);
}
if (status != values_num)
{
ERROR ("Modbus plugin: modbus read function (%s/%s) failed. "
" status = %i, values_num = %i. Giving up.",
host->host, host->node, status, values_num);
#if LEGACY_LIBMODBUS
modbus_close (&host->connection);
#else
modbus_close (host->connection);
modbus_free (host->connection);
#endif
host->connection = NULL;
return (-1);
}
DEBUG ("Modbus plugin: mb_read_data: Success! "
"modbus_read_registers returned with status %i.", status);
if (data->register_type == REG_TYPE_FLOAT)
{
float float_value;
value_t vt;
float_value = mb_register_to_float (values[0], values[1]);
DEBUG ("Modbus plugin: mb_read_data: "
"Returned float value is %g", (double) float_value);
CAST_TO_VALUE_T (ds, vt, float_value);
mb_submit (host, slave, data, vt);
}
else if (data->register_type == REG_TYPE_INT32)
{
union
{
uint32_t u32;
int32_t i32;
} v;
value_t vt;
v.u32 = (((uint32_t) values[0]) << 16)
| ((uint32_t) values[1]);
DEBUG ("Modbus plugin: mb_read_data: "
"Returned int32 value is %"PRIi32, v.i32);
CAST_TO_VALUE_T (ds, vt, v.i32);
mb_submit (host, slave, data, vt);
}
else if (data->register_type == REG_TYPE_INT16)
{
union
{
uint16_t u16;
int16_t i16;
} v;
value_t vt;
v.u16 = values[0];
DEBUG ("Modbus plugin: mb_read_data: "
"Returned int16 value is %"PRIi16, v.i16);
CAST_TO_VALUE_T (ds, vt, v.i16);
mb_submit (host, slave, data, vt);
}
else if (data->register_type == REG_TYPE_UINT32)
{
uint32_t v32;
value_t vt;
v32 = (((uint32_t) values[0]) << 16)
| ((uint32_t) values[1]);
DEBUG ("Modbus plugin: mb_read_data: "
"Returned uint32 value is %"PRIu32, v32);
CAST_TO_VALUE_T (ds, vt, v32);
mb_submit (host, slave, data, vt);
}
else /* if (data->register_type == REG_TYPE_UINT16) */
{
value_t vt;
DEBUG ("Modbus plugin: mb_read_data: "
"Returned uint16 value is %"PRIu16, values[0]);
CAST_TO_VALUE_T (ds, vt, values[0]);
mb_submit (host, slave, data, vt);
}
return (0);
} /* }}} int mb_read_data */
int main(int argc, char *argv[])
{
uint16_t regs[1];
float heaterTemp;
float heaterSetPoint;
char * heaterEnableState;
char * heaterElementState;
char * modbusClientIP = DEFAULT_CLIENT_IP;
int modbusPort = DEFAULT_PORT;
int packet_counter = 0;
uint8_t *tab_bits;
uint16_t *tab_input_value_registers;
tab_bits = (uint8_t *)malloc(NO_BITS * sizeof(uint8_t));
memset(tab_bits, 0, NO_BITS * sizeof(uint8_t));
tab_input_value_registers = (uint16_t *)malloc(NO_REGS* sizeof(uint16_t));
memset(tab_input_value_registers, 0, NO_REGS * sizeof(uint16_t));
bool sts;
int rc;
uint8_t coils[2];
ctx = modbus_new_tcp(modbusClientIP, modbusPort);
if (ctx == NULL) {
fprintf(stderr, "Unable to allocate libmodbus context\n");
return -1;
}
if (modbus_connect(ctx) == -1) {
sts = false;
}
rc = modbus_read_input_registers(ctx,0,1, regs);
packet_counter++;
printf(" Value of current:%d\n", regs[0]);
modbus_close(ctx);
ctx = modbus_new_tcp(modbusClientIP, modbusPort);
if (ctx == NULL) {
fprintf(stderr, "Unable to allocate libmodbus context\n");
return -1;
}
if (modbus_connect(ctx) == -1) {
sts = false;
}
coils[0] = false;
coils[1] = false;
rc = modbus_write_bits(ctx,0,2,coils);
printf("Status of circuit breakers to be written: CB1 = %d, CB2 = %d\n", coils[0],coils[1]);
/* Close the connection */
modbus_close(ctx);
modbus_free(ctx);
sleep(5);
return 0;
}
uint8_t * sdl_read_buffsize(int *size)
{
int resend=0,re_con=0;
int rc, size_per_record,num_parameter,i,j;
unsigned long num_record, num_byte;
modbus_t *sdl;
uint16_t * SDL_DATA_BUFF;
uint8_t * rsp = malloc(MODBUS_RTU_MAX_ADU_LENGTH);
reconnect:
sdl = modbus_new_tcp("169.254.114.25",502);
struct timeval old_response_timeout;
struct timeval response_timeout;
/* Save original timeout */
modbus_get_response_timeout(sdl, &old_response_timeout);
/* Define a new and too short timeout! */
response_timeout.tv_sec = 2;
response_timeout.tv_usec = 0;
modbus_set_response_timeout(sdl, &response_timeout);
modbus_set_debug(sdl, TRUE);
modbus_set_error_recovery(sdl,
MODBUS_ERROR_RECOVERY_LINK |
MODBUS_ERROR_RECOVERY_PROTOCOL);
modbus_set_slave(sdl,1);
if (modbus_connect(sdl) == -1)
{
if(re_con < 3)
{
re_con++;
sleep(2);
goto reconnect;
}
else return NULL;
}
tryagain:
SDL_DATA_BUFF = (uint16_t*)malloc(4*sizeof(uint16_t));
rc = modbus_read_input_registers(sdl,UT_REGISTERS_RD_DATA_BUFFSIZE_ADDRESS,2,SDL_DATA_BUFF);
if (rc != 2)
{
if(resend < 3)
{
resend++;
sleep(2);
goto tryagain;
}
else return -100000;
}
i = 0;
for(j=0;j<rc;j++)
{
rsp[i+1] = SDL_DATA_BUFF[j] & 0x00ff;
rsp[i] = SDL_DATA_BUFF[j]>>8;
i = i+2;
}
*size = 2*rc;
return rsp;
}
int main(int argc, char *argv[])
{
uint8_t *tab_bits;
uint8_t *tab_input_bits;
uint16_t *tab_value_registers;
uint16_t *tab_input_value_registers;
modbus_t *ctx;
int rc;
float heaterTemp;
float heaterSetPoint;
char * heaterEnableState;
char * heaterElementState;
bool displayCnt = true;
char * displayFlag;
char * modbusClientIP = DEFAULT_CLIENT_IP;
int modbusPort = DEFAULT_PORT;
int modbusPollIntervalmSec = DEFAULT_POLL_INTERVAL_MSEC;
int argcnt = 1;
while(argc > argcnt) {
if(strcmp(argv[argcnt], IP_ADDRESS_FLAG) == 0) {
argcnt++;
if(argcnt < argc) {
modbusClientIP = argv[argcnt++];
}
else {
printf("Illegal command flags\n");
exit(1);
}
}
else if(strcmp(argv[argcnt], IP_PORT_FLAG) == 0) {
argcnt++;
if(argcnt < argc) {
sscanf(argv[argcnt++], "%i", &modbusPort);
}
else {
printf("Illegal command flags\n");
exit(1);
}
}
else if(strcmp(argv[argcnt], POLL_INTERVAL_FLAG) == 0) {
argcnt++;
if(argcnt < argc) {
sscanf(argv[argcnt++], "%i", &modbusPollIntervalmSec);
/* interpret command line argument as seconds. Convert to ms. */
modbusPollIntervalmSec *= 1000;
}
else {
printf("Illegal command flags\n");
exit(1);
}
}
else {
printf("Illegal command flags\n");
exit(1);
}
}
/* create modbus context structure */
ctx = modbus_new_tcp(modbusClientIP, modbusPort);
if (ctx == NULL) {
fprintf(stderr, "Unable to allocate libmodbus context\n");
return -1;
}
/* allocate room to hold water heater register data */
tab_bits = (uint8_t *)malloc(NUM_BIT_REG * sizeof(uint8_t));
memset(tab_bits, 0, NUM_BIT_REG * sizeof(uint8_t));
tab_input_bits = (uint8_t *)malloc(NUM_INPUT_BIT_REG * sizeof(uint8_t));
memset(tab_input_bits, 0, NUM_INPUT_BIT_REG * sizeof(uint8_t));
tab_value_registers = (uint16_t *)malloc(NUM_VALUE_REG * sizeof(uint16_t));
memset(tab_value_registers, 0, NUM_VALUE_REG * sizeof(uint16_t));
tab_input_value_registers = (uint16_t *)malloc(NUM_INPUT_VALUE_REG * sizeof(uint16_t));
memset(tab_input_value_registers, 0, NUM_INPUT_VALUE_REG * sizeof(uint16_t));
/* loop forever */
while(1) {
if (modbus_connect(ctx) == -1) {
fprintf(stderr, "Connection failed: %s\n", modbus_strerror(errno));
modbus_free(ctx);
break;
}
rc = modbus_read_bits(ctx, HEATER_COIL_ENABLE,
NUM_BIT_REG, tab_bits);
if (rc != 1) {
break;
}
rc = modbus_read_input_bits(ctx, HEATER_COIL_ENERGIZED,
NUM_INPUT_BIT_REG, tab_input_bits);
if (rc != 1) {
break;
}
rc = modbus_read_input_registers(ctx, HEATER_WATER_TEMP_REG,
NUM_INPUT_VALUE_REG, tab_input_value_registers);
if (rc != 1) {
break;
}
rc = modbus_read_registers(ctx, HEATER_WATER_TARGET_TEMP_REG,
NUM_VALUE_REG, tab_value_registers);
if (rc != 1) {
break;
}
/* close the connection */
modbus_close(ctx);
if(tab_bits[0]) {
heaterEnableState = "Heater Enabled, ";
}
else {
heaterEnableState = "Heater Disabled, ";
}
if(tab_input_bits[0]) {
heaterElementState = "Heater Element On, ";
}
else {
heaterElementState = "Heater Element Off, ";
}
heaterSetPoint = ((float)tab_value_registers[0]) / 10.0;
heaterTemp = ((float)tab_input_value_registers[0]) / 10.0;
if (displayCnt) {
displayCnt = false;
displayFlag = "+";
}
else {
displayCnt = true;
displayFlag = "-";
}
printf("Status(%s): %s%sSet Point: %5.1f, Temp: %5.1f\n",
displayFlag,
heaterEnableState, heaterElementState,
heaterSetPoint, heaterTemp);
memset(tab_input_value_registers, 0, NUM_INPUT_VALUE_REG * sizeof(uint16_t));
usleep(modbusPollIntervalmSec * 1000);
}
printf("Failed modbus read %d\n", rc);
printf("Exiting due to read failure.\n");
/* Free the memory */
free(tab_bits);
free(tab_input_bits);
free(tab_value_registers);
free(tab_input_value_registers);
/* Close the connection */
modbus_close(ctx);
modbus_free(ctx);
return 0;
}
ssize_t MeterModbus::read(std::vector<Reading> &rds, size_t max_readings) {
uint16_t in;
double out;
int rc;
const struct addressparam *current_address;
int read_count = 0;
if(_reset_connection) {
int success;
print(log_info, "Resetting Connection to %s because of error", name().c_str(), _ip.c_str());
rc = open();
if(rc == SUCCESS)
_reset_connection = false;
else
return 0;
}
current_address = _addressparams;
unsigned char highest_digit, power;
while((current_address->function_code != 0xFF) && (max_readings > read_count)) {
getHighestDigit(current_address->address, &highest_digit, &power);
switch(current_address->function_code){
case READ_HOLDING_REGISTERS:
print(log_debug, "Accessing Holding Register %u", name().c_str(), current_address->address);
rc = modbus_read_registers(_mb, current_address->address-4*(unsigned int)pow((double)10,(double)power)-1, 1, &in);
break;
case READ_INPUT_REGISTERS:
print(log_debug, "Accessing Input Register %u", name().c_str(), current_address->address);
rc = modbus_read_input_registers(_mb, current_address->address-3*(unsigned int)pow((double)10,(double)power)-1, 1, &in);
break;
case READ_COIL_STATUS:
print(log_debug, "Accessing Coil Status register %u", name().c_str(), current_address->address);
rc = modbus_read_bits(_mb, current_address->address, 1, (uint8_t *)&in);
break;
case READ_INPUT_STATUS:
print(log_debug, "Accessing Input Status register %u", name().c_str(), current_address->address);
rc = modbus_read_input_bits(_mb, current_address->address-2*(unsigned int)pow((double)10,(double)power)-1, 1, (uint8_t *)&in);
break;
}
if (rc == -1 && errno != 112345680) { //Except Illegal Data Address
print(log_error, "Unable to fetch data (FC: %u, ADR: %u): %s", name().c_str(), current_address->function_code, current_address->address, modbus_strerror(errno));
if(errno == 104 || errno == 32){
close();
_reset_connection = true;
}
return read_count;
}
if(rc == -1 && errno == 112345680){
print(log_error, "Unable to fetch data (FC: %u, ADR: %u): %s", name().c_str(), current_address->function_code, current_address->address, modbus_strerror(errno));
current_address++;
continue;
}
print(log_debug, "Got %u via Modbus", "", in);
// TODO ERRORS possible if wrong format string input from config file
char *math_expression;
asprintf(&math_expression, current_address->recalc_str, in);
print(log_debug, "Calulating: %s --> %s", "", current_address->recalc_str, math_expression);
out = parse_expression(math_expression);
if(isnan(out)) {
print(log_error, "Unable to use value read from address %u. Error calculating: %s", name().c_str(), current_address->address, math_expression);
}
else {
rds[read_count].value(out);
rds[read_count].time();
rds[read_count].identifier(new AddressIdentifier(current_address->address));
read_count++;
}
free(math_expression);
current_address++;
}
return read_count;
}
int main() {
int nb_points=50;
uint16_t *tab_rp_registers;
int rc = -1;
int rtu=0x01;
int rtuArray[MAX_RTU];
int i=0;
sqlite3 *db;
char *zErrMsg = 0;
char sql[1024];
char *tmp;
char *tty;
char parity;
int baud_rate;
int stop_bits;
int length;
int nap_time=30; // Fasted sample rate allowed is 2 times a minute.
int retries=RETRIES;
int modbus_base_address=0; // The offset into the modbus registers.
tmp=getenv("DATABASE");
if( !tmp ) {
tmp=strsave("/var/data/RS.db");
}
printf("Opening db %s\n",tmp);
rc=sqlite3_open(tmp,&db);
if ( rc != SQLITE_OK) { // something went wrong
fprintf(stderr, "Can't open database: %s\n", sqlite3_errmsg(db));
sqlite3_close( db );
exit(-1);
}
/*
* The config table should have a single row. I have not assumed that this
* constraint is enforced by the database, so the follwoing SQL statement,
* select the wows by descinding index, i.e. the newest first,
* and then limits the number of rows to 1.
*
* A potetntial benefit is that I could keep old configs and then revert by
* deleteing the newset.
*
*/
strcpy(sql,"select tty_port,baud_rate, parity,stop_bits,length,freq from config order by idx desc limit 1;");
rc = sqlite3_exec(db, sql, callback, 0, &zErrMsg);
if( rc!=SQLITE_OK ) {
fprintf(stderr, "SQL error: %s\n", zErrMsg);
sqlite3_free(zErrMsg);
}
printf("res=%s\n",results);
tty=strtok(results,":");
if(!tty) {
fprintf(stderr,"tty NULL\n");
exit(1);
}
tmp=strtok(NULL,":");
if(!tmp) {
fprintf(stderr,"baud NULL\n");
exit(1);
}
baud_rate=atoi(tmp);
tmp=strtok(NULL,":");
if(!tmp) {
fprintf(stderr,"parity NULL\n");
exit(1);
}
parity=*tmp;
tmp=strtok(NULL,":");
if(!tmp) {
fprintf(stderr,"stop bits NULL\n");
exit(1);
}
stop_bits=atoi(tmp);
tmp=strtok(NULL,":");
if(!tmp) {
fprintf(stderr,"length NULL\n");
exit(1);
}
length=atoi(tmp);
tmp=strtok(NULL,":");
if(!tmp) {
fprintf(stderr,"freq NULL\n");
exit(1);
}
printf("tmp=%s\n",tmp);
nap_time=max(nap_time,atoi(tmp));
printf("nap_time=%d\n",nap_time);
// tty=strsave("/dev/tty.usbserial-A600drA9");
modbus_t *ctx;
// ctx = modbus_new_rtu(tty, 9600, 'E', 8, 2);
ctx = modbus_new_rtu(tty, baud_rate, parity, length, stop_bits);
(void) memset(results,0x00,sizeof(results));
strcpy(sql,"select distinct(rtu) from data ;");
rc = sqlite3_exec(db, sql, callback, 0, &zErrMsg);
if( rc!=SQLITE_OK ) {
fprintf(stderr, "SQL error: %s\n", zErrMsg);
sqlite3_free(zErrMsg);
}
/*
printf("=================================\n");
printf("res=%s\n",results);
printf("=================================\n");
*/
(void *)memset( &rtuArray[0],0,sizeof(rtuArray));
tmp=strtok(results,";");
while(tmp != 0) {
i=atoi(tmp);
printf("%d\n",i);
rtuArray[i]=-1;
tmp=strtok(NULL,";");
}
if (ctx == NULL) {
fprintf(stderr, "Unable to allocate libmodbus context\n");
return -1;
}
while (TRUE) {
strcpy(sql,"select freq from config order by idx desc limit 1;");
memset( results,0,sizeof(results));
rc = sqlite3_exec(db, sql, callback, 0, &zErrMsg);
nap_time = max(atoi(results), 30);
printf(">>%d\n",atoi(results) );
for(i=1;i<MAX_RTU;i++) {
if( rtuArray[i] == -1 ) {
rtu=i;
// modbus_set_debug(ctx, TRUE);
if (modbus_connect(ctx) == -1) {
fprintf(stderr, "Connection failed: %s\n",modbus_strerror(errno));
modbus_free(ctx);
return -1;
}
// nb_points = (UT_REGISTERS_NB > UT_INPUT_REGISTERS_NB) ? UT_REGISTERS_NB : UT_INPUT_REGISTERS_NB;
tab_rp_registers = (uint16_t *) malloc(nb_points * sizeof(uint16_t));
memset(tab_rp_registers, 0, nb_points * sizeof(uint16_t));
/*
* Set modbus RTU id.
*/
modbus_set_slave(ctx, rtu);
retries=RETRIES;
while (retries > 0) {
printf("RTU=%d\nretries=%d\n",rtu,retries);
// rc = modbus_read_registers(ctx, 00, nb_points, tab_rp_registers);
modbus_base_address=88;
rc = modbus_read_input_registers(ctx, 00, nb_points, tab_rp_registers);
printf("modbus_read_registers: %d\n",rc);
if (rc > 0) {
retries = 0;
dump_packet(tab_rp_registers, nb_points);
printf("\n");
printf("L1 %4d Watts\n", tab_rp_registers[L1_POWER - modbus_base_address] );
printf("L2 %4d Watts\n", tab_rp_registers[L2_POWER - modbus_base_address] );
printf("L3 %4d Watts\n", tab_rp_registers[L3_POWER - modbus_base_address] );
printf("=============================\n");
/*
printf("L1 %4d Volts\n", tab_rp_registers[L1_VOLTS] );
printf("L2 %4d Volts\n", tab_rp_registers[L2_VOLTS] );
printf("L3 %4d Volts\n", tab_rp_registers[L3_VOLTS] );
printf("=============================\n");
*/
sprintf(sql,"insert into data (RTU,phaseA,phaseB,phaseC) values( %d,%d,%d,%d);", rtu,tab_rp_registers[0],tab_rp_registers[1],tab_rp_registers[3]);
printf("%s\n", sql);
rc = sqlite3_exec(db, sql, callback, 0, &zErrMsg);
if( rc!=SQLITE_OK ) {
fprintf(stderr, "SQL error: %s\n", zErrMsg);
sqlite3_free(zErrMsg);
}
/*
modbus_base_address=L1_POWER;
rc = modbus_read_input_registers(ctx, modbus_base_address, nb_points, tab_rp_registers);
if ( rc > 0) {
printf("---->>>>>>> modbus_read_registers: %d\n",rc);
dump_packet(tab_rp_registers, nb_points);
printf("\n");
printf("L1 %4d Watts\n", tab_rp_registers[L1_POWER - modbus_base_address] );
printf("L2 %4d Watts\n", tab_rp_registers[L2_POWER - modbus_base_address] );
printf("L3 %4d Watts\n", tab_rp_registers[L3_POWER - modbus_base_address] );
printf("=============================\n");
sprintf(sql,"insert into data (RTU,phaseAKWH,phaseBKWH,phaseCKWH) values( %d,%d,%d,%d);",
rtu,
tab_rp_registers[0],
tab_rp_registers[1],
tab_rp_registers[2]
);
printf("%s\n", sql);
rc = sqlite3_exec(db, sql, callback, 0, &zErrMsg);
if( rc!=SQLITE_OK ) {
fprintf(stderr, "SQL error: %s\n", zErrMsg);
sqlite3_free(zErrMsg);
}
}
*/
} else {
printf("FAILED RTU %d)\n", rtu);
retries--;
sleep(1);
}
}
modbus_close( ctx );
}
}
printf("Sleeping for %d\n",nap_time);
sleep( nap_time);
}
sqlite3_close(db);
return(0);
}
/* At each loop, the program works in the range ADDRESS_START to
* ADDRESS_END then ADDRESS_START + 1 to ADDRESS_END and so on.
*/
int main(void)
{
modbus_t *ctx;
int rc;
int nb_fail;
int nb_loop;
int addr;
int nb;
uint16_t *tab_rp_registers;
char query[100];
int reg_address;
/* RTU */
ctx = modbus_new_rtu("/dev/ttyUSB0", 9600, 'N', 8, 1);
modbus_set_slave(ctx, SERVER_ID);
if (modbus_connect(ctx) == -1) {
fprintf(stderr, "Connection failed: %s\n",
modbus_strerror(errno));
modbus_free(ctx);
return -1;
}
MYSQL *con = mysql_init(NULL);
if (con == NULL)
{
fprintf(stderr, "%s\n", mysql_error(con));
exit(1);
}
if (mysql_real_connect(con, "localhost", "root", "root",
NULL, 0, NULL, 0) == NULL)
{
fprintf(stderr, "%s\n", mysql_error(con));
mysql_close(con);
exit(1);
}
if(mysql_select_db(con, "embedded")==0)/*success*/
printf( "Database Selected\n");
else
printf( "Failed to connect to Database: Error: \n");
if (mysql_query(con, "select address from configuration")) {
fprintf(stderr, "%s\n", mysql_error(con));
exit(1);
}
MYSQL_RES * res = mysql_use_result(con);
nb = mysql_num_rows(res);
tab_rp_registers = (uint16_t *) malloc(nb * sizeof(uint16_t));
memset(tab_rp_registers, 0, nb * sizeof(uint16_t));
MYSQL_ROW row;
/* output table name */
printf("Addresses from database:\n");
while ((row = mysql_fetch_row(res)) != NULL) {
reg_address = atoi(row[0]);
addr = reg_address - 30000;
printf("%i \n", addr);
rc = modbus_read_input_registers(ctx, addr, 1, tab_rp_registers);
if (rc == -1) {
printf("ERROR modbus_read_input_registers (%d)\n", rc);
nb_fail++;
} else{
printf("Address = %d, value %d \n",addr, tab_rp_registers[0]);
}
sprintf(query, "INSERT INTO solar_panel_data (address, value) VALUES (%i, %d)", reg_address, tab_rp_registers[0]);
printf("%s\n", query);
/* if (mysql_query(con, "INSERT INTO solar_panel_data (address, value) VALUES (30001, 65273)")) {
printf("ERROR writing to database");
}
*/ }
mysql_free_result(res);
//do the queries in to matrix
if (mysql_query(con, "INSERT INTO solar_panel_data (address, value) VALUES (30001, 65273)")) {
printf("ERROR writing to database");
}
/* Free the memory */
free(tab_rp_registers);
/* Close the connection */
modbus_close(ctx);
modbus_free(ctx);
mysql_close(con);
return 0;
}
void TDefaultModbusContext::ReadInputRegisters(int addr, int nb, uint16_t *dest)
{
if (modbus_read_input_registers(InnerContext, addr, 1, dest) < nb)
throw TModbusException("failed to read " + std::to_string(nb) +
" input register(s) @ " + std::to_string(addr));
}
