13#include <yarp/os/Time.h>
14#include <yarp/os/Network.h>
15#include <yarp/conf/environment.h>
16#include <yarp/os/NetType.h>
19#include "EOropframe_hid.h"
21#include "EoProtocol.h"
22#include "EoManagement.h"
23#include "EoProtocolMN.h"
26using namespace yarp::os;
27using namespace yarp::os::impl;
42 isInRunningMode =
false;
44 verifiedBoardPresence =
false;
45 askedBoardVersion =
false;
46 verifiedBoardTransceiver =
false;
48 cleanedBoardBehaviour =
false;
50 boardMNprotocolversion.major = boardMNprotocolversion.minor;
52 txconfig.runnermode = eomn_appl_runnermode_synchronized;
53 txconfig.safetygap = 0;
54 txconfig.cycletime = defcycletime;
55 txconfig.txratedivider = defTXrateOfRegularROPs;
56 txconfig.maxtimeRX = defmaxtimeRX;
57 txconfig.maxtimeDO = defmaxtimeDO;
58 txconfig.maxtimeTX = defmaxtimeTX;
59 txconfig.logging.flags = (0x0001 << eomn_appl_log_asynchro_exectime_rxdotx_overflow);
60 txconfig.logging.period10ms = 0;
62 memset(verifiedEPprotocol, 0,
sizeof(verifiedEPprotocol));
64 usedNumberOfRegularROPs = 0;
66 for(
int i = 0; i<16; i++)
68 c_string_handler[i] = NULL;
71 std::string tmp = yarp::conf::environment::get_string(
"ETH_VERBOSEWHENOK");
74 verbosewhenok = (bool)(yarp::conf::numeric::from_string(tmp, 0U));
78 verbosewhenok =
false;
83 regularsAreSet =
false;
92 for(
int i=0; i<16; i++)
94 if (c_string_handler[i] != NULL)
96 delete c_string_handler[i];
97 c_string_handler[i] = NULL;
102bool EthResource::lock(
bool on)
157 if(
false == transceiver.
init2(
this, cfgtotal, localIPv4, remIP))
159 yError() <<
"EthResource::open2() cannot init transceiver w/ HostTransceiver::init2() for BOARD" << properties.
boardnameString <<
"IP" << properties.
ipv4addrString;
166 monitorpresence.
config(mpConfig);
167 monitorpresence.
tick();
187 const void * udp = transceiver.
getUDP(sizeofpacket, numofrops);
206 monitorpresence.
tick();
213 if(
false == regularsAreSet)
218 return monitorpresence.
check();
243bool EthResource::isRunning(
void)
245 return(isInRunningMode);
250bool EthResource::verifyBoardTransceiver()
254 if(verifiedBoardTransceiver)
260#define DONT_ASK_COMM_STATUS
262#if defined(DONT_ASK_COMM_STATUS)
264 const eoprot_version_t * pc104versionMN = eoprot_version_of_endpoint_get(eoprot_endpoint_management);
265 const eoprot_version_t * brdversionMN = &boardMNprotocolversion;
274 const eoprot_version_t * pc104versionMN = eoprot_version_of_endpoint_get(eoprot_endpoint_management);
275 const double timeout = 0.100;
277 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_comm, 0, eoprot_tag_mn_comm_status);
278 eOmn_comm_status_t brdstatus = {0};
281 bool rr = nvman.
ask(ipv4addr, id32, &brdstatus, timeout);
289 eoprot_version_t * brdversionMN = (eoprot_version_t*)&brdstatus.managementprotocolversion;
293 if(pc104versionMN->major != brdversionMN->major)
295 yError() <<
"EthResource::verifyBoardTransceiver() detected different mn protocol major versions: local =" << pc104versionMN->major <<
", remote =" << brdversionMN->major <<
": cannot proceed any further";
300 if(pc104versionMN->minor != brdversionMN->minor)
302 yError() <<
"EthResource::verifyBoardTransceiver() detected different mn protocol minor versions: local =" << pc104versionMN->minor <<
", remote =" << brdversionMN->minor <<
": cannot proceed any further.";
313 verifiedBoardTransceiver =
true;
320bool EthResource::setTimingOfRunningCycle()
333 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_appl, 0, eoprot_tag_mn_appl_config);
337 if(
false == nvman.
setcheck(properties.
ipv4addr, id32, &txconfig, 5, 0.010, 2.0))
339 yWarning() <<
"EthResource::setTimingOfRunningCycle() for BOARD" <<
getProperties().
boardnameString <<
"with IP" <<
getProperties().
ipv4addrString <<
"could not configure: cycletime =" << txconfig.cycletime <<
"usec, RX DO TX = (" << txconfig.maxtimeRX << txconfig.maxtimeDO << txconfig.maxtimeTX <<
") usec and TX rate =" << txconfig.txratedivider <<
" every cycle";
346 yDebug() <<
"EthResource::setTimingOfRunningCycle() for BOARD" <<
getProperties().
boardnameString <<
"with IP" <<
getProperties().
ipv4addrString <<
"has succesfully set: cycletime =" << txconfig.cycletime <<
"usec, RX DO TX = (" << txconfig.maxtimeRX << txconfig.maxtimeDO << txconfig.maxtimeTX <<
") usec and TX rate =" << txconfig.txratedivider <<
" every cycle";
357bool EthResource::cleanBoardBehaviour(
void)
359 if(cleanedBoardBehaviour)
371 regularsAreSet =
false;
379 cleanedBoardBehaviour =
true;
385bool EthResource::testMultipleASK()
393 eOprotID32_t id32_commstatus = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_comm, 0, eoprot_tag_mn_comm_status);
394 eOmn_comm_status_t value_commstatus = {0};
396 eOprotID32_t id32_applconfig = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_appl, 0, eoprot_tag_mn_appl_config);
397 eOmn_appl_config_t value_applconfig = {0};
399 eOprotID32_t id32_applconfig_txratedivider = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_appl, 0, eoprot_tag_mn_appl_config_txratedivider);
400 uint8_t txratedivider = 0;
402 std::vector<eOprotID32_t> id32s;
403 std::vector<void*> values;
405 id32s.push_back(id32_commstatus);
406 values.push_back(&value_commstatus);
408 id32s.push_back(id32_applconfig);
409 values.push_back(&value_applconfig);
411 id32s.push_back(id32_applconfig_txratedivider);
412 values.push_back(&txratedivider);
414 id32s.push_back(id32_applconfig_txratedivider);
415 values.push_back(&txratedivider);
417 id32s.push_back(id32_applconfig_txratedivider);
418 values.push_back(&txratedivider);
420 id32s.push_back(id32_applconfig_txratedivider);
421 values.push_back(&txratedivider);
423 id32s.push_back(id32_applconfig_txratedivider);
424 values.push_back(&txratedivider);
426 id32s.push_back(id32_applconfig_txratedivider);
427 values.push_back(&txratedivider);
429 id32s.push_back(id32_applconfig_txratedivider);
430 values.push_back(&txratedivider);
432 id32s.push_back(id32_applconfig_txratedivider);
433 values.push_back(&txratedivider);
437 double tprev = SystemClock::nowSystem();
438 double tcurr = SystemClock::nowSystem();
440 double delta = tcurr - tprev;
442 yDebug() <<
"before";
443 yDebug() <<
"value_commstatus.managementprotocolversion.major = " << value_commstatus.managementprotocolversion.major <<
"value_commstatus.managementprotocolversion.minor = " << value_commstatus.managementprotocolversion.minor;
444 yDebug() <<
"value_applconfig.cycletime = " << value_applconfig.cycletime <<
"value_applconfig.txratedivider" << value_applconfig.txratedivider <<
"etc";
445 yDebug() <<
"txratedivider = " << txratedivider;
447 tprev = SystemClock::nowSystem();
450 delta = SystemClock::nowSystem() - tprev;
452 yDebug() <<
"parallel mode: after" << delta <<
"seconds";
453 yDebug() <<
"value_commstatus.managementprotocolversion.major = " << value_commstatus.managementprotocolversion.major <<
"value_commstatus.managementprotocolversion.minor = " << value_commstatus.managementprotocolversion.minor;
454 yDebug() <<
"value_applconfig.cycletime = " << value_applconfig.cycletime <<
"value_applconfig.txratedivider" << value_applconfig.txratedivider <<
"etc";
455 yDebug() <<
"txratedivider = " << txratedivider;
457 memset(&value_commstatus, 0,
sizeof(value_commstatus));
458 memset(&value_applconfig, 0,
sizeof(value_applconfig));
462 tprev = SystemClock::nowSystem();
463 nvman.
ask(&transceiver, id32_commstatus, &value_commstatus, 3.0);
464 nvman.
ask(&transceiver, id32_applconfig, &value_applconfig, 3.0);
465 nvman.
ask(&transceiver, id32_applconfig_txratedivider, &txratedivider, 3.0);
466 nvman.
ask(&transceiver, id32_applconfig_txratedivider, &txratedivider, 3.0);
467 nvman.
ask(&transceiver, id32_applconfig_txratedivider, &txratedivider, 3.0);
468 nvman.
ask(&transceiver, id32_applconfig_txratedivider, &txratedivider, 3.0);
469 nvman.
ask(&transceiver, id32_applconfig_txratedivider, &txratedivider, 3.0);
470 nvman.
ask(&transceiver, id32_applconfig_txratedivider, &txratedivider, 3.0);
471 nvman.
ask(&transceiver, id32_applconfig_txratedivider, &txratedivider, 3.0);
472 nvman.
ask(&transceiver, id32_applconfig_txratedivider, &txratedivider, 3.0);
473 delta = SystemClock::nowSystem() - tprev;
475 yDebug() <<
"serial mode: after" << delta <<
"seconds";
476 yDebug() <<
"value_commstatus.managementprotocolversion.major = " << value_commstatus.managementprotocolversion.major <<
"value_commstatus.managementprotocolversion.minor = " << value_commstatus.managementprotocolversion.minor;
477 yDebug() <<
"value_applconfig.cycletime = " << value_applconfig.cycletime <<
"value_applconfig.txratedivider" << value_applconfig.txratedivider <<
"etc";
478 yDebug() <<
"txratedivider = " << txratedivider;
490 if((uint8_t)ep >= eoprot_endpoints_numberof)
492 yError() <<
"EthResource::verifyEPprotocol() called with wrong ep = " << ep <<
": cannot proceed any further";
496 if(
true == verifiedEPprotocol[ep])
501 if(
false == verifyBoard())
507 if(
false == askBoardVersion())
522 const double timeout = 0.100;
524 eOprotID32_t id2send = eo_prot_ID32dummy;
525 eOprotID32_t id2wait = eo_prot_ID32dummy;
526 eOmn_command_t command = {0};
530 id2send = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_comm, 0, eoprot_tag_mn_comm_cmmnds_command_queryarray);
531 memset(&command, 0,
sizeof(command));
532 command.cmd.opc = eomn_opc_query_array_EPdes;
533 command.cmd.queryarray.opcpar.opc = eomn_opc_query_array_EPdes;
534 command.cmd.queryarray.opcpar.endpoint = eoprot_endpoint_all;
535 command.cmd.queryarray.opcpar.setnumber = 0;
536 command.cmd.queryarray.opcpar.setsize = 0;
539 id2wait = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_comm, 0, eoprot_tag_mn_comm_cmmnds_command_replyarray);
543 if(
false == nvman.
command(properties.
ipv4addr, id2send, &command, id2wait, &command, timeout))
550 eOmn_cmd_replyarray_t* cmdreplyarray = (eOmn_cmd_replyarray_t*)&command.cmd.replyarray;
551 EOarray* array = (EOarray*)cmdreplyarray->array;
554 uint8_t sizeofarray = eo_array_Size(array);
557 for(
int i=0; i<sizeofarray; i++)
559 eoprot_endpoint_descriptor_t *epd = (eoprot_endpoint_descriptor_t*)eo_array_At(array, i);
561 if(epd->endpoint == eoprot_endpoint_management)
563 const eoprot_version_t * pc104versionMN = eoprot_version_of_endpoint_get(eoprot_endpoint_management);
564 if(pc104versionMN->major != epd->version.major)
566 yError() <<
"EthResource::verifyEPprotocol() for ep =" << eoprot_EP2string(epd->endpoint) <<
"detected: pc104.version.major =" << pc104versionMN->major <<
"and board.version.major =" << epd->version.major;
571 if(pc104versionMN->minor != epd->version.minor)
573 yError() <<
"EthResource::verifyEPprotocol() for ep =" << eoprot_EP2string(epd->endpoint) <<
"detected: pc104.version.minor =" << pc104versionMN->minor <<
"and board.version.minor =" << epd->version.minor;
579 if(epd->endpoint == ep)
581 const eoprot_version_t * pc104versionEP = eoprot_version_of_endpoint_get(ep);
582 if(pc104versionEP->major != epd->version.major)
584 yError() <<
"EthResource::verifyEPprotocol() for ep =" << eoprot_EP2string(epd->endpoint) <<
"detected: pc104.version.major =" << pc104versionEP->major <<
"and board.version.major =" << epd->version.major;
589 if(pc104versionEP->minor != epd->version.minor)
591 yError() <<
"EthResource::verifyEPprotocol() for ep =" << eoprot_EP2string(epd->endpoint) <<
"detected: pc104.version.minor =" << pc104versionEP->minor <<
"and board.version.minor =" << epd->version.minor;
599 verifiedEPprotocol[ep] =
true;
607bool EthResource::verifyBoard(
void)
609 if((
true == verifyBoardPresence()) &&
610 (
true == verifyBoardTransceiver()) &&
611 (
true == cleanBoardBehaviour()) &&
612 (
true == setTimingOfRunningCycle()) )
621bool EthResource::verifyBoardPresence(
void)
623 if(verifiedBoardPresence)
628 const double timeout = 1.00;
629 const int retries = 20;
631 double start_time = yarp::os::Time::now();
634 verifiedBoardPresence = nvman.
ping(properties.
ipv4addr, boardMNprotocolversion, timeout, retries);
636 double end_time = yarp::os::Time::now();
638 if(
true == verifiedBoardPresence)
640 verifiedBoardPresence =
true;
651 return(verifiedBoardPresence);
655bool EthResource::askBoardVersion(
void)
658 if(askedBoardVersion)
663 const double timeout = 0.500;
665 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_appl, 0, eoprot_tag_mn_appl_status);
666 eOmn_appl_status_t applstatus = {0};
670 askedBoardVersion = nvman.
ask(properties.
ipv4addr, id32, &applstatus, timeout);
672 if(
false == askedBoardVersion)
685 properties.
firmwaredate.year = applstatus.buildate.year;
686 properties.
firmwaredate.month = applstatus.buildate.month;
688 properties.
firmwaredate.hour = applstatus.buildate.hour;
691 char versstr[32] = {0};
693 char datestr[32] = {0};
694 eo_common_date_to_string(properties.
firmwaredate, datestr,
sizeof(datestr));
699 if(eobool_true == eoboards_is_eth((eObrd_type_t)applstatus.boardtype))
701 detectedBoardType = (eObrd_ethtype_t) applstatus.boardtype;
705 detectedBoardType = eobrd_ethtype_unknown;
708 if(detectedBoardType != properties.
boardtype)
710 yWarning() <<
"EthResource::askBoardVersion(): detected wrong board. expecting" << properties.
boardtypeString <<
"and detected" << eoboards_type2string2(eoboards_ethtype2type(detectedBoardType), eobool_true);
717 return(askedBoardVersion);
723 bool replied =
false;
725 double start_time = yarp::os::Time::now();
729 for(
unsigned int numOfattempts=0; numOfattempts<(retries+1); numOfattempts++)
731 if(
true == nvman.
ask(&transceiver, id32, value, timeout))
745 double end_time = yarp::os::Time::now();
760 double start_time = yarp::os::Time::now();
762 bool replied = nvman.
ask(&transceiver, id32s, values, timeout);
764 double end_time = yarp::os::Time::now();
778 return nvman.
set(properties.
ipv4addr, id32, value);
784 return nvman.
setcheck(properties.
ipv4addr, id32, value, retries, waitbeforecheck, timeout);
790 char canfullmessage[128];
792 static const char * sourcestrings[] =
799 int source = EOMN_INFO_PROPERTIES_FLAGS_get_source(infobasic->properties.flags);
800 const char * str_source = (source > eomn_info_source_can2) ? (sourcestrings[3]) : (sourcestrings[source]);
801 uint16_t address = EOMN_INFO_PROPERTIES_FLAGS_get_address(infobasic->properties.flags);
802 uint8_t *p64 = (uint8_t*)&(infobasic->properties.par64);
804 int msg_id = (p64[1]&0xF0) >> 4;
806 uint32_t sec = infobasic->timestamp / 1000000;
807 uint32_t msec = (infobasic->timestamp % 1000000) / 1000;
808 uint32_t usec = infobasic->timestamp % 1000;
815 snprintf(canfullmessage,
sizeof(canfullmessage),
"Error while parsing the message: CAN address detected is out of allowed range");
816 snprintf(str,
sizeof(str),
"from BOARD %s (%s), src %s, adr %d, time %ds %dm %du: CAN PRINT MESSAGE[id %d] -> %s",
832 if (c_string_handler[address] == NULL)
836 can_msg.
setCanData(infobasic->properties.par64);
837 can_msg.
setId(msg_id);
838 can_msg.
setSize(infobasic->properties.par16);
839 int ret = c_string_handler[address]->
add_string(&can_msg);
844 char* themsg = c_string_handler[address]->
get_string(ret);
845 memcpy(canfullmessage, themsg,
sizeof(canfullmessage));
846 canfullmessage[63] = 0;
849 snprintf(str,
sizeof(str),
"from BOARD %s (%s), src %s, adr %d, time %ds %dm %du: CAN PRINT MESSAGE[id %d] -> %s",
867bool EthResource::serviceCommand(eOmn_serv_operation_t operation, eOmn_serv_category_t category,
const eOmn_serv_parameter_t* param,
double timeout,
int times)
869 eOprotID32_t id2send = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_service, 0, eoprot_tag_mn_service_cmmnds_command);
870 eOprotID32_t id2wait = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_service, 0, eoprot_tag_mn_service_status_commandresult);
872 eOmn_service_cmmnds_command_t command = {0};
873 eOmn_service_command_result_t result = {0};
875 command.operation = operation;
876 command.category = category;
879 memcpy(&command.parameter, param,
sizeof(eOmn_serv_parameter_t));
883 memset(&command.parameter, 0,
sizeof(eOmn_serv_parameter_t));
884 if((eomn_serv_operation_regsig_load == operation) || ((eomn_serv_operation_regsig_clear == operation)))
886 eo_array_New(eOmn_serv_capacity_arrayof_id32, 4, &command.parameter.arrayofid32);
890 command.parameter.configuration.type = eomn_serv_NONE;
897 bool replied =
false;
898 for(
int i=0; i<times; i++)
900 if(
true == nvman.
command(properties.
ipv4addr, id2send, &command, id2wait, &result, timeout))
915 return(result.latestcommandisok);
921 return(serviceCommand(eomn_serv_operation_verifyactivate, category, param, timeout, 3));
927 eOmn_serv_parameter_t param = {0};
928 EOarray *array = eo_array_New(eOmn_serv_capacity_arrayof_id32, 4, ¶m.arrayofid32);
929 for(
int i=0; i<id32vector.size(); i++)
931 eOprotID32_t id32 = id32vector.at(i);
932 eo_array_PushBack(array, &id32);
935 regularsAreSet = serviceCommand(eomn_serv_operation_regsig_load, category, ¶m, timeout, 3);
937 return regularsAreSet;
944 bool ret = serviceCommand(eomn_serv_operation_start, category, NULL, timeout, 3);
948 isInRunningMode =
true;
957 bool ret = serviceCommand(eomn_serv_operation_stop, category, NULL, timeout, 3);
959 if(ret && (category == eomn_serv_category_all))
961 regularsAreSet =
false;
973 return transceiver.
read(id32,
data);
979 return transceiver.
write(id32, value, overrideROprotection);
void setSize(uint8_t size)
void setCanData(uint64_t data)
char * get_string(int buffer_num)
int add_string(CanFrame *can_packet)
Process a string can packet.
void clear_string(int buffer_num)
Resets the string buffer.
void config(const Config &cfg)
bool getRemoteValues(const std::vector< eOprotID32_t > &id32s, const std::vector< void * > &values, const double timeout=0.500)
const void * getUDPtransmit(eOipv4addressing_t &destination, size_t &sizeofpacket, uint16_t &numofrops)
bool processRXpacket(const void *data, size_t size)
HostTransceiver * getTransceiver()
bool getLocalValue(const eOprotID32_t id32, void *value)
bool serviceSetRegulars(eOmn_serv_category_t category, vector< eOprotID32_t > &id32vector, double timeout=0.500)
bool open2(eOipv4addr_t remIP, yarp::os::Searchable &cfgtotal)
bool serviceStop(eOmn_serv_category_t category, double timeout=0.500)
bool verifyEPprotocol(eOprot_endpoint_t ep)
bool getRemoteValue(const eOprotID32_t id32, void *value, const double timeout=0.100, const unsigned int retries=0)
bool setLocalValue(eOprotID32_t id32, const void *value, bool overrideROprotection=false)
bool setRemoteValue(const eOprotID32_t id32, void *value)
bool setcheckRemoteValue(const eOprotID32_t id32, void *value, const unsigned int retries=10, const double waitbeforecheck=0.001, const double timeout=0.050)
bool serviceVerifyActivate(eOmn_serv_category_t category, const eOmn_serv_parameter_t *param, double timeout=0.500)
const Properties & getProperties()
bool serviceStart(eOmn_serv_category_t category, double timeout=0.500)
bool CANPrintHandler(eOmn_info_basic_t *infobasic)
static TheEthManager * instance()
const eOipv4addressing_t & getLocalIPV4addressing(void)
static theNVmanager & getInstance()
bool setcheck(const eOprotIP_t ipv4, const eOprotID32_t id32, const void *value, const unsigned int retries=10, double waitbeforecheck=0.001, double timeout=0.5)
bool ask(const eOprotIP_t ipv4, const eOprotID32_t id32, void *value, const double timeout=0.5)
bool command(const eOprotIP_t ipv4, const eOprotID32_t id32cmd, const void *cmd, const eOprotID32_t id32rep, void *rep, double timeout=0.5)
bool ping(const eOprotIP_t ipv4, eoprot_version_t &mnprotversion, const double timeout=0.5, const unsigned int retries=20)
bool set(const eOprotIP_t ipv4, const eOprotID32_t id32, const void *value)
bool write(const eOprotID32_t id32, const void *data, bool forcewriteOfReadOnly)
bool init2(AbstractEthResource *owner, yarp::os::Searchable &cfgtotal, eOipv4addressing_t &localIPaddressing, eOipv4addr_t remoteIP, uint16_t rxpktsize=maxSizeOfRXpacket)
const void * getUDP(size_t &size, uint16_t &numofrops)
bool parseUDP(const void *data, const uint16_t size)
bool read(const eOprotID32_t id32, void *data)
void feat_PrintInfo(char *string)
void feat_PrintError(char *string)
bool read(yarp::os::Searchable &cfgtotal, pc104Data &pc104data)
bool print(const pc104Data &pc104data)
string ipv4addressingString
eOipv4addressing_t ipv4addressing
eObrd_ethtype_t boardtype
eOversion_t firmwareversion
double periodmissingreport
double monitorpresence_periodofmissingreport
bool monitorpresence_enabled
double monitorpresence_timeout
boardProperties properties
eOipv4addressing_t ipv4addressing
std::string ipv4addressingstring
eOmn_appl_config_t txconfig