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);
61 yError(
"Error opening PolyDriver check parameters\n");
67 yError (
"Error opening can device not available\n");
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();
87 else if (tmpFormat == 16)
93 for (
int id=0;
id<16; ++id)
101 int chan=config.find(
"channels").asInt32();
103 Bottle fullScaleTmp = config.findGroup(
"fullScale");
107 double tmp = fullScaleTmp.get(i+1).asFloat64();
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++)
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;
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;
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;
191bool CanBusVirtualAnalogSensor::readFullScaleAnalog(
int ch)
195 unsigned int canMessages=0;
199 msg.getData()[0]=0x18;
206 bool full_scale_read=
false;
210 unsigned int read_messages = 0;
213 for (
unsigned int i=0; i<read_messages; 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);
241bool CanBusVirtualAnalogSensor::sensor_start(yarp::os::Searchable& analogConfig)
243 fprintf(stderr,
"--> Initializing analog device %s\n", analogConfig.find(
"deviceId").toString().c_str());
336bool CanBusVirtualAnalogSensor::sensor_stop()
338 unsigned int canMessages=0;
342 msg.getData()[0]=0x07;
343 msg.getData()[1]=0x01;
353 PeriodicThread::stop();
371 return yarp::dev::VAS_status::VAS_OK;
384bool 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;
407 data[k+3]=(((
unsigned short)(msg[2*k+1]))<<8)+msg[2*k]-0x8000;
422 fprintf(stderr,
"Warning, got unexpected class 0x3 msg(s): groupId 0x%x\n", groupId);
433bool 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++)
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);
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;
525 unsigned int id = (msgid & 0x00f0)>>4;
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;
565 status= yarp::dev::VAS_status::VAS_TIMEOUT;
571 yTrace(
"CanBusVirtualAnalogSensor Thread released\n");
const int CAN_DRIVER_BUFFER_SIZE
const int CAN_DRIVER_BUFFER_SIZE
yarp::dev::VAS_status status
yarp::sig::Vector scaleFactor
virtual int getVirtualAnalogSensorChannels()
virtual yarp::dev::VAS_status getVirtualAnalogSensorStatus(int ch)
AnalogDataFormat dataFormat
virtual bool updateVirtualAnalogSensorMeasure(int ch, double &measure)
virtual void threadRelease()
virtual bool threadInit()
ICanBufferFactory * pCanBufferFactory
virtual bool open(yarp::os::Searchable &config)