9 #include <yarp/os/Time.h>
10 #include <yarp/os/LogStream.h>
11 #include <yarp/os/Log.h>
24 fprintf(stderr,
"%s\n", config.toString().c_str());
26 correct &= config.check(
"canbusDevice");
27 correct &= config.check(
"canDeviceNum");
28 correct &= config.check(
"canAddress");
29 correct &= config.check(
"format");
30 correct &= config.check(
"period");
31 correct &= config.check(
"channels");
32 correct &= config.check(
"physDevice");
33 correct &= config.check(
"sensorName");
37 yError() <<
"Insufficient parameters to CanBusFtSensor\n";
41 if (!config.check(
"frameName"))
43 yWarning() <<
"frameName not found.. using default value unknown_frame_name";
46 int period=config.find(
"period").asInt32();
47 setPeriod((
double)period/1000.0);
51 prop.put(
"device", config.find(
"canbusDevice").asString().c_str());
52 prop.put(
"physDevice", config.find(
"physDevice").asString().c_str());
53 prop.put(
"canTxTimeout", 500);
54 prop.put(
"canRxTimeout", 500);
55 canDeviceNum = config.find(
"canDeviceNum").asInt32();
56 prop.put(
"canDeviceNum", canDeviceNum);
57 prop.put(
"canMyAddress", 0);
65 if (!driver.isValid())
67 yError() <<
"Error opening PolyDriver check parameters";
73 yError() <<
"Error opening can device not available";
76 driver.view(pCanBufferFactory);
81 pCanBus->canSetBaudRate(0);
86 this->boardId = config.find(
"canAddress").asInt32();
87 this->useCalibration = config.find(
"useCalibration").asInt32();
89 this->sensorName = config.find(
"sensorName").asString();
90 this->frameName = config.check(
"frameName", yarp::os::Value(
"unknown_frame_name")).asString();
91 unsigned int tmpFormat = config.find(
"format").asInt32();
93 this->dataFormat = ANALOG_FORMAT_8_BIT;
94 else if (tmpFormat == 16)
95 this->dataFormat = ANALOG_FORMAT_16_BIT;
97 this->dataFormat = ANALOG_FORMAT_ERR;
100 if( config.check(
"diagnostic") && config.find(
"diagnostic").asInt32() == 1)
102 this->diagnostic =
true;
106 this->diagnostic =
false;
111 for (
int id=0;
id<16; ++id)
113 pCanBus->canIdAdd(0x300+(boardId<<4)+
id);
115 pCanBus->canIdAdd(0x200+boardId);
116 pCanBus->canIdAdd(0x200+(boardId<<4));
119 this->channelsNum = config.find(
"channels").asInt32();
120 data.resize(channelsNum);
122 scaleFactor.resize(channelsNum);
126 sensor_start(config);
128 PeriodicThread::start();
132 bool CanBusFtSensor::readFullScaleAnalog(
int ch)
134 scaleFactor[ch]=1
e-20;
136 unsigned int canMessages=0;
137 unsigned id = 0x200 + boardId;
138 CanMessage &msg=outBuffer[0];
140 msg.getData()[0]=0x18;
144 pCanBus->canWrite(outBuffer, 1, &canMessages);
147 bool full_scale_read=
false;
151 unsigned int read_messages = 0;
152 bool b = pCanBus->canRead(inBuffer,max_messages,&read_messages,
false);
154 for (
unsigned int i=0; i<read_messages; i++)
156 CanMessage& m= inBuffer[i];
157 unsigned int currId=m.getId();
158 if (currId==(0x0200 | boardId << 4))
160 m.getData()[0]==0x18 &&
163 scaleFactor[ch]=m.getData()[2]<<8 | m.getData()[3];
164 full_scale_read=
true;
168 yarp::os::Time::delay(0.002);
171 while(timeout<32 && full_scale_read==
false);
173 if (full_scale_read==
false)
175 yError(
"Trying to get fullscale data from sensor %d net [%d]: no answer received or message lost (ch:%d)\n", boardId, canDeviceNum, ch);
182 bool CanBusFtSensor::sensor_start(yarp::os::Searchable& analogConfig)
184 yTrace(
"Initializing analog device %s\n", analogConfig.find(
"deviceId").toString().c_str());
186 unsigned int canMessages=0;
187 unsigned id = 0x200 + boardId;
189 if (analogConfig.check(
"period"))
191 int period=analogConfig.find(
"period").asInt32();
192 CanMessage &msg=outBuffer[0];
194 msg.getData()[0]=0x08;
195 msg.getData()[1]=period;
198 pCanBus->canWrite(outBuffer, 1, &canMessages);
199 yDebug(
"using broadcast period %d on device %s\n", period, analogConfig.find(
"deviceId").toString().c_str());
203 if (channelsNum==16 && dataFormat==ANALOG_FORMAT_8_BIT)
205 CanMessage &msg=outBuffer[0];
207 msg.getData()[0]=0x07;
208 msg.getData()[1]=0x00;
211 pCanBus->canWrite(outBuffer, 1, &canMessages);
215 else if (channelsNum==6 && dataFormat==ANALOG_FORMAT_16_BIT)
218 if (useCalibration>0)
220 yDebug(
"Using internal calibration on device %s\n", analogConfig.find(
"deviceId").toString().c_str());
222 for (
int ch=0; ch<6; ch++)
228 b = readFullScaleAnalog(ch);
231 if (attempts>0) yWarning(
"Trying to get fullscale data from sensor: channel recovered (ch:%d)\n", ch);
235 yarp::os::Time::delay(0.020);
239 yError(
"Trying to get fullscale data from sensor: all attempts failed (ch:%d)\n", ch);
245 fprintf(stderr,
"Sensor Fullscale Id %#4X: ",boardId);
246 fprintf(stderr,
" %f ", scaleFactor[0]);
247 fprintf(stderr,
" %f ", scaleFactor[1]);
248 fprintf(stderr,
" %f ", scaleFactor[2]);
249 fprintf(stderr,
" %f ", scaleFactor[3]);
250 fprintf(stderr,
" %f ", scaleFactor[4]);
251 fprintf(stderr,
" %f ", scaleFactor[5]);
256 CanMessage &msg=outBuffer[0];
258 msg.getData()[0]=0x07;
259 if (useCalibration == 1) msg.getData()[1]=0x00;
260 if (useCalibration == 2) msg.getData()[1]=0x00;
261 if (useCalibration == 3) msg.getData()[1]=0x00;
264 pCanBus->canWrite(outBuffer, 1, &canMessages);
269 yInfo(
"Using uncalibrated raw data for device %s\n", analogConfig.find(
"deviceId").toString().c_str());
270 CanMessage &msg=outBuffer[0];
272 msg.getData()[0]=0x07;
273 msg.getData()[1]=0x03;
276 pCanBus->canWrite(outBuffer, 1, &canMessages);
282 bool CanBusFtSensor::sensor_stop()
284 unsigned int canMessages=0;
285 unsigned id = 0x200 + boardId;
286 CanMessage &msg=outBuffer[0];
288 msg.getData()[0]=0x07;
289 msg.getData()[1]=0x01;
292 pCanBus->canWrite(outBuffer, 1, &canMessages);
299 PeriodicThread::stop();
305 if (pCanBufferFactory)
307 pCanBufferFactory->destroyBuffer(inBuffer);
308 pCanBufferFactory->destroyBuffer(outBuffer);
320 bool CanBusFtSensor::decode16(
const unsigned char *msg,
int msg_id,
double *
data)
322 const char groupId=(msg_id & 0x00f);
331 data[k]=(((
unsigned short)(msg[2*k+1]))<<8)+msg[2*k]-0x8000;
332 if (useCalibration>0)
334 data[k]=
data[k]*scaleFactor[k]/float(0x8000);
343 data[k+3]=(((
unsigned short)(msg[2*k+1]))<<8)+msg[2*k]-0x8000;
344 if (useCalibration>0)
346 data[k+3]=
data[k+3]*scaleFactor[k+3]/float(0x8000);
358 yWarning(
"Got unexpected class 0x3 msg(s): groupId 0x%x\n", groupId);
369 bool CanBusFtSensor::decode8(
const unsigned char *msg,
int msg_id,
double *
data)
371 const char groupId=(msg_id & 0x00f);
378 for(
int k=0;k<=6;k++)
384 for(
int k=0;k<=7;k++)
395 yWarning(
"CanBusFtSensor got unexpected class 0x3 msg(s): groupId 0x%x\n", groupId);
407 lock_guard<mutex> lck(mtx);
409 unsigned int canMessages=0;
415 yError()<<
"CanBusFtSensor canRead failed\n";
418 double timeNow=Time::now();
419 for (
unsigned int i=0; i<canMessages; i++)
421 CanMessage &msg=inBuffer[i];
423 unsigned int msgid = msg.getId();
424 unsigned char *buff = msg.getData();
425 unsigned int len = msg.getLen();
426 unsigned int id = (msgid & 0x00f0)>>4;
427 const char type = ((msgid&0x700)>>8);
430 status=MAS_status::MAS_OK;
436 timeStamp=Time::now();
439 case ANALOG_FORMAT_8_BIT:
440 ret=decode8(buff, msgid,
data.data());
441 status=MAS_status::MAS_OK;
443 case ANALOG_FORMAT_16_BIT:
446 ret=decode16(buff, msgid,
data.data());
447 status=MAS_status::MAS_OK;
451 if ((len==7) && (buff[6] != 0))
453 status=MAS_status::MAS_OVF;
457 status=MAS_status::MAS_ERROR;
459 ret=decode16(buff, msgid,
data.data());
463 status=MAS_status::MAS_ERROR;
471 if (timeStamp+0.1<timeNow)
473 status=MAS_status::MAS_TIMEOUT;
479 yTrace(
"CanBusVirtualAnalogSensor Thread released\n");
490 return yarp::dev::MAS_OK;
495 name = this->sensorName;
501 frameName = this->frameName;
507 std::lock_guard<std::mutex> lck(mtx);
509 timestamp = timeStamp;
const int CAN_DRIVER_BUFFER_SIZE
virtual bool open(yarp::os::Searchable &config)
virtual yarp::dev::MAS_status getSixAxisForceTorqueSensorStatus(size_t sens_index) const override
virtual bool getSixAxisForceTorqueSensorFrameName(size_t sens_index, std::string &frameName) const override
virtual size_t getNrOfSixAxisForceTorqueSensors() const override
virtual bool getSixAxisForceTorqueSensorName(size_t sens_index, std::string &name) const override
virtual bool threadInit()
virtual bool getSixAxisForceTorqueSensorMeasure(size_t sens_index, yarp::sig::Vector &out, double ×tamp) const override
virtual void threadRelease()