Skip to content
Snippets Groups Projects
Commit cb09532f authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Used multi-socket interface.

parent 502b35df
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
config/params.yaml
+ 1
− 1
View file @ cb09532f
rate: 10
modbus_address: 127.0.0.1
modbus_address: 10.70.13.77
modbus_port: 5020
map_frame: map
home_x: 0.0
......
include/looming_modbus_driver.h
+ 6
− 4
View file @ cb09532f
......@@ -60,19 +60,22 @@ class LoomingModbusDriver : public iri_base_driver::IriBaseDriver
modbus_t *context;
modbus_mapping_t *mapping;
int server_socket;
int client_socket;
bool connected;
std::vector<int> client_sockets;
int status;
CMutex access;
/* thread attributes */
CThreadServer *thread_server;
std::string request_thread_id;
std::string new_clients_thread_id;
/* event attributes */
CEventServer *event_server;
std::string finish_thread_event_id;
std::string finish_request_thread_event_id;
std::string finish_new_clients_thread_event_id;
CEvent new_client_event;
protected:
static void *request_thread(void *param);
static void *new_clients_thread(void *param);
public:
/**
......@@ -164,7 +167,6 @@ class LoomingModbusDriver : public iri_base_driver::IriBaseDriver
// here define all looming_modbus_driver interface methods to retrieve and set
// the driver parameters
bool client_connected(void);
void set_ready(void);
void set_navigating(void);
void set_success(void);
......
src/looming_modbus_driver.cpp
+ 101
− 54
View file @ cb09532f
......@@ -2,23 +2,28 @@
#include "looming_registers.h"
#include <sys/select.h>
LoomingModbusDriver::LoomingModbusDriver(void)
LoomingModbusDriver::LoomingModbusDriver(void) : new_client_event("modbus_new_client_event")
{
//setDriverId(driver string id);
this->server_socket=-1;
this->context=NULL;
this->mapping=NULL;
this->connected=false;
this->status=0x03;
this->client_sockets.clear();
/* initialize events */
this->event_server=CEventServer::instance();
this->finish_thread_event_id="modbus_finish_thread_event";
this->event_server->create_event(this->finish_thread_event_id);
this->finish_request_thread_event_id="modbus_finish_request_thread_event";
this->event_server->create_event(this->finish_request_thread_event_id);
this->finish_new_clients_thread_event_id="modbus_finish_new_clients_thread_event";
this->event_server->create_event(this->finish_new_clients_thread_event_id);
/* initialize threads */
this->thread_server=CThreadServer::instance();
this->request_thread_id="modbus_request_thread";
this->thread_server->create_thread(this->request_thread_id);
this->thread_server->attach_thread(this->request_thread_id,this->request_thread,this);
this->new_clients_thread_id="new_clients_thread";
this->thread_server->create_thread(this->new_clients_thread_id);
this->thread_server->attach_thread(this->new_clients_thread_id,this->new_clients_thread,this);
}
bool LoomingModbusDriver::openDriver(void)
......@@ -90,6 +95,7 @@ bool LoomingModbusDriver::startDriver(void)
}
// start thread
this->thread_server->start_thread(this->request_thread_id);
this->thread_server->start_thread(this->new_clients_thread_id);
return true;
}
......@@ -99,9 +105,15 @@ bool LoomingModbusDriver::stopDriver(void)
if(this->thread_server->get_thread_state(this->request_thread_id)==starting ||
this->thread_server->get_thread_state(this->request_thread_id)==active)
{
this->event_server->set_event(this->finish_thread_event_id);
this->event_server->set_event(this->finish_request_thread_event_id);
this->thread_server->end_thread(this->request_thread_id);
}
if(this->thread_server->get_thread_state(this->new_clients_thread_id)==starting ||
this->thread_server->get_thread_state(this->new_clients_thread_id)==active)
{
this->event_server->set_event(this->finish_new_clients_thread_event_id);
this->thread_server->end_thread(this->new_clients_thread_id);
}
// close socket
if(this->server_socket!=-1)
close(this->server_socket);
......@@ -132,87 +144,111 @@ void LoomingModbusDriver::config_update(Config& new_cfg, uint32_t level)
this->unlock();
}
void *LoomingModbusDriver::request_thread(void *param)
void *LoomingModbusDriver::new_clients_thread(void *param)
{
LoomingModbusDriver *modbus=(LoomingModbusDriver *)param;
fd_set receive_set;
timeval time;
uint8_t req[MODBUS_TCP_MAX_ADU_LENGTH];
int num;
int new_client_socket,num;
bool end=false;
while(!end)
{
if(modbus->connected==false)
FD_ZERO(&receive_set);
FD_SET(modbus->server_socket,&receive_set);
time.tv_sec=0;
time.tv_usec=100000;
if((num=select(modbus->server_socket+1,&receive_set,NULL,NULL,&time))!=-1)
{
FD_ZERO(&receive_set);
FD_SET(modbus->server_socket,&receive_set);
time.tv_sec=0;
time.tv_usec=100000;
if((num=select(modbus->server_socket+1,&receive_set,NULL,NULL,&time))!=-1)
if(FD_ISSET(modbus->server_socket,&receive_set))
{
if(FD_ISSET(modbus->server_socket,&receive_set))
if((new_client_socket=modbus_tcp_accept(modbus->context,&modbus->server_socket))==-1)
ROS_WARN_STREAM("Impossible to accept client connection: " << modbus_strerror(errno));
else
{
if((modbus->client_socket=modbus_tcp_accept(modbus->context,&modbus->server_socket))==-1)
ROS_WARN_STREAM("Impossible to accept client connection: " << modbus_strerror(errno));
else
{
modbus->access.enter();
std::cout << "connected" << std::endl;
modbus->connected=true;
modbus->access.exit();
}
modbus->access.enter();
std::cout << "connected new client: " << new_client_socket << std::endl;
modbus->client_sockets.push_back(new_client_socket);
modbus->new_client_event.set();
modbus->access.exit();
}
}
}
else
if(modbus->event_server->event_is_set(modbus->finish_new_clients_thread_event_id))
end=true;
}
pthread_exit(NULL);
}
void *LoomingModbusDriver::request_thread(void *param)
{
LoomingModbusDriver *modbus=(LoomingModbusDriver *)param;
fd_set receive_set;
timeval time;
uint8_t req[MODBUS_TCP_MAX_ADU_LENGTH];
int num,max_id;
bool end=false;
FD_ZERO(&receive_set);
FD_SET(modbus->new_client_event.get_fd(),&receive_set);
max_id=modbus->new_client_event.get_fd()+1;
time.tv_sec=0;
time.tv_usec=100000;
while(!end)
{
if((num=select(max_id,&receive_set,NULL,NULL,&time))!=-1)
{
// handle request
FD_ZERO(&receive_set);
FD_SET(modbus->client_socket,&receive_set);
time.tv_sec=0;
time.tv_usec=1000000;
if((num=select(modbus->client_socket+1,&receive_set,NULL,NULL,&time))!=-1)
modbus->access.enter();
for(std::vector<int>::iterator it=modbus->client_sockets.begin();it!=modbus->client_sockets.end();)
{
if(FD_ISSET(modbus->client_socket,&receive_set))
{
modbus->access.enter();
modbus_set_socket(modbus->context,modbus->client_socket);
if(FD_ISSET(*it,&receive_set))
{
modbus_set_socket(modbus->context,*it);
if((num=modbus_receive(modbus->context,req))==-1)
{
ROS_WARN_STREAM("Error while receiving request: " << modbus_strerror(errno));
close(modbus->client_socket);
modbus->connected=false;
ROS_WARN_STREAM("Error while receiving request on socket " << *it << ": " << modbus_strerror(errno));
close(*it);
it=modbus->client_sockets.erase(it);
}
else if(num>0)
{
modbus->mapping->tab_input_registers[STATUS_REG]=modbus->status;
modbus_reply(modbus->context,req,num,modbus->mapping);
it++;
}
else
{
ROS_WARN_STREAM("Client disconnected");
close(modbus->client_socket);
modbus->connected=false;
ROS_WARN_STREAM("Client socket " << *it << " disconnected");
close(*it);
it=modbus->client_sockets.erase(it);
}
modbus->access.exit();
}
}
else
it++;
}
else
ROS_WARN_STREAM("timeout while waiting to receive a request" << modbus_strerror(errno));
if(modbus->new_client_event.is_set())
modbus->new_client_event.reset();
FD_ZERO(&receive_set);
FD_SET(modbus->new_client_event.get_fd(),&receive_set);
max_id=modbus->new_client_event.get_fd()+1;
for(unsigned int i=0;i<modbus->client_sockets.size();i++)
{
FD_SET(modbus->client_sockets[i],&receive_set);
if((modbus->client_sockets[i]+1)>max_id)
max_id=modbus->client_sockets[i]+1;
}
modbus->access.exit();
}
if(modbus->event_server->event_is_set(modbus->finish_thread_event_id))
time.tv_sec=0;
time.tv_usec=100000;
if(modbus->event_server->event_is_set(modbus->finish_request_thread_event_id))
end=true;
}
pthread_exit(NULL);
}
bool LoomingModbusDriver::client_connected(void)
{
return this->connected;
}
void LoomingModbusDriver::set_ready(void)
{
if(this->mapping!=NULL)
......@@ -321,12 +357,23 @@ LoomingModbusDriver::~LoomingModbusDriver(void)
if(this->thread_server->get_thread_state(this->request_thread_id)==starting ||
this->thread_server->get_thread_state(this->request_thread_id)==active)
{
this->event_server->set_event(this->finish_thread_event_id);
this->event_server->set_event(this->finish_request_thread_event_id);
this->thread_server->end_thread(this->request_thread_id);
}
this->thread_server->detach_thread(this->request_thread_id);
this->thread_server->delete_thread(this->request_thread_id);
this->request_thread_id="";
this->event_server->delete_event(this->finish_thread_event_id);
this->finish_thread_event_id="";
if(this->thread_server->get_thread_state(this->new_clients_thread_id)==starting ||
this->thread_server->get_thread_state(this->new_clients_thread_id)==active)
{
this->event_server->set_event(this->finish_new_clients_thread_event_id);
this->thread_server->end_thread(this->request_thread_id);
}
this->thread_server->detach_thread(this->new_clients_thread_id);
this->thread_server->delete_thread(this->new_clients_thread_id);
this->new_clients_thread_id="";
this->event_server->delete_event(this->finish_request_thread_event_id);
this->finish_request_thread_event_id="";
this->event_server->delete_event(this->finish_new_clients_thread_event_id);
this->finish_new_clients_thread_event_id="";
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment