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