9 #include <yarp/os/Time.h>
10 #include <yarp/os/Log.h>
15 #include <iCubCanProto_skinMessages.h>
36 const std::string& paramName)
38 bool correct = config.check(paramName);
41 correct = config.find(paramName).isString();
46 yError(
"CanBusInertialMTB: problem loading parameter %s as string",paramName.c_str());
53 const std::string& paramName)
55 bool correct = config.check(paramName);
58 correct = config.find(paramName).isInt32();
63 yError(
"CanBusInertialMTB: problem loading parameter %s as int",paramName.c_str());
70 const std::string& paramName,
71 std::vector<int> & output_vector)
73 bool correct = !(config.findGroup(paramName).isNull());
76 Bottle ids = config.findGroup(paramName).tail();
77 output_vector.resize(ids.size());
78 for(
int i = 0; i < ids.size(); i++ )
80 output_vector[i] = ids.get(i).asInt32();
86 yError(
"CanBusInertialMTB: problem loading parameter %s as vector of int",paramName.c_str());
94 const std::string& paramName,
95 std::vector<std::string> & output_vector)
97 bool correct = !(config.findGroup(paramName).isNull());
101 Bottle ids = config.findGroup(paramName).tail();
102 std::cout <<
"ids : " << ids.toString() << std::endl;
103 std::cout <<
"ids : " << config.find(paramName).toString() << std::endl;
104 output_vector.resize(ids.size());
105 for(
int i = 0; i < ids.size(); i++ )
107 output_vector[i] = ids.get(i).asString().c_str();
113 yError(
"CanBusInertialMTB: problem loading parameter %s as vector of string",paramName.c_str());
120 bool CanBusInertialMTB::validateConf(yarp::os::Searchable& config,
121 std::vector<int> & canAddresses)
123 std::cout <<
"CanBusInertialMTB::validateConf : " << config.toString() << std::endl;
137 std::vector<int> canAddresses;
138 bool correct = this->validateConf(config,canAddresses);
142 yError(
"CanBusInertialMTB: Insufficient parameters to CanBusInertialMTB\n");
148 if (config.check(
"sensorPeriod"))
150 int int_sensorPeriod = config.find(
"sensorPeriod").asInt32();
151 if( int_sensorPeriod < 1 || int_sensorPeriod > 255 )
153 yError(
"CanBusInertialMTB: sensorPeriod is lower than 1 or bigger then 255\n");
156 sensorPeriod = int_sensorPeriod;
160 this->boards.resize(canAddresses.size());
161 this->nrOfTotalChannels = 0;
166 this->boards[
board].boardId = canAddresses[
board];
167 if( config.find(
"sensorType").asString() ==
"acc" )
170 this->boards[
board].enabledGyro =
false;
171 this->boards[
board].nrOfChannels = 3;
172 this->boards[
board].vectorOffset = this->nrOfTotalChannels;
173 this->nrOfTotalChannels += this->boards[
board].nrOfChannels;
175 else if( config.find(
"sensorType").asString() ==
"extAccAndGyro" )
179 this->boards[
board].enabledGyro =
true;
180 this->boards[
board].nrOfChannels = 6;
181 this->boards[
board].vectorOffset = this->nrOfTotalChannels;
182 this->nrOfTotalChannels += this->boards[
board].nrOfChannels;
186 yError(
"CanBusInertialMTB: unknown sensorType %s",config.find(
"sensorType").asString().c_str());
191 if (config.check(
"period")==
true)
194 period=config.find(
"period").asInt32();
195 setPeriod((
double)period/1000.0);
200 prop.put(
"device", config.find(
"canbusDevice").asString().c_str());
201 prop.put(
"physDevice", config.find(
"physDevice").asString().c_str());
202 prop.put(
"canTxTimeout", 500);
203 prop.put(
"canRxTimeout", 500);
204 prop.put(
"canDeviceNum", config.find(
"canDeviceNum").asInt32());
205 prop.put(
"canMyAddress", 0);
213 if (!driver.isValid())
215 yError(
"Unable to open CanBusInertialMTB check parameters\n");
218 driver.view(pCanBus);
221 yError(
"Unable to open CAN device not available\n");
224 driver.view(pCanBufferFactory);
229 pCanBus->canSetBaudRate(0);
235 unsigned short boardId = this->boards[
board].boardId;
236 if( this->boards[
board].enabledGyro )
244 data.resize(this->nrOfTotalChannels);
246 privateData.resize(this->nrOfTotalChannels);
248 sharedStatus.resize(this->nrOfTotalChannels,IAnalogSensor::AS_OK);
249 privateStatus.resize(this->nrOfTotalChannels,IAnalogSensor::AS_OK);
251 PeriodicThread::start();
258 PeriodicThread::stop();
261 if (pCanBufferFactory)
263 pCanBufferFactory->destroyBuffer(inBuffer);
264 pCanBufferFactory->destroyBuffer(outBuffer);
273 lock_guard<mutex> lck(mtx);
275 return this->sharedGlobalStatus;
280 lock_guard<mutex> lck(mtx);
281 return this->sharedStatus[ch];
286 return this->nrOfTotalChannels;
317 unsigned int canMessages=0;
318 unsigned id = 0x200 + this->boards[
board].boardId;
320 CanMessage &msg=outBuffer[0];
322 msg.getData()[0]=ICUBCANPROTO_POL_SK_CMD__ACC_GYRO_SETUP;
323 msg.getData()[1]=this->boards[
board].enabledSensors;
324 msg.getData()[2]=this->boards[
board].sensorPeriod;
327 bool ret = pCanBus->canWrite(outBuffer, 1, &canMessages);
331 yError(
"CanBusInertialMTB: canWrite returned false for sensor with address %x",this->boards[
board].boardId);
339 void CanBusInertialMTB::setPrivateBoardStatus(
int boardIndex,
short status)
341 int vectorOffset = this->boards[boardIndex].vectorOffset;
342 int nrOfBoardChannels = this->boards[boardIndex].nrOfChannels;
343 for(
int i=0; i < nrOfBoardChannels; i++ )
345 privateStatus[vectorOffset+i] = status;
349 void CanBusInertialMTB::setPrivateBoardGyroStatus(
int boardIndex,
short status)
351 int vectorOffset = this->boards[boardIndex].vectorOffset;
352 int nrOfBoardChannels = this->boards[boardIndex].nrOfChannels;
353 for(
int i=3; i < 6; i++ )
355 privateStatus[vectorOffset+i] = status;
359 void CanBusInertialMTB::setPrivateBoardAccStatus(
int boardIndex,
short status)
361 int vectorOffset = this->boards[boardIndex].vectorOffset;
362 int nrOfBoardChannels = this->boards[boardIndex].nrOfChannels;
363 for(
int i=0; i < 3; i++ )
365 privateStatus[vectorOffset+i] = status;
371 unsigned int canMessages=0;
375 yError(
"CanBusInertialMTB::run(): canRead failed\n");
377 double timeNow=Time::now();
381 yError(
"CanBusInertialMTB::run() get %d canMessages\n", canMessages);
384 setPrivateBoardStatus(
board,IAnalogSensor::AS_ERROR);
389 for (
unsigned int i=0; i<canMessages; i++)
391 CanMessage &msg=inBuffer[i];
393 unsigned int msgid = msg.getId();
394 unsigned char *buff = msg.getData();
395 unsigned int len = msg.getLen();
396 unsigned char id = ((msgid & 0x00f0)>>4);
397 unsigned int msg_class = ((msgid & 0x0700));
398 unsigned char msg_type = ((msgid & 0x000f));
403 unsigned short boardId = this->boards[
board].boardId;
404 unsigned int vectorOffset = this->boards[
board].vectorOffset;
408 this->boards[
board].accTimeStamp=timeNow;
409 privateData[vectorOffset+0]= (
signed short) ((buff[1]<<8) + buff[0]);
410 privateData[vectorOffset+1]= (
signed short) ((buff[3]<<8) + buff[2]);
411 privateData[vectorOffset+2]= (
signed short) ((buff[5]<<8) + buff[4]);
412 setPrivateBoardAccStatus(
board,IAnalogSensor::AS_OK);
417 this->boards[
board].gyroTimeStamp=timeNow;
418 privateData[vectorOffset+3]= (
signed short) (buff[1]<<8)+buff[0];
419 privateData[vectorOffset+4]= (
signed short) (buff[3]<<8)+buff[2];
420 privateData[vectorOffset+5]= (
signed short) (buff[5]<<8)+buff[4];
421 setPrivateBoardGyroStatus(
board,IAnalogSensor::AS_OK);
433 unsigned short boardId = this->boards[
board].boardId;
436 double delay = timeNow-this->boards[
board].gyroTimeStamp;
439 yError(
"CanBusInertialMTB::run(): gyroscope read for board %x timed out (last received %lf sec ago)",boardId,delay);
440 setPrivateBoardGyroStatus(
board,IAnalogSensor::AS_TIMEOUT);
444 delay = timeNow-this->boards[
board].accTimeStamp;
447 yError(
"CanBusInertialMTB::run(): accelerometer read for board %x timed out (last received %lf sec ago)",boardId,delay);
448 setPrivateBoardAccStatus(
board,IAnalogSensor::AS_TIMEOUT);
462 this->privateGlobalStatus = IAnalogSensor::AS_OK;
463 for(
int ch=0; ch < this->nrOfTotalChannels; ch+= 3)
465 if( this->privateStatus[ch] == IAnalogSensor::AS_ERROR )
467 this->privateGlobalStatus = IAnalogSensor::AS_ERROR;
470 if( this->privateStatus[ch] == IAnalogSensor::AS_TIMEOUT )
472 this->privateGlobalStatus = IAnalogSensor::AS_TIMEOUT;
478 lock_guard<mutex> lck(mtx);
479 memcpy(
data.data(), privateData.data(),
sizeof(
double)*privateData.size());
480 for(
size_t ch=0; ch < privateStatus.size(); ch++ )
482 this->sharedStatus[ch] = this->privateStatus[ch];
484 this->sharedGlobalStatus = this->privateGlobalStatus;
493 unsigned int canMessages=0;
494 unsigned id = 0x200 + this->boards[
board].boardId;
496 CanMessage &msg=outBuffer[0];
498 msg.getData()[0]=ICUBCANPROTO_POL_SK_CMD__ACC_GYRO_SETUP;
499 msg.getData()[1]=0x0;
500 msg.getData()[2]=0x1;
503 bool ret = pCanBus->canWrite(outBuffer, 1, &canMessages);
507 yError(
"CanBusInertialMTB: canWrite returned false for sensor with address %x",this->boards[
board].boardId);
const char CANBUS_INERTIAL_MTB_EXTERNAL_GYRO_BIT
const unsigned int CAN_MSG_CLASS_ACC_GYRO
const double CANBUS_INERTIAL_MTB_TIMEOUT
bool checkRequiredParamIsInt(yarp::os::Searchable &config, const std::string ¶mName)
const char CANBUS_INERTIAL_MTB_EXTERNAL_ACC_BIT
bool checkRequiredParamIsVectorOfInt(yarp::os::Searchable &config, const std::string ¶mName, std::vector< int > &output_vector)
const unsigned int MSG_TYPE_GYRO
const char CANBUS_INERTIAL_MTB_INTERNAL_ACC_BIT
const unsigned short CANBUS_INERTIAL_MTB_DEFAULT_SENSOR_PERIOD
const unsigned int MSG_TYPE_ACC
const int CANBUS_INERTIAL_MTB_CAN_DRIVER_BUFFER_SIZE
bool checkRequiredParamIsVectorOfString(yarp::os::Searchable &config, const std::string ¶mName, std::vector< std::string > &output_vector)
bool checkRequiredParamIsString(yarp::os::Searchable &config, const std::string ¶mName)
virtual void threadRelease()
virtual int read(yarp::sig::Vector &out)
virtual int getChannels()
virtual bool open(yarp::os::Searchable &config)
virtual int getState(int ch)
virtual int calibrateChannel(int ch, double v)
virtual bool threadInit()