int modbus_close_device(modbus_t* context)
{
if (modbus_flush(context) < 0)
{
my_log_error("modbus_close_rtu_device");
return -1;
}
modbus_close(context);
modbus_free(context);
return 0;
}
static ssize_t _modbus_rtu_send(modbus_t *ctx, const uint8_t *req, int req_length)
{
#if defined(_WIN32)
modbus_rtu_t *ctx_rtu = ctx->backend_data;
DWORD n_bytes = 0;
return (WriteFile(ctx_rtu->w_ser.fd, req, req_length, &n_bytes, NULL)) ? (ssize_t)n_bytes : -1;
#else
#if HAVE_DECL_TIOCM_RTS
modbus_rtu_t *ctx_rtu = ctx->backend_data;
if (ctx_rtu->rts != MODBUS_RTU_RTS_NONE) {
ssize_t size;
if (ctx->debug) {
fprintf(stderr, "Sending request using RTS signal\n");
}
GPIO_SET = 1 << 24; /* Turn on yellow LED to signal traffic */
_modbus_rtu_ioctl_rts(ctx->s, ctx_rtu->rts == MODBUS_RTU_RTS_UP);
usleep(rts_delay_before);
size = write(ctx->s, req, req_length);
usleep(ctx_rtu->onebyte_time * req_length + rts_delay_after);
_modbus_rtu_ioctl_rts(ctx->s, ctx_rtu->rts != MODBUS_RTU_RTS_UP);
GPIO_CLR = 1 << 24; /* Turn off yellow LED */
return size;
} else {
#endif
ssize_t size;
GPIO_SET = 1 << 17; /* Set p1-11 high to set DE high while sending */
GPIO_SET = 1 << 24; /* Turn on yellow LED to signal traffic */
usleep(1000); /* Temp: sleep 1ms */
size = write(ctx->s, req, req_length);
modbus_flush(ctx);
usleep(1000); /* Temp: sleep 1 ms */
GPIO_CLR = 1 << 17; /* Set p1-11 low to allow response */
GPIO_CLR = 1 << 24; /* Message sent, turn off yellow LED */
return size;
#if HAVE_DECL_TIOCM_RTS
}
#endif
#endif
}
int _sleep_and_flush(modbus_t *ctx)
{
#ifdef _WIN32
/* usleep doesn't exist on Windows */
Sleep((ctx->response_timeout.tv_sec * 1000) +
(ctx->response_timeout.tv_usec / 1000));
#else
/* usleep source code */
struct timespec request, remaining;
request.tv_sec = ctx->response_timeout.tv_sec;
request.tv_nsec = ((long int)ctx->response_timeout.tv_usec % 1000000)
* 1000;
while (nanosleep(&request, &remaining) == -1 && errno == EINTR)
request = remaining;
#endif
return modbus_flush(ctx);
}
bool RbCtrlIface::connectModbus( int retryCount/*=-1*/)
{
// >>>>> Simulation?
if(mSimulActive)
{
ROS_WARN_STREAM( "ModBus replies are simulated!" );
ros::Duration(1).sleep(); // sleep for a second
return true;
}
// <<<<< Simulation?
if( !mModbus )
{
ROS_FATAL_STREAM( "ModBus data structure not initialized!" );
return false;
}
// Closing to reset active connections
//modbus_close( mModbus);
if( modbus_connect( mModbus ) == -1 )
{
ROS_FATAL_STREAM( "Modbus connection failed" );
mBoardConnected = false;
return false;
}
int res=-1;
// res = modbus_flush( mModbus );
ros::Duration(1).sleep(); // sleep for a second
timeval new_timeout;
new_timeout.tv_sec = 2;
new_timeout.tv_usec = 0;
modbus_set_response_timeout( mModbus, &new_timeout );
modbus_set_byte_timeout( mModbus, &new_timeout );
res = modbus_set_slave( mModbus, mBoardIdx );
if( res != 0 )
{
ROS_FATAL_STREAM( "modbus_set_slave error -> " << modbus_strerror( errno ) );
modbus_flush( mModbus );
mBoardConnected = false;
return false;
}
int tryCount=0;
bool ok = false;
mBoardConnected = true;
while(1)
{
ROS_WARN_STREAM( "- testBoardConnection - Attempt: " << (tryCount+1) );
ok = testBoardConnection();
tryCount++;
if( tryCount==retryCount || ok )
break;
else
ROS_WARN_STREAM( "Trying again...");
}
if( !ok )
{
ROS_FATAL_STREAM( "Error on modbus: " << modbus_strerror( errno ) );
mBoardConnected = false;
return false;
}
return true;
}
modbus_close((modbus_t *)(SYS_INT)PARAM(0));
RETURN_NUMBER(0)
END_IMPL
//------------------------------------------------------------------------
CONCEPT_FUNCTION_IMPL(modbus_free, 1)
T_HANDLE(modbus_free, 0) // modbus_t*
modbus_free((modbus_t *)(SYS_INT)PARAM(0));
RETURN_NUMBER(0)
END_IMPL
//------------------------------------------------------------------------
CONCEPT_FUNCTION_IMPL(modbus_flush, 1)
T_HANDLE(modbus_flush, 0) // modbus_t*
RETURN_NUMBER(modbus_flush((modbus_t *)(SYS_INT)PARAM(0)))
END_IMPL
//------------------------------------------------------------------------
CONCEPT_FUNCTION_IMPL(modbus_set_debug, 2)
T_HANDLE(modbus_set_debug, 0) // modbus_t*
T_NUMBER(modbus_set_debug, 1) // int
modbus_set_debug((modbus_t *)(SYS_INT)PARAM(0), (int)PARAM(1));
RETURN_NUMBER(0)
END_IMPL
//------------------------------------------------------------------------
CONCEPT_FUNCTION_IMPL(modbus_strerror, 1)
T_NUMBER(modbus_strerror, 0) // int
RETURN_STRING((char *)modbus_strerror((int)PARAM(0)))
END_IMPL
void *link_loop_and_logic(void *thrd_link_num)
{
char *fnct_name = "link_loop_and_logic";
int ret, ret_available, ret_connected;
int tx_counter;
mb_tx_t *this_mb_tx = NULL;
int this_mb_tx_num;
mb_link_t *this_mb_link = NULL;
int this_mb_link_num;
if (thrd_link_num == NULL) {
ERR(gbl.init_dbg, "NULL pointer");
return NULL;
}
this_mb_link_num = *((int *)thrd_link_num);
if (this_mb_link_num < 0 || this_mb_link_num >= gbl.tot_mb_links) {
ERR(gbl.init_dbg, "parameter out of range this_mb_link_num[%d]", this_mb_link_num);
return NULL;
}
this_mb_link = &gbl.mb_links[this_mb_link_num];
while (1) {
for (tx_counter = 0; tx_counter < gbl.tot_mb_tx; tx_counter++) {
if (gbl.quit_flag != 0) { //tell the threads to quit (SIGTERM o SGIQUIT) (unloadusr mb2hal).
return NULL;
}
this_mb_tx_num = tx_counter;
this_mb_tx = &gbl.mb_tx[this_mb_tx_num];
DBG(this_mb_tx->cfg_debug, "mb_tx_num[%d] mb_links[%d] thread[%d] fd[%d] going to TEST availability",
this_mb_tx_num, this_mb_tx->mb_link_num, this_mb_link_num, modbus_get_socket(this_mb_link->modbus));
//corresponding link and time (update_rate)
if (is_this_tx_ready(this_mb_link_num, this_mb_tx_num, &ret_available) != retOK) {
ERR(this_mb_tx->cfg_debug, "mb_tx_num[%d] mb_links[%d] thread[%d] fd[%d] is_this_tx_ready ERR",
this_mb_tx_num, this_mb_tx->mb_link_num, this_mb_link_num, modbus_get_socket(this_mb_link->modbus));
return NULL;
}
if (ret_available == 0) {
DBG(this_mb_tx->cfg_debug, "mb_tx_num[%d] mb_links[%d] thread[%d] fd[%d] NOT available",
this_mb_tx_num, this_mb_tx->mb_link_num, this_mb_link_num, modbus_get_socket(this_mb_link->modbus));
usleep(1000);
continue;
}
DBG(this_mb_tx->cfg_debug, "mb_tx_num[%d] mb_links[%d] thread[%d] fd[%d] going to TEST connection",
this_mb_tx_num, this_mb_tx->mb_link_num, this_mb_link_num, modbus_get_socket(this_mb_link->modbus));
//first time connection or reconnection, run time parameters setting
if (get_tx_connection(this_mb_tx_num, &ret_connected) != retOK) {
ERR(this_mb_tx->cfg_debug, "mb_tx_num[%d] mb_links[%d] thread[%d] fd[%d] get_tx_connection ERR",
this_mb_tx_num, this_mb_tx->mb_link_num, this_mb_link_num, modbus_get_socket(this_mb_link->modbus));
return NULL;
}
if (ret_connected == 0) {
DBG(this_mb_tx->cfg_debug, "mb_tx_num[%d] mb_links[%d] thread[%d] fd[%d] NOT connected",
this_mb_tx_num, this_mb_tx->mb_link_num, this_mb_link_num, modbus_get_socket(this_mb_link->modbus));
usleep(1000);
continue;
}
DBG(this_mb_tx->cfg_debug, "mb_tx_num[%d] mb_links[%d] thread[%d] fd[%d] lk_dbg[%d] going to EXECUTE transaction",
this_mb_tx_num, this_mb_tx->mb_link_num, this_mb_link_num, modbus_get_socket(this_mb_link->modbus),
this_mb_tx->protocol_debug);
switch (this_mb_tx->mb_tx_fnct) {
case mbtx_02_READ_DISCRETE_INPUTS:
ret = fnct_02_read_discrete_inputs(this_mb_tx, this_mb_link);
break;
case mbtx_03_READ_HOLDING_REGISTERS:
ret = fnct_03_read_holding_registers(this_mb_tx, this_mb_link);
break;
case mbtx_04_READ_INPUT_REGISTERS:
ret = fnct_04_read_input_registers(this_mb_tx, this_mb_link);
break;
case mbtx_06_WRITE_SINGLE_REGISTER:
ret = fnct_06_write_single_register(this_mb_tx, this_mb_link);
break;
case mbtx_15_WRITE_MULTIPLE_COILS:
ret = fnct_15_write_multiple_coils(this_mb_tx, this_mb_link);
break;
case mbtx_16_WRITE_MULTIPLE_REGISTERS:
ret = fnct_16_write_multiple_registers(this_mb_tx, this_mb_link);
break;
default:
ret = -1;
ERR(this_mb_tx->cfg_debug, "case error with mb_tx_fnct %d [%s] in mb_tx_num[%d]",
this_mb_tx->mb_tx_fnct, this_mb_tx->mb_tx_fnct_name, this_mb_tx_num);
break;
}
if (gbl.quit_flag != 0) { //tell the threads to quit (SIGTERM o SGIQUIT) (unloadusr mb2hal).
return NULL;
}
if (ret != retOK && modbus_get_socket(this_mb_link->modbus) < 0) { //link failure
(*this_mb_tx->num_errors)++;
ERR(this_mb_tx->cfg_debug, "mb_tx_num[%d] mb_links[%d] thread[%d] fd[%d] link failure, going to close link",
this_mb_tx_num, this_mb_tx->mb_link_num, this_mb_link_num, modbus_get_socket(this_mb_link->modbus));
modbus_close(this_mb_link->modbus);
}
else if (ret != retOK) { //transaction failure but link OK
(**this_mb_tx->num_errors)++;
ERR(this_mb_tx->cfg_debug, "mb_tx_num[%d] mb_links[%d] thread[%d] fd[%d] transaction failure, num_errors[%d]",
this_mb_tx_num, this_mb_tx->mb_link_num, this_mb_link_num, modbus_get_socket(this_mb_link->modbus), **this_mb_tx->num_errors);
// Clear any unread data. Otherwise the link might get out of sync
modbus_flush(this_mb_link->modbus);
}
else { //transaction and link OK
OK(this_mb_tx->cfg_debug, "mb_tx_num[%d] mb_links[%d] thread[%d] fd[%d] transaction OK, update_HZ[%0.03f]",
this_mb_tx_num, this_mb_tx->mb_link_num, this_mb_link_num, modbus_get_socket(this_mb_link->modbus),
1.0/(get_time()-this_mb_tx->last_time_ok));
this_mb_tx->last_time_ok = get_time();
(**this_mb_tx->num_errors) = 0;
}
//set the next (waiting) time for update rate
this_mb_tx->next_time = get_time() + this_mb_tx->time_increment;
//wait time for serial lines
if (this_mb_tx->cfg_link_type == linkRTU) {
DBG(this_mb_tx->cfg_debug, "mb_tx_num[%d] mb_links[%d] thread[%d] fd[%d] SERIAL_DELAY_MS activated [%d]",
this_mb_tx_num, this_mb_tx->mb_link_num, this_mb_link_num, modbus_get_socket(this_mb_link->modbus),
this_mb_tx->cfg_serial_delay_ms);
usleep(this_mb_tx->cfg_serial_delay_ms * 1000);
}
//wait time to gbl.slowdown activity (debugging)
if (gbl.slowdown > 0) {
DBG(this_mb_tx->cfg_debug, "mb_tx_num[%d] mb_links[%d] thread[%d] fd[%d] gbl.slowdown activated [%0.3f]",
this_mb_tx_num, this_mb_tx->mb_link_num, this_mb_link_num, modbus_get_socket(this_mb_link->modbus), gbl.slowdown);
usleep(gbl.slowdown * 1000 * 1000);
}
} //end for
} //end while
return NULL;
}
int main(void)
{
int s = -1;
modbus_t *ctx;
modbus_t *ctx_rtu;
ctx = modbus_new_tcp("127.0.0.1", 1502);
/* modbus_set_debug(ctx, TRUE); */
ctx_rtu = modbus_new_rtu("/dev/ttyUSB1", 115200, 'N', 8, 1);
if (modbus_connect(ctx_rtu) == -1) {
fprintf(stderr, "Connection failed: %s\n", modbus_strerror(errno));
modbus_free(ctx_rtu);
modbus_free(ctx);
return -1;
}
s = modbus_tcp_listen(ctx, 1);
modbus_tcp_accept(ctx, &s);
for (;;) {
uint8_t query[MODBUS_TCP_MAX_ADU_LENGTH];
int query_length;
uint8_t response[MODBUS_RTU_MAX_ADU_LENGTH];
int rc;
int data_start_index;
int raw_length;
int exception;
int slave;
rc = modbus_receive(ctx, query);
if (rc > 0) {
exception = 0;
query_length = rc;
data_start_index = modbus_get_header_length(ctx) - 1;
slave = query[data_start_index];
raw_length = rc - data_start_index - modbus_get_checksum_length(ctx);
modbus_flush(ctx_rtu);
modbus_set_slave(ctx_rtu, slave);
if (modbus_send_raw_request(ctx_rtu, query + data_start_index, raw_length) != -1) {
rc = modbus_receive_confirmation(ctx_rtu, response);
if ( rc != -1) {
/* rc is the response size */
data_start_index = modbus_get_header_length(ctx_rtu) - 1;
raw_length = rc - data_start_index - modbus_get_checksum_length(ctx_rtu);
modbus_reply_raw_response(ctx, query, query_length, response + data_start_index, raw_length);
} else {
exception = errno;
}
} else {
exception = errno;
}
if (exception != 0) {
if (exception > MODBUS_ENOBASE && MODBUS_ENOBASE < (MODBUS_ENOBASE + MODBUS_EXCEPTION_MAX)) {
exception -= MODBUS_ENOBASE;
} else {
exception = EMBXSFAIL;
}
modbus_reply_exception(ctx, query, exception);
}
} else if (rc == -1) {
/* Connection closed by the client or error */
break;
}
}
printf("Quit the loop: %s\n", modbus_strerror(errno));
if (s != -1) {
close(s);
}
modbus_close(ctx_rtu);
modbus_free(ctx_rtu);
modbus_close(ctx);
modbus_free(ctx);
return 0;
}
void TDefaultModbusContext::Connect()
{
if (modbus_connect(InnerContext) != 0)
throw TModbusException("couldn't initialize modbus connection");
modbus_flush(InnerContext);
}
