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; }