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());
120bool 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());
167 if( config.find(
"sensorType").asString() ==
"acc" )
175 else if( config.find(
"sensorType").asString() ==
"extAccAndGyro" )
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);
215 yError(
"Unable to open CanBusInertialMTB check parameters\n");
221 yError(
"Unable to open CAN device not available\n");
235 unsigned short boardId = this->
boards[
board].boardId;
251 PeriodicThread::start();
258 PeriodicThread::stop();
273 lock_guard<mutex> lck(
mtx);
280 lock_guard<mutex> lck(
mtx);
317 unsigned int canMessages=0;
322 msg.getData()[0]=ICUBCANPROTO_POL_SK_CMD__ACC_GYRO_SETUP;
323 msg.getData()[1]=this->
boards[
board].enabledSensors;
331 yError(
"CanBusInertialMTB: canWrite returned false for sensor with address %x",this->
boards[
board].boardId);
339void 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++ )
349void 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++ )
359void 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++ )
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++)
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;
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);
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);
447 yError(
"CanBusInertialMTB::run(): accelerometer read for board %x timed out (last received %lf sec ago)",boardId,delay);
448 setPrivateBoardAccStatus(
board,IAnalogSensor::AS_TIMEOUT);
478 lock_guard<mutex> lck(
mtx);
493 unsigned int canMessages=0;
498 msg.getData()[0]=ICUBCANPROTO_POL_SK_CMD__ACC_GYRO_SETUP;
499 msg.getData()[1]=0x0;
500 msg.getData()[2]=0x1;
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)
std::vector< short > sharedStatus
virtual void threadRelease()
std::vector< short > privateStatus
ICanBufferFactory * pCanBufferFactory
short privateGlobalStatus
virtual int read(yarp::sig::Vector &out)
std::vector< MTBInertialBoardInfo > boards
virtual int getChannels()
yarp::sig::Vector privateData
virtual bool open(yarp::os::Searchable &config)
virtual int getState(int ch)
virtual int calibrateChannel(int ch, double v)
virtual bool threadInit()