39#include "EOYtheSystem.h"
41#include "EOropframe.h"
44#include <yarp/os/Network.h>
45#include <yarp/os/NetType.h>
46#include <ace/Time_Value.h>
53using namespace yarp::os;
54using namespace yarp::os::impl;
83std::mutex TheEthManager::managerSem {};
84std::mutex TheEthManager::txSem {};
85std::mutex TheEthManager::rxSem {};
89TheEthManager::TheEthManager()
92 communicationIsInitted =
false;
99 TheEthManager::initEOYsystem();
102 ipv4local.addr = eo_common_ipv4addr(10, 0, 1, 104);
103 ipv4local.port = 12345;
106 startUpTime = yarp::os::Time::now();
117TheEthManager::~TheEthManager()
125 if(isCommunicationInitted())
129 yarp::os::Time::delay(0.250);
130 stopCommunicationThreads();
136 communicationIsInitted =
false;
164 std::lock_guard<std::mutex> lck(managerSem);
167 yTrace() <<
"Calling EthManager Constructor";
170 yError() <<
"While calling EthManager constructor";
188void embOBJerror(eOerrmanErrorType_t errtype,
const char *info, eOerrmanCaller_t *caller,
const eOerrmanDescriptor_t *des)
190 const char defobjstr[] =
"EO?";
191 const char *eobjstr = (NULL == caller) ? (defobjstr) : (caller->eobjstr);
193 yError() <<
"embOBJerror(): errtype = " << eo_errman_ErrorStringGet(eo_errman_GetHandle(), errtype) <<
"from EOobject = " << eobjstr <<
" w/ message = " << info;
195 if(errtype == eo_errortype_fatal)
197 yError() <<
"embOBJerror(): FATAL ERROR: the calling thread shall now be stopped in a forever loop here inside";
203void TheEthManager::initEOYsystem(
void)
206 eOerrman_cfg_t errmanconfig = {0};
209 const eOmempool_cfg_t *mpoolcfg = NULL;
210 eoy_sys_Initialise(syscfg, mpoolcfg, &errmanconfig);
218 if((NULL == r) || (NULL ==
p) || (r->
isFake()))
226 uint16_t numofbytes = 0;
227 uint16_t numofrops = 0;
228 uint8_t* data2send = NULL;
229 bool transmitthepacket = r->getTXpacket(&data2send, numofbytes, numofrops);
231 if(
true == transmitthepacket)
233 eOipv4addressing_t ipv4addressing;
234 r->getIPv4addressing(ipv4addressing);
235 ethman->
sendPacket(data2send,
static_cast<size_t>(numofbytes), ipv4addressing);
239 eOipv4addressing_t ipv4addressing;
240 size_t numofbytes = 0;
241 uint16_t numofrops = 0;
242 const void * data2send = r->
getUDPtransmit(ipv4addressing, numofbytes, numofrops);
244 if(
nullptr != data2send)
246 ethman->
sendPacket(data2send, numofbytes, ipv4addressing);
267 if((NULL == r) || (NULL ==
p))
288 yError() <<
"TheEthManager::verifyEthBoardInfo() fails";
303bool TheEthManager::initCommunication(yarp::os::Searchable &cfgtotal)
308 yError() <<
"TheEthManager::initCommunication() fails";
315 int txrate = pc104data.
txrate;
316 int rxrate = pc104data.
rxrate;
321 if(
false == createCommunicationObjects(tmpaddress, txrate, rxrate) )
323 yError () <<
"TheEthManager::initCommunication() cannot create communication objects";
328 ipv4local.addr = tmpaddress.addr;
329 ipv4local.port = tmpaddress.port;
338 if(
false == communicationIsInitted)
340 yTrace() <<
"TheEthManager::requestResource2(): we need to init the communication";
342 if(
false == initCommunication(cfgtotal))
344 yError() <<
"TheEthManager::requestResource2(): cannot init the communication";
352 yError() <<
"TheEthManager::requestResource2() fails";
369 if(
true == embBoardsConnected)
378 if(
true == rr->
open2(ipv4addr, cfgtotal))
384 yError() <<
"TheEthManager::requestResource2(): error creating a new ethResource for IP = " << bdata.
properties.
ipv4string;
412 if((NULL == ethresource) || (NULL == interface))
414 yError() <<
"TheEthManager::releaseResource2(): there is an attempt to release a NULL EthResource or IethResource";
424 eOmn_serv_category_t category = eomn_serv_category_all;
429 category = eomn_serv_category_mais;
434 category = eomn_serv_category_strain;
439 category = eomn_serv_category_mc;
444 category = eomn_serv_category_skin;
449 category = eomn_serv_category_none;
454 category = eomn_serv_category_inertials3;
459 category = eomn_serv_category_none;
468 category = eomn_serv_category_all;
470 if(eomn_serv_category_none != category)
518 return interfacePointer;
526 return interfacePointer;
531 return(yarp::os::Time::now() - startUpTime);
536bool TheEthManager::createCommunicationObjects(
const eOipv4addressing_t &localaddress,
int txrate,
int rxrate)
540 ACE_INET_Addr inetaddr =
toaceinet(localaddress);
542 if(!communicationIsInitted)
544 UDP_socket =
new ACE_SOCK_Dgram();
545 if((embBoardsConnected) && (-1 == UDP_socket->open(inetaddr)))
548 inetaddr.addr_to_string(tmp, 64);
549 yError() <<
"\n/--------------------------------------------------------------------------------------------------------------\\"
550 <<
"\n| TheEthManager::createCommunicationObjects() is unable to bind to local IP address " << tmp
551 <<
"\n\\--------------------------------------------------------------------------------------------------------------/";
554 communicationIsInitted =
false;
558 communicationIsInitted =
true;
559 ipv4local = localaddress;
572 sender->
config(UDP_socket,
this);
573 receiver->
config(UDP_socket,
this);
580 ret1 = sender->start();
581 ret2 = receiver->start();
585 yError() <<
"TheEthManager::createCommunicationObjects() fails in starting UDP communication threads ethSender / ethReceiver";
588 stopCommunicationThreads();
591 communicationIsInitted =
false;
596 yTrace() <<
"TheEthManager::createCommunicationObjects(): both UDP communication threads ethSender / ethReceiver start correctly!";
603 return communicationIsInitted;
607bool TheEthManager::isCommunicationInitted(
void)
612 ret = communicationIsInitted;
619bool TheEthManager::stopCommunicationThreads()
623 if(sender->isRunning())
627 if(receiver->isRunning())
638 ACE_INET_Addr inetaddr =
toaceinet(toaddressing);
639 ssize_t ret = UDP_socket->send(udpframe, len, inetaddr);
647 ACE_UINT32 a32 = aceinetaddr.get_ip_address();
648 uint8_t ip4 = a32 & 0xff;
649 uint8_t ip3 = (a32 >> 8) & 0xff;
650 uint8_t ip2 = (a32 >> 16) & 0xff;
651 uint8_t ip1 = (a32 >> 24) & 0xff;
652 return eo_common_ipv4addr(ip1, ip2, ip3, ip4);
657 uint8_t ip1, ip2, ip3, ip4;
658 eo_common_ipv4addr_to_decimal(ipv4addressing.addr, &ip1, &ip2, &ip3, &ip4);
659 ACE_UINT32 hostip = (ip1 << 24) | (ip2 << 16) | (ip3 << 8) | (ip4);
660 ACE_INET_Addr myIP((u_short)ipv4addressing.port, hostip);
672 if((size >=0) && (NULL != r) && (!r->
isFake()))
678 yError() <<
"TheEthManager::Reception() cannot give a received packet of size" << size <<
"to EthResource because EthResource::processRXpacket() returns false.";
714bool TheEthManager::lock(
bool on)
729bool TheEthManager::lockTX(
bool on)
744bool TheEthManager::lockRX(
bool on)
758bool TheEthManager::lockTXRX(
bool on)
virtual const void * getUDPtransmit(eOipv4addressing_t &destination, size_t &sizeofpacket, uint16_t &numofrops)=0
virtual const Properties & getProperties()=0
virtual bool serviceStop(eOmn_serv_category_t category, double timeout=0.500)=0
virtual bool processRXpacket(const void *data, const size_t size)=0
virtual bool open2(eOipv4addr_t remIP, yarp::os::Searchable &cfgtotal)=0
size_t number_of_interfaces(eth::AbstractEthResource *res)
const string & name(eOipv4addr_t ipv4)
eth::IethResource * get_interface(eOipv4addr_t ipv4, eOprotID32_t id32)
bool execute(void(*action)(eth::AbstractEthResource *res, void *p), void *par)
bool add(eth::AbstractEthResource *res)
bool rem(eth::AbstractEthResource *res)
size_t number_of_resources(void)
eth::AbstractEthResource * get_resource(eOipv4addr_t ipv4)
bool config(ACE_SOCK_Dgram *pSocket, eth::TheEthManager *_ethManager)
bool config(ACE_SOCK_Dgram *pSocket, TheEthManager *_ethManager)
virtual iethresType_t type()=0
int releaseResource2(eth::AbstractEthResource *ethresource, IethResource *interface)
bool verifyEthBoardInfo(yarp::os::Searchable &cfgtotal, eOipv4addr_t &boardipv4, string boardipv4string, string boardname)
int sendPacket(const void *udpframe, size_t len, const eOipv4addressing_t &toaddressing)
eOipv4addr_t toipv4addr(const ACE_INET_Addr &aceinetaddr)
eth::AbstractEthResource * getEthResource(eOipv4addr_t ipv4)
IethResource * getInterface(eOipv4addr_t ipv4, eOprotID32_t id32)
static bool killYourself()
eth::EthBoards * ethBoards
bool Reception(eOipv4addr_t from, uint64_t *data, ssize_t size)
const string & getName(eOipv4addr_t ipv4)
static TheEthManager * instance()
ACE_INET_Addr toaceinet(const eOipv4addressing_t &ipv4addressing)
eth::AbstractEthResource * requestResource2(IethResource *interface, yarp::os::Searchable &cfgtotal)
int getNumberOfResources(void)
const eOipv4addressing_t & getLocalIPV4addressing(void)
void embOBJerror(eOerrmanErrorType_t errtype, const char *info, eOerrmanCaller_t *caller, const eOerrmanDescriptor_t *des)
void ethEvalPresence(eth::AbstractEthResource *r, void *p)
void delete_resources(eth::AbstractEthResource *p, void *par)
void ethEvalTXropframe(eth::AbstractEthResource *r, void *p)
const eOysystem_cfg_t * feat_getSYSconfig()
void feat_Initialise(void *handleOfTheEthManager)
bool read(yarp::os::Searchable &cfgtotal, pc104Data &pc104data)
bool print(const pc104Data &pc104data)
@ iethres_analoginertial3
boardProperties properties
eOipv4addressing_t ipv4addressing
eOipv4addressing_t localaddressing