/**
* \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;
}
