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>
53 using namespace yarp::os;
54 using namespace yarp::os::impl;
83 std::mutex TheEthManager::managerSem {};
84 std::mutex TheEthManager::txSem {};
85 std::mutex TheEthManager::rxSem {};
89 TheEthManager::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();
117 TheEthManager::~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";
178 bool TheEthManager::killYourself()
188 void 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";
203 void 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);
253 bool TheEthManager::Transmission(
void)
267 if((NULL == r) || (NULL ==
p))
275 bool TheEthManager::CheckPresence(
void)
283 bool TheEthManager::verifyEthBoardInfo(yarp::os::Searchable &cfgtotal, eOipv4addr_t &boardipv4,
string boardipv4string,
string boardname)
288 yError() <<
"TheEthManager::verifyEthBoardInfo() fails";
303 bool 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;
399 ethBoards->add(rr, interface);
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_inertials;
459 category = eomn_serv_category_inertials3;
464 category = eomn_serv_category_none;
473 category = eomn_serv_category_all;
475 if(eomn_serv_category_none != category)
489 ethBoards->rem(rr, type);
491 int remaining = ethBoards->number_of_interfaces(rr);
499 if(0 == ethBoards->number_of_resources())
513 const eOipv4addressing_t& TheEthManager::getLocalIPV4addressing(
void)
519 IethResource* TheEthManager::getInterface(eOipv4addr_t ipv4, eOprotID32_t id32)
521 IethResource *interfacePointer = ethBoards->get_interface(ipv4, id32);
523 return interfacePointer;
527 double TheEthManager::getLifeTime(
void)
529 return(yarp::os::Time::now() - startUpTime);
534 bool TheEthManager::createCommunicationObjects(
const eOipv4addressing_t &localaddress,
int txrate,
int rxrate)
538 ACE_INET_Addr inetaddr = toaceinet(localaddress);
540 if(!communicationIsInitted)
542 UDP_socket =
new ACE_SOCK_Dgram();
543 if((embBoardsConnected) && (-1 == UDP_socket->open(inetaddr)))
546 inetaddr.addr_to_string(
tmp, 64);
547 yError() <<
"\n/--------------------------------------------------------------------------------------------------------------\\"
548 <<
"\n| TheEthManager::createCommunicationObjects() is unable to bind to local IP address " <<
tmp
549 <<
"\n\\--------------------------------------------------------------------------------------------------------------/";
552 communicationIsInitted =
false;
556 communicationIsInitted =
true;
557 ipv4local = localaddress;
559 if((txrate <= 0) || (txrate > EthSender::EthSenderMaxRate))
563 if((rxrate <= 0) | (rxrate > EthReceiver::EthReceiverMaxRate))
565 rxrate = EthReceiver::EthReceiverDefaultRate;
570 sender->config(UDP_socket,
this);
571 receiver->config(UDP_socket,
this);
578 ret1 = sender->start();
579 ret2 = receiver->start();
583 yError() <<
"TheEthManager::createCommunicationObjects() fails in starting UDP communication threads ethSender / ethReceiver";
586 stopCommunicationThreads();
589 communicationIsInitted =
false;
594 yTrace() <<
"TheEthManager::createCommunicationObjects(): both UDP communication threads ethSender / ethReceiver start correctly!";
601 return communicationIsInitted;
605 bool TheEthManager::isCommunicationInitted(
void)
610 ret = communicationIsInitted;
617 bool TheEthManager::stopCommunicationThreads()
621 if(sender->isRunning())
625 if(receiver->isRunning())
634 int TheEthManager::sendPacket(
const void *udpframe,
size_t len,
const eOipv4addressing_t &toaddressing)
636 ACE_INET_Addr inetaddr = toaceinet(toaddressing);
637 ssize_t ret = UDP_socket->send(udpframe, len, inetaddr);
643 eOipv4addr_t TheEthManager::toipv4addr(
const ACE_INET_Addr &aceinetaddr)
645 ACE_UINT32 a32 = aceinetaddr.get_ip_address();
646 uint8_t ip4 = a32 & 0xff;
647 uint8_t ip3 = (a32 >> 8) & 0xff;
648 uint8_t ip2 = (a32 >> 16) & 0xff;
649 uint8_t ip1 = (a32 >> 24) & 0xff;
650 return eo_common_ipv4addr(ip1, ip2, ip3, ip4);
653 ACE_INET_Addr TheEthManager::toaceinet(
const eOipv4addressing_t &ipv4addressing)
655 uint8_t ip1, ip2, ip3, ip4;
656 eo_common_ipv4addr_to_decimal(ipv4addressing.addr, &ip1, &ip2, &ip3, &ip4);
657 ACE_UINT32 hostip = (ip1 << 24) | (ip2 << 16) | (ip3 << 8) | (ip4);
658 ACE_INET_Addr myIP((u_short)ipv4addressing.port, hostip);
664 bool TheEthManager::Reception(eOipv4addr_t from, uint64_t*
data, ssize_t size)
670 if((size >=0) && (NULL != r) && (!r->
isFake()))
676 yError() <<
"TheEthManager::Reception() cannot give a received packet of size" << size <<
"to EthResource because EthResource::processRXpacket() returns false.";
694 int TheEthManager::getNumberOfResources(
void)
696 return(ethBoards->number_of_resources());
700 const string & TheEthManager::getName(eOipv4addr_t ipv4)
702 return ethBoards->name(ipv4);
708 return(ethBoards->get_resource(ipv4));
712 bool TheEthManager::lock(
bool on)
727 bool TheEthManager::lockTX(
bool on)
742 bool TheEthManager::lockRX(
bool on)
756 bool TheEthManager::lockTXRX(
bool on)
virtual const Properties & getProperties()=0
virtual const void * getUDPtransmit(eOipv4addressing_t &destination, size_t &sizeofpacket, uint16_t &numofrops)=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
virtual iethresType_t type()=0
int sendPacket(const void *udpframe, size_t len, const eOipv4addressing_t &toaddressing)
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