15 #include <yarp/os/Time.h>
19 #include "EoProtocol.h"
20 #include "EoProtocolMN.h"
21 #include "EoProtocolSK.h"
24 #include "iCubCanProtocol.h"
28 #include "../embObjLib/hostTransceiver.hpp"
32 #include <yarp/os/NetType.h>
33 #include <yarp/conf/environment.h>
42 return ((adr>0) && (adr<15));
48 for (
size_t i = 0; i< cardAddrList.size(); i++)
50 if(cardAddrList[i] == cardAddr)
65 std::string
tmp = yarp::conf::environment::get_string(
"ETH_VERBOSEWHENOK");
68 verbosewhenok = (bool)(yarp::conf::numeric::from_string(
tmp, 0U));
72 verbosewhenok =
false;
77 memset(ðservice.configuration, 0,
sizeof(ethservice.configuration));
78 ethservice.configuration.type = eomn_serv_NONE;
95 unsigned int numOfSets;
99 unsigned int numofcfg;
117 for(j=0; j<numofcfg; j++)
136 for(
int a=boardCfgList[j].boardAddrStart; a<=boardCfgList[j].
boardAddrEnd; a++)
147 eOsk_cmd_boardsCfg_t bcfg;
149 bcfg.candestination[0] = bcfg.candestination[1] = 0;
150 for(
int adr=boardCfgList[j].boardAddrStart; adr<=boardCfgList[j].
boardAddrEnd; adr++)
154 eo_common_hlfword_bitset(&bcfg.candestination[canport], adr);
159 bcfg.cfg.period = boardCfgList[j].
cfg.
period;
160 bcfg.cfg.noload = boardCfgList[j].
cfg.
noLoad;
164 for (
size_t sensorId = 0; sensorId < 16; sensorId++)
166 size_t index = 16 * 12 * boardIdx + sensorId * 12;
169 for(
int k = 0; k < 12; k++)
174 yError() <<
"readNewSpecialConfiguration: index too big";
183 protoid = eoprot_ID_get(eoprot_endpoint_skin, eoprot_entity_sk_skin,
_skCfg.
patchInfoList[
p].indexNv, eoprot_tag_sk_skin_cmmnds_boardscfg);
193 SystemClock::delaySystem(0.010);
205 for(j=0; j<numofcfg; j++)
229 protoid = eoprot_ID_get(eoprot_endpoint_skin, eoprot_entity_sk_skin,
_skCfg.
patchInfoList[
p].indexNv, eoprot_tag_sk_skin_cmmnds_trianglescfg);
232 eOsk_cmd_trianglesCfg_t tcfg = {0};
235 tcfg.candestination[0] = tcfg.candestination[1] = 0;
239 eo_common_hlfword_bitset(&tcfg.candestination[canport], adr);
246 tcfg.cfg.shift = triangleCfg[j].
cfg.
shift;
257 SystemClock::delaySystem(0.010);
266 SystemClock::delaySystem(0.010);
274 Bottle bPatches, bPatchList, xtmp;
282 bPatches = config.findGroup(
"patches",
"skin patches connected to this device");
283 if(bPatches.isNull())
291 for (
int i=1; i<=2; i++)
293 sprintf(
tmp,
"skinCanAddrsPatch%d",i);
294 if (bPatches.check(
tmp))
297 bPatchList.addInt32(i);
305 int id = bPatchList.get(j-1).asInt32();
306 if((
id!=1) && (
id!=2))
322 snprintf(
tmp,
sizeof(
tmp),
"skinCanAddrsPatch%d",
id);
324 xtmp = bPatches.findGroup(
tmp);
333 for(
int j=1; j<xtmp.size(); j++)
335 int addr = xtmp.get(j).asInt32();
349 for (
int i=0; i < ttt; i++)
358 ethservice.configuration.type = eomn_serv_SK_skin;
361 ethservice.configuration.data.sk.skin.boardinfo.type = skinconfig.
canboard.
type;
363 ethservice.configuration.data.sk.skin.boardinfo.protocol.major = skinconfig.
canboard.
protocol.major;
364 ethservice.configuration.data.sk.skin.boardinfo.protocol.minor = skinconfig.
canboard.
protocol.minor;
366 ethservice.configuration.data.sk.skin.boardinfo.firmware.major = skinconfig.
canboard.
firmware.major;
367 ethservice.configuration.data.sk.skin.boardinfo.firmware.minor = skinconfig.
canboard.
firmware.minor;
368 ethservice.configuration.data.sk.skin.boardinfo.firmware.build = skinconfig.
canboard.
firmware.build;
372 if(ethservice.configuration.data.sk.skin.numofpatches > eomn_serv_skin_maxpatches)
374 yError() <<
"cannot have so many skin patches. detected" << ethservice.configuration.data.sk.skin.numofpatches <<
"max is" << eomn_serv_skin_maxpatches;
378 for(
int np=0; np<ethservice.configuration.data.sk.skin.numofpatches; np++)
381 ethservice.configuration.data.sk.skin.canmapskin[np][0] = 0;
385 ethservice.configuration.data.sk.skin.canmapskin[np][1] = 0;
396 eo_common_hlfword_bitset(ðservice.configuration.data.sk.skin.canmapskin[np][canport], adr);
421 for (
int triangleId = 0; triangleId < 16; triangleId++)
423 int index = 16*12*board_idx + triangleId*12;
426 for (
size_t k = 0; k < 12; k++)
431 yError() <<
"readNewConfiguration: index too big";
453 yFatal() <<
"EmbObjSkin::open() fails to instantiate ethManager";
459 yError() <<
"embObjSkin::open(): object TheEthManager fails in parsing ETH propertiex from xml file";
488 yError() <<
"embObjSkin::open() fails because could not instantiate the ethResource for BOARD w/ IP = " <<
boardIPstring <<
" ... unable to continue";
505 const eOmn_serv_parameter_t* servparam = ðservice;
520 SystemClock::delaySystem(0.500);
581 std::lock_guard<std::mutex> lck(
mtx);
583 return yarp::dev::IAnalogSensor::AS_OK;
588 return yarp::dev::IAnalogSensor::AS_OK;;
619 eOprotID32_t protoid;
626 dat = eosk_sigmode_signal;
630 dat = eosk_sigmode_signal_oldway;
635 protoid = eoprot_ID_get(eoprot_endpoint_skin, eoprot_entity_sk_skin,
_skCfg.
patchInfoList[i].indexNv, eoprot_tag_sk_skin_config_sigmode);
650 vector<eOprotID32_t> id32v(0);
651 eOprotID32_t protoid = eo_prot_ID32dummy;
658 protoid = eoprot_ID_get(eoprot_endpoint_skin, eoprot_entity_sk_skin,
_skCfg.
patchInfoList[i].indexNv, eoprot_tag_sk_skin_status_arrayofcandata);
659 id32v.push_back(protoid);
665 yError() <<
"EmbObjSkin::configPeriodicMessage() fails to add its variables to regulars: cannot proceed any further";
674 for (
size_t r = 0; r<id32v.size(); r++)
676 uint32_t id32 = id32v.at(r);
677 eoprot_ID2information(id32, nvinfo,
sizeof(nvinfo));
678 yDebug() <<
"\t it added regular rop for" << nvinfo;
682 SystemClock::delaySystem(0.005);
690 eOprotID32_t protoid;
699 eOsk_cmd_boardsCfg_t defBoardCfg = {0};
700 eOsk_cmd_trianglesCfg_t defTriangleCfg = {0};
707 defTriangleCfg.idstart = 0;
708 defTriangleCfg.idend = 15;
715 protoid = eoprot_ID_get(eoprot_endpoint_skin, eoprot_entity_sk_skin,
_skCfg.
patchInfoList[i].indexNv, eoprot_tag_sk_skin_cmmnds_boardscfg);
718 uint8_t minAddr = 16;
738 defBoardCfg.candestination[0] = defBoardCfg.candestination[1] = 0xffff;
747 SystemClock::delaySystem(0.010);
751 protoid = eoprot_ID_get(eoprot_endpoint_skin, eoprot_entity_sk_skin,
_skCfg.
patchInfoList[i].indexNv, eoprot_tag_sk_skin_cmmnds_trianglescfg);
755 defTriangleCfg.candestination[0] = defTriangleCfg.candestination[1] = 0xffff;
780 #undef DEBUG_PRINT_RX_STATS
781 #if defined(DEBUG_PRINT_RX_STATS)
782 static uint32_t receivedpatches[2][16] = {0};
783 static uint32_t counterpa = 0;
789 uint8_t i, triangle = 0;
790 static int error = 0;
792 EOarray* arrayof = (EOarray*)rxdata;
793 uint8_t sizeofarray = eo_array_Size(arrayof);
795 eOprotIndex_t indexpatch = eoprot_ID2index(id32);
810 errors.resize(sizeofarray);
812 for(i=0; i<sizeofarray; i++)
814 eOsk_candata_t *candata = (eOsk_candata_t*) eo_array_At(arrayof, i);
821 uint16_t canframeid11 = EOSK_CANDATA_INFO2IDCAN(candata->info);
822 uint8_t canframesize = EOSK_CANDATA_INFO2SIZE(candata->info);
823 uint8_t *canframedata = candata->data;
826 uint8_t cardAddr = 0;
831 skinClass = ICUBCANPROTO_CLASS_PERIODIC_SKIN;
833 skinClass = ICUBCANPROTO_CLASS_PERIODIC_ANALOGSENSOR;
837 valid = (((canframeid11 & 0x0f00) >> 8) == skinClass) ? 1 : 0;
841 cardAddr = (canframeid11 & 0x00f0) >> 4;
861 triangle = (canframeid11 & 0x000f);
862 msgtype = (int) canframedata[0];
864 int index=16*12*mtbId + triangle*12;
871 #if defined(DEBUG_PRINT_RX_STATS)
872 receivedpatches[
p][cardAddr]++;
876 for(
int k = 0; k < 7; k++)
878 skindata[index + k] = canframedata[k + 1];
881 else if (msgtype == 0xC0)
884 for(
int k = 0; k < 5; k++)
886 skindata[index + k + 7] = canframedata[k + 1];
892 if (canframesize == 8)
895 _isDiagnosticPresent =
true;
898 short head = canframedata[6];
899 short tail = canframedata[7];
900 int fullMsg = (head << 8) | (tail & 0xFF);
903 errors[i].net = indexpatch;
904 errors[i].board = cardAddr;
905 errors[i].sensor = triangle;
906 errors[i].error = fullMsg;
908 if (fullMsg != SkinErrorCode::StatusOK)
910 yError() <<
"embObjSkin error code: " <<
913 "canDeviceNum: " << errors[i].net <<
914 "board: " << errors[i].board <<
915 "sensor: " << errors[i].sensor <<
921 _isDiagnosticPresent =
false;
927 else if(canframeid11 == 0x100)
935 yError() <<
"EMS: " <<
res->
getProperties().
ipv4addrString <<
" Unknown Message received from skin (" << i<<
"/"<< sizeofarray <<
"): frameID=" << canframeid11<<
" len="<<canframesize <<
"canframe.data="<<canframedata[0] <<
" " <<canframedata[1] <<
" " <<canframedata[2] <<
" " <<canframedata[3] <<
"\n" ;
942 #if defined(DEBUG_PRINT_RX_STATS)
943 if(counterpa >= 10000)
946 yDebug(
"pa = {{%d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d} {%d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d}}",
947 receivedpatches[0][0], receivedpatches[0][1], receivedpatches[0][2], receivedpatches[0][3], receivedpatches[0][4], receivedpatches[0][5], receivedpatches[0][6], receivedpatches[0][7],
948 receivedpatches[0][8], receivedpatches[0][9], receivedpatches[0][10], receivedpatches[0][11], receivedpatches[0][12], receivedpatches[0][13], receivedpatches[0][14], receivedpatches[0][15],
949 receivedpatches[1][0], receivedpatches[1][1], receivedpatches[1][2], receivedpatches[1][3], receivedpatches[1][4], receivedpatches[1][5], receivedpatches[1][6], receivedpatches[1][7],
950 receivedpatches[1][8], receivedpatches[1][9], receivedpatches[1][10], receivedpatches[1][11], receivedpatches[1][12], receivedpatches[1][13], receivedpatches[1][14], receivedpatches[1][15]
virtual int read(yarp::sig::Vector &out)
bool fromConfig(yarp::os::Searchable &config)
eth::AbstractEthResource * res
SkinConfigReader _cfgReader
virtual int calibrateSensor()
SkinBoardCfgParam _brdCfg
virtual bool initialised()
@ SPECIAL_TRIANGLE_CFG_MAX_NUM
virtual int calibrateChannel(int ch, double v)
eOprotIndex_t convertIdPatch2IndexNv(int idPatch)
bool initWithSpecialConfig(yarp::os::Searchable &config)
SkinTriangleCfgParam _triangCfg
virtual bool open(yarp::os::Searchable &config)
virtual eth::iethresType_t type()
virtual int getState(int ch)
virtual int getChannels()
virtual bool update(eOprotID32_t id32, double timestamp, void *rxdata)
bool configPeriodicMessage(void)
eth::TheEthManager * ethManager
bool parseService(yarp::os::Searchable &config, servConfigMais_t &maisconfig)
void setDefaultValues(void)
bool readDefaultBoardCfg(yarp::os::Searchable &config, SkinBoardCfgParam *boardCfg)
bool isDefaultBoardCfgPresent(yarp::os::Searchable &config)
bool readSpecialBoardCfg(yarp::os::Searchable &config, SpecialSkinBoardCfgParam *boardCfg, unsigned int *numofcfg)
bool readDefaultTriangleCfg(yarp::os::Searchable &config, SkinTriangleCfgParam *triangCfg)
bool readSpecialTriangleCfg(yarp::os::Searchable &config, SpecialSkinTriangleCfgParam *triangleCfg, unsigned int *numofcfg)
bool isDefaultTriangleCfgPresent(yarp::os::Searchable &config)
std::vector< SkinPatchInfo > patchInfoList
int checkCardAddrIsInList(int cardAddr)
void setDefaultValues(void)
virtual const Properties & getProperties()=0
virtual bool serviceVerifyActivate(eOmn_serv_category_t category, const eOmn_serv_parameter_t *param, double timeout=0.500)=0
virtual bool setRemoteValue(const eOprotID32_t id32, void *value)=0
virtual bool verifyEPprotocol(eOprot_endpoint_t ep)=0
virtual bool serviceSetRegulars(eOmn_serv_category_t category, vector< eOprotID32_t > &id32vector, double timeout=0.500)=0
virtual bool serviceStart(eOmn_serv_category_t category, double timeout=0.500)=0
int releaseResource2(eth::AbstractEthResource *ethresource, IethResource *interface)
bool verifyEthBoardInfo(yarp::os::Searchable &cfgtotal, eOipv4addr_t &boardipv4, string boardipv4string, string boardname)
static bool killYourself()
static TheEthManager * instance()
eth::AbstractEthResource * requestResource2(IethResource *interface, yarp::os::Searchable &cfgtotal)
bool isCANaddressValid(int adr)
std::string printErrorCode(int code)
eObrd_protocolversion_t protocol
eObrd_firmwareversion_t firmware