iCub-main
Loading...
Searching...
No Matches
CanBusFtSensor.cpp
Go to the documentation of this file.
1// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2
3// Copyright: (C) 2013 iCub Facility
4// Authors: Marco Randazzo <marco.randazzo@iit.it> Valentina Vasco <valentina.vasco@iit.it>
5// CopyPolicy: Released under the terms of the GNU GPL v2.0.
6
7#include <CanBusFtSensor.h>
8
9#include <yarp/os/Time.h>
10#include <yarp/os/LogStream.h>
11#include <yarp/os/Log.h>
12#include <iostream>
13#include <string.h>
14
16
17using namespace std;
18
19bool CanBusFtSensor::open(yarp::os::Searchable& config)
20{
21 bool correct=true;
22
23 //debug
24 fprintf(stderr, "%s\n", config.toString().c_str());
25
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");
34
35 if (!correct)
36 {
37 yError() << "Insufficient parameters to CanBusFtSensor\n";
38 return false;
39 }
40
41 if (!config.check("frameName"))
42 {
43 yWarning() << "frameName not found.. using default value unknown_frame_name";
44 }
45
46 int period=config.find("period").asInt32();
47 setPeriod((double)period/1000.0);
48
49 Property prop;
50
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);
58 prop.put("canTxQueueSize", CAN_DRIVER_BUFFER_SIZE);
59 prop.put("canRxQueueSize", CAN_DRIVER_BUFFER_SIZE);
60 pCanBus=0;
62
63 //open the can driver
64 driver.open(prop);
65 if (!driver.isValid())
66 {
67 yError() << "Error opening PolyDriver check parameters";
68 return false;
69 }
70 driver.view(pCanBus);
71 if (!pCanBus)
72 {
73 yError() << "Error opening can device not available";
74 return false;
75 }
79
80 //select the communication speed
81 pCanBus->canSetBaudRate(0); //default 1MB/s
82
83
84 //set the internal configuration
85 //this->isVirtualSensor = false;
86 this->boardId = config.find("canAddress").asInt32();
87 this->useCalibration = config.find("useCalibration").asInt32();
88 //this->SensorFullScale = config.find("fullScale").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();
92 if (tmpFormat == 8)
93 this->dataFormat = ANALOG_FORMAT_8_BIT;
94 else if (tmpFormat == 16)
95 this->dataFormat = ANALOG_FORMAT_16_BIT;
96 else
97 this->dataFormat = ANALOG_FORMAT_ERR;
98
99 // Parse diagnostic information
100 if( config.check("diagnostic") && config.find("diagnostic").asInt32() == 1)
101 {
102 this->diagnostic = true;
103 }
104 else
105 {
106 this->diagnostic = false;
107 }
108
109
110 //open the can mask for the specific canDeviceId
111 for (int id=0; id<16; ++id)
112 {
113 pCanBus->canIdAdd(0x300+(boardId<<4)+id);
114 }
115 pCanBus->canIdAdd(0x200+boardId);
116 pCanBus->canIdAdd(0x200+(boardId<<4));
117
118 //create the data vector:
119 this->channelsNum = config.find("channels").asInt32();
120 data.resize(channelsNum);
121 data.zero();
122 scaleFactor.resize(channelsNum);
123 scaleFactor.zero();
124
125 //start the sensor broadcast
126 sensor_start(config);
127
128 PeriodicThread::start();
129 return true;
130}
131
132bool CanBusFtSensor::readFullScaleAnalog(int ch)
133{
134 scaleFactor[ch]=1e-20;
135
136 unsigned int canMessages=0;
137 unsigned id = 0x200 + boardId;
138 CanMessage &msg=outBuffer[0];
139 msg.setId(id);
140 msg.getData()[0]=0x18;
141 msg.getData()[1]=ch;
142 msg.setLen(2);
143 canMessages=0;
144 pCanBus->canWrite(outBuffer, 1, &canMessages);
145
146 long int timeout=0;
147 bool full_scale_read=false;
148 do
149 {
150 unsigned int max_messages=CAN_DRIVER_BUFFER_SIZE;
151 unsigned int read_messages = 0;
152 bool b = pCanBus->canRead(inBuffer,max_messages,&read_messages,false);
153
154 for (unsigned int i=0; i<read_messages; i++)
155 {
156 CanMessage& m= inBuffer[i];
157 unsigned int currId=m.getId();
158 if (currId==(0x0200 | boardId << 4))
159 if (m.getLen()==4 &&
160 m.getData()[0]==0x18 &&
161 m.getData()[1]==ch)
162 {
163 scaleFactor[ch]=m.getData()[2]<<8 | m.getData()[3];
164 full_scale_read=true;
165 break;
166 }
167 }
168 yarp::os::Time::delay(0.002);
169 timeout++;
170 }
171 while(timeout<32 && full_scale_read==false);
172
173 if (full_scale_read==false)
174 {
175 yError("Trying to get fullscale data from sensor %d net [%d]: no answer received or message lost (ch:%d)\n", boardId, canDeviceNum, ch);
176 return false;
177 }
178
179 return true;
180}
181
182bool CanBusFtSensor::sensor_start(yarp::os::Searchable& analogConfig)
183{
184 yTrace("Initializing analog device %s\n", analogConfig.find("deviceId").toString().c_str());
185
186 unsigned int canMessages=0;
187 unsigned id = 0x200 + boardId;
188
189 if (analogConfig.check("period"))
190 {
191 int period=analogConfig.find("period").asInt32();
192 CanMessage &msg=outBuffer[0];
193 msg.setId(id);
194 msg.getData()[0]=0x08;
195 msg.getData()[1]=period;
196 msg.setLen(2);
197 canMessages=0;
198 pCanBus->canWrite(outBuffer, 1, &canMessages);
199 yDebug("using broadcast period %d on device %s\n", period, analogConfig.find("deviceId").toString().c_str());
200 }
201
202 //init message for mais board
203 if (channelsNum==16 && dataFormat==ANALOG_FORMAT_8_BIT)
204 {
205 CanMessage &msg=outBuffer[0];
206 msg.setId(id);
207 msg.getData()[0]=0x07;
208 msg.getData()[1]=0x00;
209 msg.setLen(2);
210 canMessages=0;
211 pCanBus->canWrite(outBuffer, 1, &canMessages);
212 }
213
214 //init message for strain board
215 else if (channelsNum==6 && dataFormat==ANALOG_FORMAT_16_BIT)
216 {
217 //calibrated astrain board
218 if (useCalibration>0)
219 {
220 yDebug("Using internal calibration on device %s\n", analogConfig.find("deviceId").toString().c_str());
221 //get the full scale values from the strain board
222 for (int ch=0; ch<6; ch++)
223 {
224 bool b=false;
225 int attempts = 0;
226 while(attempts<15)
227 {
228 b = readFullScaleAnalog(ch);
229 if (b==true)
230 {
231 if (attempts>0) yWarning("Trying to get fullscale data from sensor: channel recovered (ch:%d)\n", ch);
232 break;
233 }
234 attempts++;
235 yarp::os::Time::delay(0.020);
236 }
237 if (attempts>=15)
238 {
239 yError("Trying to get fullscale data from sensor: all attempts failed (ch:%d)\n", ch);
240 }
241 }
242
243 // debug messages
244 #if 1
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]);
252 fprintf(stderr, " \n ");
253 #endif
254
255 // start the board
256 CanMessage &msg=outBuffer[0];
257 msg.setId(id);
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;
262 msg.setLen(2);
263 canMessages=0;
264 pCanBus->canWrite(outBuffer, 1, &canMessages);
265 }
266 //not calibrated strain board (useCalibration = 0)
267 else
268 {
269 yInfo("Using uncalibrated raw data for device %s\n", analogConfig.find("deviceId").toString().c_str());
270 CanMessage &msg=outBuffer[0];
271 msg.setId(id);
272 msg.getData()[0]=0x07;
273 msg.getData()[1]=0x03;
274 msg.setLen(2);
275 canMessages=0;
276 pCanBus->canWrite(outBuffer, 1, &canMessages);
277 }
278 }
279 return true;
280}
281
282bool CanBusFtSensor::sensor_stop()
283{
284 unsigned int canMessages=0;
285 unsigned id = 0x200 + boardId;
286 CanMessage &msg=outBuffer[0];
287 msg.setId(id);
288 msg.getData()[0]=0x07;
289 msg.getData()[1]=0x01;
290 msg.setLen(2);
291 canMessages=0;
292 pCanBus->canWrite(outBuffer, 1, &canMessages);
293 return true;
294}
295
297{
298 //stop the thread
299 PeriodicThread::stop();
300
301 //stop the sensor
302 sensor_stop();
303
304 //stop the driver
306 {
307 pCanBufferFactory->destroyBuffer(inBuffer);
308 pCanBufferFactory->destroyBuffer(outBuffer);
309 }
310 driver.close();
311
312 return true;
313}
314
316{
317 return true;
318}
319
320bool CanBusFtSensor::decode16(const unsigned char *msg, int msg_id, double *data)
321{
322 const char groupId=(msg_id & 0x00f);
323 int baseIndex=0;
324 {
325 switch (groupId)
326 {
327 case 0xA:
328 {
329 for(int k=0;k<3;k++)
330 {
331 data[k]=(((unsigned short)(msg[2*k+1]))<<8)+msg[2*k]-0x8000;
332 if (useCalibration>0)
333 {
334 data[k]=data[k]*scaleFactor[k]/float(0x8000);
335 }
336 }
337 }
338 break;
339 case 0xB:
340 {
341 for(int k=0;k<3;k++)
342 {
343 data[k+3]=(((unsigned short)(msg[2*k+1]))<<8)+msg[2*k]-0x8000;
344 if (useCalibration>0)
345 {
346 data[k+3]=data[k+3]*scaleFactor[k+3]/float(0x8000);
347 }
348 }
349 }
350 break;
351 case 0xC:
352 {} //skip these, they are not for us
353 break;
354 case 0xD:
355 {} //skip these, they are not for us
356 break;
357 default:
358 yWarning("Got unexpected class 0x3 msg(s): groupId 0x%x\n", groupId);
359 return false;
360 break;
361 }
362 //@@@DEBUG ONLY
363 //fprintf(stderr, " %+8.1f %+8.1f %+8.1f %+8.1f %+8.1f %+8.1f\n",data[0],data[1],data[2],data[3],data[4],data[5],data[6]);
364 }
365
366 return true;
367}
368
369bool CanBusFtSensor::decode8(const unsigned char *msg, int msg_id, double *data)
370{
371 const char groupId=(msg_id & 0x00f);
372 int baseIndex=0;
373 {
374 switch (groupId)
375 {
376 case 0xC:
377 {
378 for(int k=0;k<=6;k++)
379 data[k]=msg[k];
380 }
381 break;
382 case 0xD:
383 {
384 for(int k=0;k<=7;k++)
385 data[7+k]=msg[k];
386 }
387 break;
388 case 0xA:
389 {} //skip these, they are not for us
390 break;
391 case 0xB:
392 {} //skip these, they are not for us
393 break;
394 default:
395 yWarning("CanBusFtSensor got unexpected class 0x3 msg(s): groupId 0x%x\n", groupId);
396 return false;
397 break;
398 }
399 //@@@DEBUG ONLY
400 //fprintf(stderr, " %+8.1f %+8.1f %+8.1f %+8.1f %+8.1f %+8.1f\n",data[0],data[1],data[2],data[3],data[4],data[5],data[6]);
401 }
402 return true;
403}
404
406{
407 lock_guard<mutex> lck(mtx);
408
409 unsigned int canMessages=0;
410 bool ret=true; //return true by default
411
412 bool res=pCanBus->canRead(inBuffer,CAN_DRIVER_BUFFER_SIZE,&canMessages);
413 if (!res)
414 {
415 yError()<<"CanBusFtSensor canRead failed\n";
416 }
417
418 double timeNow=Time::now();
419 for (unsigned int i=0; i<canMessages; i++)
420 {
421 CanMessage &msg=inBuffer[i];
422
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);
428
429 //parse data here
430 status=MAS_status::MAS_OK;
431
432 if (type==0x03) //analog data
433 {
434 if (id==boardId)
435 {
436 timeStamp=Time::now();
437 switch (dataFormat)
438 {
439 case ANALOG_FORMAT_8_BIT:
440 ret=decode8(buff, msgid, data.data());
441 status=MAS_status::MAS_OK;
442 break;
443 case ANALOG_FORMAT_16_BIT:
444 if (len==6)
445 {
446 ret=decode16(buff, msgid, data.data());
447 status=MAS_status::MAS_OK;
448 }
449 else
450 {
451 if ((len==7) && (buff[6] != 0)) // changed from "== 1" in order to maintain compatibility with extra overflow information in byte 6.
452 {
453 status=MAS_status::MAS_OVF;
454 }
455 else
456 {
457 status=MAS_status::MAS_ERROR;
458 }
459 ret=decode16(buff, msgid, data.data());
460 }
461 break;
462 default:
463 status=MAS_status::MAS_ERROR;
464 ret=false;
465 }
466 }
467 }
468 }
469
470 //if 100ms have passed since the last received message
471 if (timeStamp+0.1<timeNow)
472 {
473 status=MAS_status::MAS_TIMEOUT;
474 }
475}
476
478{
479 yTrace("CanBusVirtualAnalogSensor Thread released\n");
480}
481
482//------------------------- ISixAxisForceTorqueSensors -------------------------
484{
485 return 1;
486}
487
488yarp::dev::MAS_status CanBusFtSensor::getSixAxisForceTorqueSensorStatus(size_t sens_index) const
489{
490 return yarp::dev::MAS_OK;
491}
492
493bool CanBusFtSensor::getSixAxisForceTorqueSensorName(size_t sens_index, std::string &name) const
494{
495 name = this->sensorName;
496 return true;
497}
498
499bool CanBusFtSensor::getSixAxisForceTorqueSensorFrameName(size_t sens_index, std::string &frameName) const
500{
501 frameName = this->frameName;
502 return true;
503}
504
505bool CanBusFtSensor::getSixAxisForceTorqueSensorMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
506{
507 std::lock_guard<std::mutex> lck(mtx);
508 out = data;
509 timestamp = timeStamp;
510
511 return true;
512}
513
const int CAN_DRIVER_BUFFER_SIZE
const int CAN_DRIVER_BUFFER_SIZE
@ data
virtual bool open(yarp::os::Searchable &config)
std::string frameName
ICanBus * pCanBus
unsigned short boardId
virtual yarp::dev::MAS_status getSixAxisForceTorqueSensorStatus(size_t sens_index) const override
CanBuffer outBuffer
virtual bool getSixAxisForceTorqueSensorFrameName(size_t sens_index, std::string &frameName) const override
virtual void run()
yarp::sig::Vector data
unsigned short useCalibration
PolyDriver driver
virtual size_t getNrOfSixAxisForceTorqueSensors() const override
ICanBufferFactory * pCanBufferFactory
AnalogDataFormat dataFormat
virtual bool getSixAxisForceTorqueSensorName(size_t sens_index, std::string &name) const override
std::string sensorName
unsigned int channelsNum
virtual bool threadInit()
yarp::sig::Vector scaleFactor
CanBuffer inBuffer
virtual bool getSixAxisForceTorqueSensorMeasure(size_t sens_index, yarp::sig::Vector &out, double &timestamp) const override
virtual void threadRelease()
virtual bool close()
fprintf(fid,'\n')
out
Definition sine.m:8