bool Tracks::getValue(const std::string &output, const int length)
{
//int len = output.length();
int start;
//int dataLen = stoi(output.substr(0,2));
int deviceId;
std::string buff;
float value;
buff = output.substr(0,4);
value = dataToFloat32(buff);
std::string payload = output.substr(0,length*5+4);
//payload[3] = (unsigned char)0x0d;
unsigned short crc = (unsigned short)output[length*5+4]<<8;
crc +=output[length*5+5];
unsigned short crc_cal = getCRC16(payload);
if(crc!=crc_cal)
return false;
printf("crc: %x %x",crc,crc_cal);
//int cast = reinterpret_cast<int>(value);
printf("Head : %x %x %x %x \n",(unsigned char)buff[0],(unsigned char)buff[1],(unsigned char)buff[2],(unsigned char)buff[3]);
int j=0;
for(start = 4;start<(length*5+1);start+=5)
{
buff = output.substr(start,5);
value = dataToFloat32(buff);
printf("%x %x %x %x %x\t\t",(unsigned char)buff[0],(unsigned char)buff[1],(unsigned char)buff[2],(unsigned char)buff[3],(unsigned char)buff[4]);
std::cout<<value<<"\n";
_data.data[j++]=value;
}
std::cout<<std::endl;
return true;
}
void Tracks::setAllMode()
{
//char payload[16] = {0x00, 0x12, 0x0D,0x19, 0x18, 0x05, 0x15, 0x16, 0x17, 0x1B, 0x1C, 0x1D, 0x4A, 0x4B, 0x4C, 0x07};
//{0x19, 0x18, 0x05, 0x15, 0x16, 0x17, 0x1B, 0x1C, 0x1D, 0x4A, 0x4B, 0x4C, 0x07};
sensor_used = 0x0D;
std::string payload;
payload+=(char)0x00;
payload+=(char)(sensor_used+6);
payload+=(char)0x03;
payload+=(char)sensor_used;
payload+=(char)0x19;//r
payload+=(char)0x18;//p
payload+=(char)0x05;//y
payload+=(char)0x15;//ax
payload+=(char)0x16;//ay
payload+=(char)0x17;//az
payload+=(char)0x1B;//mx
payload+=(char)0x1C;//my
payload+=(char)0x1D;//mz
payload+=(char)0x4A;//gx
payload+=(char)0x4B;//gy
payload+=(char)0x4C;//gz
payload+=(char)0x07;//4*/
unsigned short crc= getCRC16(payload);
payload+=(char)(crc>>8);
payload+=(char)(crc);
this->writeData(payload);
usleep(_delay);
}
int makeSendMsg(UINT8 *body, UINT32 encryptLen,UINT16 cmdType, UINT8 *sendbuf)
{
UINT16 crc;
makeHeader(cmdType, encryptLen, sendbuf);
makeBody(body, encryptLen, sendbuf);
crc=getCRC16(sendbuf, 10+encryptLen);
makeTail(crc, encryptLen, sendbuf);
return 0;
}
void Tracks::getData(int dataLength)
{
//char getData[8] = {0x00,0x05,0x04,0xbf,0x71};
std::string payload;
payload+=(char)0x00;
payload+=(char)0x05;
payload+=(char)0x04;
unsigned short crc= getCRC16(payload);
payload+=(char)(crc>>8);
payload+=(char)(crc);
this->writeData(payload);
usleep(_delay);
}
void Tracks::setContinousMode()
{
std::string payload;
payload+=(char)0x00;
payload+=(char)0x05;
payload+=(char)0x15;
//payload+=(char)0x07;//4*/
unsigned short crc= getCRC16(payload);
payload+=(char)(crc>>8);
payload+=(char)(crc);
this->writeData(payload);
usleep(_delay);
}
void Tracks::setMode(int deviceNum)
{
std::string payload;
payload+=(char)0x00;
payload+=(char)0x07;
payload+=(char)0x03 ;
payload+=(char)0x01 ;
payload+=(char)componentId[deviceNum];
unsigned short crc= getCRC16(payload);
payload+=(char)(crc>>8);
payload+=(char)(crc);
this->writeData(payload);
usleep(_delay);
}
void Tracks::setTemp()
{
sensor_used = 0x01;
std::string payload;
payload+=(char)0x00;
payload+=(char)(sensor_used+6);
payload+=(char)0x03;
payload+=(char)sensor_used;
payload+=(char)0x07;//4
unsigned short crc= getCRC16(payload);
payload+=(char)(crc>>8);
payload+=(char)(crc);
this->writeData(payload);
usleep(_delay);
}
bool Tracks::getTemp()
{
setTemp();
int start;
std::string output;
getData(4);
usleep(_delay);
while(ros::ok())
{
readData(output,1);
if(output[0]==0x00)
break;
}
this->readData(output, 5 + 5*sensor_used);
output.insert(output.begin(),0x00);
std::string buff;
float value;
buff = output.substr(0,4);
value = dataToFloat32(buff);
std::string payload = output.substr(0,sensor_used*5+4);
unsigned short crc = (unsigned short)output[sensor_used*5+4]<<8;
crc +=output[sensor_used*5+5];
unsigned short crc_cal = getCRC16(payload);
if(crc!=crc_cal)
return false;
printf("crc: %x %x ",crc,crc_cal);
//int cast = reinterpret_cast<int>(value);
printf("Temp : %x %x %x %x \n",(unsigned char)buff[0],(unsigned char)buff[1],(unsigned char)buff[2],(unsigned char)buff[3]);
int j=temp;
for(start = 4;start<(sensor_used*5+1);start+=5)
{
buff = output.substr(start,5);
value = dataToFloat32(buff);
printf("%x %x %x %x %x\t\t",(unsigned char)buff[0],(unsigned char)buff[1],(unsigned char)buff[2],(unsigned char)buff[3],(unsigned char)buff[4]);
std::cout<<value<<"\n";
_data.data[j++]=value;
}
std::cout<<std::endl;
return true;
}
