9 #include <yarp/os/Time.h>
10 #include <yarp/os/Log.h>
13 #include <yarp/os/Log.h>
14 #include <yarp/os/LogStream.h>
25 fprintf(stderr,
"%s\n", config.toString().c_str());
27 correct &= config.check(
"canbusDevice");
28 correct &= config.check(
"canDeviceNum");
29 correct &= config.check(
"canAddress");
30 correct &= config.check(
"format");
31 correct &= config.check(
"period");
32 correct &= config.check(
"channels");
33 correct &= config.check(
"fullScale");
37 yError()<<
"insufficient parameters to CanBusVirtualAnalogSensor\n";
41 int period=config.find(
"period").asInt32();
42 setPeriod((
double)period/1000.0);
46 prop.put(
"device", config.find(
"canbusDevice").asString().c_str());
47 prop.put(
"physDevice", config.find(
"physDevice").asString().c_str());
48 prop.put(
"canTxTimeout", 500);
49 prop.put(
"canRxTimeout", 500);
50 prop.put(
"canDeviceNum", config.find(
"canDeviceNum").asInt32());
51 prop.put(
"canMyAddress", 0);
59 if (!driver.isValid())
61 yError(
"Error opening PolyDriver check parameters\n");
67 yError (
"Error opening can device not available\n");
70 driver.view(pCanBufferFactory);
75 pCanBus->canSetBaudRate(0);
80 this->boardId = config.find(
"canAddress").asInt32();
81 this->canId = config.find(
"canDeviceNum").asInt32();
82 this->channelsNum = config.find(
"channels").asInt32();
83 this->useCalibration = (config.find(
"useCalibration").asInt32()==1);
84 unsigned int tmpFormat = config.find(
"format").asInt32();
86 this->dataFormat = ANALOG_FORMAT_8_BIT;
87 else if (tmpFormat == 16)
88 this->dataFormat = ANALOG_FORMAT_16_BIT;
90 this->dataFormat = ANALOG_FORMAT_ERR;
93 for (
int id=0;
id<16; ++id)
95 pCanBus->canIdAdd(0x300+(boardId<<4)+id);
97 pCanBus->canIdAdd(0x200+boardId);
98 pCanBus->canIdAdd(0x200+(boardId<<4));
101 int chan=config.find(
"channels").asInt32();
102 data.resize(channelsNum);
103 Bottle fullScaleTmp = config.findGroup(
"fullScale");
104 this->scaleFactor.resize(channelsNum);
105 for (
unsigned int i=0; i<channelsNum; i++)
107 double tmp = fullScaleTmp.get(i+1).asFloat64();
108 this->scaleFactor[i] =
tmp;
112 sensor_start(config);
114 PeriodicThread::start();
127 short int val[6] = {0,0,0,0,0,0};
128 static int count_saturation = 0;
130 static double curr_time = Time::now();
131 for (
int i=0; i<6; i++)
133 double fullScale = scaleFactor[i];
134 if (dval[i] > fullScale)
136 if (Time::now() - curr_time > 2)
139 yWarning(
"VIRTUAL FT SENSOR can:%d id:%d SATURATED CH:%d : %+4.4f COUNT: %d \n",this->canId,this->boardId, i, dval[i], count_saturation);
140 curr_time = Time::now();
145 else if (dval[i] < -fullScale)
147 if (Time::now() - curr_time > 2)
150 yWarning(
"VIRTUAL FT SENSOR can:%d id:%d SATURATED CH:%d : %+4.4f COUNT: %d \n",this->canId, this->boardId, i, dval[i], count_saturation);
151 curr_time = Time::now();
153 dval[i] = -fullScale;
156 val[i] = (
short int)(dval[i] / fullScale * 0x7fff)+0x8000;
159 unsigned int canMessages=0;
161 CanMessage &msg0=outBuffer[0];
163 fakeId = 0x300 + (boardId<<4)+ 0x0A;
165 msg0.getData()[1]=(val[0] >> 8) & 0xFF;
166 msg0.getData()[0]= val[0] & 0xFF;
167 msg0.getData()[3]=(val[1] >> 8) & 0xFF;
168 msg0.getData()[2]= val[1] & 0xFF;
169 msg0.getData()[5]=(val[2] >> 8) & 0xFF;
170 msg0.getData()[4]= val[2] & 0xFF;
172 pCanBus->canWrite(outBuffer, 1, &canMessages);
174 CanMessage &msg1=outBuffer[0];
176 fakeId = 0x300 + (boardId<<4)+ 0x0B;
178 msg1.getData()[1]=(val[3] >> 8) & 0xFF;
179 msg1.getData()[0]= val[3] & 0xFF;
180 msg1.getData()[3]=(val[4] >> 8) & 0xFF;
181 msg1.getData()[2]= val[4] & 0xFF;
182 msg1.getData()[5]=(val[5] >> 8) & 0xFF;
183 msg1.getData()[4]= val[5] & 0xFF;
186 pCanBus->canWrite(outBuffer, 1, &canMessages);
191 bool CanBusVirtualAnalogSensor::readFullScaleAnalog(
int ch)
193 scaleFactor[ch]=1
e-20;
195 unsigned int canMessages=0;
196 unsigned id = 0x200 + boardId;
197 CanMessage &msg=outBuffer[0];
199 msg.getData()[0]=0x18;
203 pCanBus->canWrite(outBuffer, 1, &canMessages);
206 bool full_scale_read=
false;
210 unsigned int read_messages = 0;
211 bool b = pCanBus->canRead(inBuffer,max_messages,&read_messages,
false);
213 for (
unsigned int i=0; i<read_messages; i++)
215 CanMessage& m= inBuffer[i];
216 unsigned int currId=m.getId();
217 if (currId==(0x0200 | boardId << 4))
219 m.getData()[0]==0x18 &&
222 scaleFactor[ch]=m.getData()[2]<<8 | m.getData()[3];
223 full_scale_read=
true;
227 yarp::os::Time::delay(0.002);
230 while(timeout<32 && full_scale_read==
false);
232 if (full_scale_read==
false)
234 yError(
"Trying to get fullscale data from sensor 0x%X: no answer received or message lost (ch:%d)\n", boardId, ch);
241 bool CanBusVirtualAnalogSensor::sensor_start(yarp::os::Searchable& analogConfig)
243 fprintf(stderr,
"--> Initializing analog device %s\n", analogConfig.find(
"deviceId").toString().c_str());
336 bool CanBusVirtualAnalogSensor::sensor_stop()
338 unsigned int canMessages=0;
339 unsigned id = 0x200 + boardId;
340 CanMessage &msg=outBuffer[0];
342 msg.getData()[0]=0x07;
343 msg.getData()[1]=0x01;
346 pCanBus->canWrite(outBuffer, 1, &canMessages);
353 PeriodicThread::stop();
359 if (pCanBufferFactory)
361 pCanBufferFactory->destroyBuffer(inBuffer);
362 pCanBufferFactory->destroyBuffer(outBuffer);
371 return yarp::dev::VAS_status::VAS_OK;
384 bool CanBusVirtualAnalogSensor::decode16(
const unsigned char *msg,
int msg_id,
double *
data)
386 const char groupId=(msg_id & 0x00f);
395 data[k]=(((
unsigned short)(msg[2*k+1]))<<8)+msg[2*k]-0x8000;
396 if (useCalibration==1)
398 data[k]=
data[k]*scaleFactor[k]/float(0x8000);
407 data[k+3]=(((
unsigned short)(msg[2*k+1]))<<8)+msg[2*k]-0x8000;
408 if (useCalibration==1)
410 data[k+3]=
data[k+3]*scaleFactor[k+3]/float(0x8000);
422 fprintf(stderr,
"Warning, got unexpected class 0x3 msg(s): groupId 0x%x\n", groupId);
433 bool CanBusVirtualAnalogSensor::decode8(
const unsigned char *msg,
int msg_id,
double *
data)
435 const char groupId=(msg_id & 0x00f);
442 for(
int k=0;k<=6;k++)
448 for(
int k=0;k<=7;k++)
459 fprintf(stderr,
"Warning, got unexpected class 0x3 msg(s): groupId 0x%x\n", groupId);
471 std::lock_guard<std::mutex> lck(mtx);
473 unsigned int canMessages=0;
479 std::cerr<<
"canRead failed\n";
482 double timeNow=Time::now();
483 for (
unsigned int i=0; i<canMessages; i++)
485 CanMessage &msg=inBuffer[i];
487 unsigned int msgid = msg.getId();
488 unsigned char *buff = msg.getData();
489 unsigned int len = msg.getLen();
490 const char type = ((msgid&0x700)>>8);
493 status=yarp::dev::VAS_status::VAS_OK;
497 unsigned int id = (msgid & 0x00f);
503 unsigned int channel = buff[1];
505 unsigned int canMessages=0;
506 unsigned id = 0x200 + (boardId << 4);
507 CanMessage &msg=outBuffer[0];
509 int tmp_fullscale = (int) scaleFactor[channel];
510 unsigned char h_scale = (tmp_fullscale >> 8) & 0xff;
511 unsigned char l_scale = tmp_fullscale & 0xff;
512 msg.getData()[0]=0x18;
513 msg.getData()[1]=channel;
514 msg.getData()[2]=h_scale;
515 msg.getData()[3]=l_scale;
518 pCanBus->canWrite(outBuffer, 1, &canMessages);
525 unsigned int id = (msgid & 0x00f0)>>4;
528 timeStamp=Time::now();
531 case ANALOG_FORMAT_8_BIT:
532 ret=decode8(buff, msgid,
data.data());
533 status=yarp::dev::VAS_status::VAS_OK;
535 case ANALOG_FORMAT_16_BIT:
538 ret=decode16(buff, msgid,
data.data());
539 status=yarp::dev::VAS_status::VAS_OK;
543 if (len==7 && buff[6] == 1)
545 status= yarp::dev::VAS_status::VAS_OVF;
549 status= yarp::dev::VAS_status::VAS_ERROR;
551 ret=decode16(buff, msgid,
data.data());
555 status=yarp::dev::VAS_status::VAS_ERROR;
563 if (timeStamp+0.1<timeNow)
565 status= yarp::dev::VAS_status::VAS_TIMEOUT;
571 yTrace(
"CanBusVirtualAnalogSensor Thread released\n");
const int CAN_DRIVER_BUFFER_SIZE
virtual int getVirtualAnalogSensorChannels()
virtual yarp::dev::VAS_status getVirtualAnalogSensorStatus(int ch)
virtual bool updateVirtualAnalogSensorMeasure(int ch, double &measure)
virtual void threadRelease()
virtual bool threadInit()
virtual bool open(yarp::os::Searchable &config)