/** * \brief Process next block '[...]' in string * * @param[in] str string * @param[in] fmt format string ('i' for 'int', 'f' for 'float') * @param[out] idat array for 'int' values (long enough due to fmt) * @param[out] fdat array for 'float' values (long enough due to fmt) * @param[out] ddat array for 'double' values (long enough due to fmt) * @return pointer behind read value in str; NULL in case of error */ char *string_get_block(char *str, const char *fmt, int *idat, float *fdat, double *ddat) { char *strend; int index_i, index_f; if ((str = strchr(str, '[')) == NULL) { // search begin of block return NULL; } if ((strend = strchr(str, ']')) == NULL) { // search end of block return NULL; } str++; // remove delimiters *strend = '\0'; index_i = index_f = 0; while (*fmt) { switch (*fmt++) { case 'i': if ((str = string_get_i(str, &idat[index_i++])) == NULL) { *strend = ']'; return NULL; } break; case 'f': if ((str = string_get_f(str, &fdat[index_f++])) == NULL) { *strend = ']'; return NULL; } break; case 'd': if ((str = string_get_d(str, &ddat[index_f++])) == NULL) { *strend = ']'; return NULL; } break; default: // unknown format character *strend = ']'; return NULL; } } // ignore additional data inside the block *strend = ']'; return strend + 1; }
bool DTrack2::receive(void) { char* s; int i, j, k, l, n, len, id; char sfmt[20]; int iarr[3]; float f, farr[6]; int loc_num_bodycal, loc_num_handcal, loc_num_flystick1, loc_num_meatool; if(!valid()){ set_data_neterror(); return false; } // defaults: act_framecounter = 0; act_timestamp = -1; // i.e. not available loc_num_bodycal = loc_num_handcal = -1; // i.e. not available loc_num_flystick1 = loc_num_meatool = 0; // receive UDP packet: len = udp_receive(d_udpsock, d_udpbuf, d_udpbufsize-1, d_udptimeout_us); if(len == -1){ set_data_timeout(); return false; } if(len <= 0){ set_data_neterror(); return false; } s = d_udpbuf; s[len] = '\0'; // process lines: set_data_parseerror(); do{ // line for frame counter: if(!strncmp(s, "fr ", 3)){ s += 3; if(!(s = string_get_ui(s, &act_framecounter))){ // get frame counter act_framecounter = 0; return false; } continue; } // line for timestamp: if(!strncmp(s, "ts ", 3)){ s += 3; if(!(s = string_get_d(s, &act_timestamp))){ // get timestamp act_timestamp = -1; return false; } continue; } // line for additional information about number of calibrated bodies: if(!strncmp(s, "6dcal ", 6)){ s += 6; if(!(s = string_get_i(s, &loc_num_bodycal))){ // get number of calibrated bodies return false; } continue; } // line for standard body data: if(!strncmp(s, "6d ", 3)){ s += 3; for(i=0; i<act_num_body; i++){ // disable all existing data memset(&act_body[i], 0, sizeof(dtrack2_body_type)); act_body[i].id = i; act_body[i].quality = -1; } if(!(s = string_get_i(s, &n))){ // get number of standard bodies (in line) return false; } for(i=0; i<n; i++){ // get data of standard bodies if(!(s = string_get_block(s, "if", &id, &f))){ return false; } if(id >= act_num_body){ // adjust length of vector act_body.resize(id + 1); for(j=act_num_body; j<=id; j++){ memset(&act_body[j], 0, sizeof(dtrack2_body_type)); act_body[j].id = j; act_body[j].quality = -1; } act_num_body = id + 1; } act_body[id].id = id; act_body[id].quality = f; if(!(s = string_get_block(s, "fff", NULL, act_body[id].loc))){ return false; } if(!(s = string_get_block(s, "fffffffff", NULL, act_body[id].rot))){ return false; } } continue; } // line for Flystick data (older format): if(!strncmp(s, "6df ", 4)){ s += 4; if(!(s = string_get_i(s, &n))){ // get number of calibrated Flysticks return false; } loc_num_flystick1 = n; if(n != act_num_flystick){ // adjust length of vector act_flystick.resize(n); act_num_flystick = n; } for(i=0; i<n; i++){ // get data of Flysticks if(!(s = string_get_block(s, "ifi", iarr, &f))){ return false; } if(iarr[0] != i){ // not expected return false; } act_flystick[i].id = iarr[0]; act_flystick[i].quality = f; act_flystick[i].num_button = 8; k = iarr[1]; for(j=0; j<8; j++){ act_flystick[i].button[j] = k & 0x01; k >>= 1; } act_flystick[i].num_joystick = 2; // additionally to buttons 5-8 if(iarr[1] & 0x20){ act_flystick[i].joystick[0] = -1; }else if(iarr[1] & 0x80){ act_flystick[i].joystick[0] = 1; }else{ act_flystick[i].joystick[0] = 0; } if(iarr[1] & 0x10){ act_flystick[i].joystick[1] = -1; }else if(iarr[1] & 0x40){ act_flystick[i].joystick[1] = 1; }else{ act_flystick[i].joystick[1] = 0; } if(!(s = string_get_block(s, "fff", NULL, act_flystick[i].loc))){ return false; } if(!(s = string_get_block(s, "fffffffff", NULL, act_flystick[i].rot))){ return false; } } continue; } // line for Flystick data (newer format): if(!strncmp(s, "6df2 ", 5)){ s += 5; if(!(s = string_get_i(s, &n))){ // get number of calibrated Flysticks return false; } if(n != act_num_flystick){ // adjust length of vector act_flystick.resize(n); act_num_flystick = n; } if(!(s = string_get_i(s, &n))){ // get number of Flysticks return false; } for(i=0; i<n; i++){ // get data of Flysticks if(!(s = string_get_block(s, "ifii", iarr, &f))){ return false; } if(iarr[0] != i){ // not expected return false; } act_flystick[i].id = iarr[0]; act_flystick[i].quality = f; if(iarr[1] > DTRACK2_FLYSTICK_MAX_BUTTON){ return false; } if(iarr[1] > DTRACK2_FLYSTICK_MAX_JOYSTICK){ return false; } act_flystick[i].num_button = iarr[1]; act_flystick[i].num_joystick = iarr[2]; if(!(s = string_get_block(s, "fff", NULL, act_flystick[i].loc))){ return false; } if(!(s = string_get_block(s, "fffffffff", NULL, act_flystick[i].rot))){ return false; } strcpy(sfmt, ""); j = 0; while(j < act_flystick[i].num_button){ strcat(sfmt, "i"); j += 32; } j = 0; while(j < act_flystick[i].num_joystick){ strcat(sfmt, "f"); j++; } if(!(s = string_get_block(s, sfmt, iarr, act_flystick[i].joystick))){ return false; } k = l = 0; for(j=0; j<act_flystick[i].num_button; j++){ act_flystick[i].button[j] = iarr[k] & 0x01; iarr[k] >>= 1; l++; if(l == 32){ k++; l = 0; } } } continue; }