Changeset 20359

Show
Ignore:
Timestamp:
06/27/07 11:14:50 (1 year ago)
Author:
patrick
Message:

MF22 r20358:

Enhanced version of the A.R.T. DTrack driver. This version provides the
following:

  • support of the new 'Flystick2' input device, especially its (now
    really analog) joystick,
  • support of A.R.T. 'measurement tools',
  • a bug fix: transmission of joystick (or hat switch) actions of
    'Flystick1' input devices into 'Analog' values was not correct.

Submitted by: Kurt Achatz < kurt dot achatz at ar-tracking dot de >

Files:

Legend:

Unmodified
Added
Removed
Modified
Copied
Moved

  • juggler/trunk/modules/gadgeteer/drivers/ART/DTrack/DTrack.cpp

    r18827 r20359  
    11/* Gadgeteer Driver for 'A.R.T. DTrack' Tracker 
    2  * Copyright (C) 2005, Advanced Realtime Tracking GmbH 
     2 * Copyright (C) 2005-2007, Advanced Realtime Tracking GmbH 
    33 *  
    44 * This library is free software; you can redistribute it and/or 
    …… 
    2222 *          Kurt Achatz, Advanced Realtime Tracking GmbH (http://www.ar-tracking.de) 
    2323 * 
    24  * Last modified: 2005/06/16 
     24 * Last modified: 2007/06/20 
    2525 * 
    26  * DTrack.cpp,v 1.2 2005/06/16 14:43:31 kurt Exp 
     26 * DTrack.cpp,v 1.5 2007/06/20 15:12:58 kurt Exp 
    2727 */ 
    2828 
    …… 
    4343// some constants: 
    4444 
    45 #define VERSION_STRING   "DTrack Driver v0.1.0; (C) 2005, Advanced Realtime Tracking GmbH" 
     45#define VERSION_STRING   "DTrack Driver v0.2.0; (C) 2005-2007, Advanced Realtime Tracking GmbH" 
    4646 
    4747#define BUTTONS_PER_FLYSTICK    8  // number of buttons per 'Flystick' (fixed) 
    4848#define VALUATORS_PER_FLYSTICK  2  // number of valuators per 'Flystick' (fixed) 
     49#define BUTTONS_PER_MEATOOL     2  // number of buttons per 'Measurement Tool' (fixed) 
    4950 
    5051 
    …… 
    9899{ 
    99100 
    100         return "dtrack"; 
     101        return std::string("dtrack"); 
    101102} 
    102103 
    …… 
    166167 
    167168        if(use_commands){ 
    168                 standalone->send(DTRACKLIB_CMD_CAMERAS_AND_CALC_ON); 
    169                 standalone->send(DTRACKLIB_CMD_SEND_DATA); 
     169                standalone->cmd_cameras(true); 
    170170        } 
    171171 
    …… 
    212212 
    213213        if(use_commands){ 
    214                 standalone->send(DTRACKLIB_CMD_STOP_DATA); 
    215                 standalone->send(DTRACKLIB_CMD_CAMERAS_OFF); 
     214                standalone->cmd_cameras(false); 
    216215        } 
    217216 
    …… 
    246245{ 
    247246        bool stat; 
    248         unsigned long i; 
    249         unsigned long nbody, nflystick; 
     247        int i, j, id; 
     248        int nbt, nvt; 
     249        int num_body, num_flystick, num_meatool; 
    250250 
    251251        if(!thrRunning){ 
    …… 
    259259        } 
    260260 
    261         nbody = (unsigned long )standalone->get_nbody(); 
    262         nflystick = (unsigned long )standalone->get_nflystick(); 
    263  
    264         if(standalone->get_nbodycal() > -1){        // number of calibrated bodies available 
    265                 unsigned long nbodycal = (unsigned long )standalone->get_nbodycal(); 
    266                 nbodycal -= standalone->get_nmeatool();  // ignore measurement tools 
     261        num_body = standalone->get_num_body(); 
     262        num_flystick = standalone->get_num_flystick(); 
     263        num_meatool = standalone->get_num_meatool(); 
    267264         
    268                 resize_curPosition(nbodycal); 
    269         } 
     265        resize_curPosition(num_flystick + num_meatool + num_body); 
     266        resize_curDigital(num_flystick * BUTTONS_PER_FLYSTICK + num_meatool * BUTTONS_PER_MEATOOL); 
     267        resize_curAnalog(num_flystick * VALUATORS_PER_FLYSTICK); 
    270268 
    271269        // get 'Flystick' data: 
    272270 
    273         resize_curPosition(nflystick); 
    274  
    275         if(nflystick * BUTTONS_PER_FLYSTICK > curDigital.size()){ 
    276                 curDigital.resize(nflystick * BUTTONS_PER_FLYSTICK);   // all elements will be set later 
    277         } 
    278  
    279         if(nflystick * VALUATORS_PER_FLYSTICK > curAnalog.size()){ 
    280                 curAnalog.resize(nflystick * VALUATORS_PER_FLYSTICK);  // all elements will be set later 
    281         } 
    282  
    283         for(i=0; i<nflystick; i++){ 
    284                 dtracklib_flystick_type dat = standalone->get_flystick(i); 
     271        for(i=0; i<num_flystick; i++){ 
     272                dtrack_flystick_type dat = standalone->get_flystick(i); 
    285273                 
    286274                if(dat.quality >= 0){  // check if Flystick position is tracked 
    …… 
    291279                // Flystick buttons: 
    292280 
    293                 unsigned long bt = dat.bt; 
    294                 for(int j=0; j<BUTTONS_PER_FLYSTICK; j++){ 
    295                         curDigital[i*BUTTONS_PER_FLYSTICK + j] = bt & 0x0001; 
    296                         curDigital[i*BUTTONS_PER_FLYSTICK + j].setTime(); 
     281                nbt = dat.num_button; 
     282                 
     283                if(nbt > BUTTONS_PER_FLYSTICK){ 
     284                        nbt = BUTTONS_PER_FLYSTICK; 
     285                } 
     286                 
     287                for(j=0; j<nbt; j++){ 
     288                        id = j + i * BUTTONS_PER_FLYSTICK;  // VRJuggler id number 
    297289                         
    298                         bt >>= 1; 
    299                 } 
    300                  
    301                 // Flystick 'HAT switch' (valuators simulated using four buttons): 
    302                  
    303                 for(int j=0; j<VALUATORS_PER_FLYSTICK; j++){ 
    304                         curAnalog[i*VALUATORS_PER_FLYSTICK + j] = 0; 
    305                         curAnalog[i*VALUATORS_PER_FLYSTICK + j].setTime(); 
    306                 } 
    307                  
    308                 float f, fn; 
    309  
    310                 f = 0; 
    311                 if(dat.bt & 0x0020){   
    312                         f = -1; 
    313                 }else if(dat.bt & 0x0080){ 
    314                         f = 1; 
    315                 } 
    316  
    317                 if(f != 0){ 
    318                         this->normalizeMinToMax(f, fn); 
    319                         curAnalog[i*VALUATORS_PER_FLYSTICK + 0] = fn; 
    320                 } 
    321  
    322                 f = 0; 
    323                 if(dat.bt & 0x0010){   
    324                         f = -1; 
    325                 }else if(dat.bt & 0x0040){ 
    326                         f = 1; 
    327                 } 
    328  
    329                 if(f != 0){ 
    330                         this->normalizeMinToMax(f, fn); 
    331                         curAnalog[i*VALUATORS_PER_FLYSTICK + 1] = fn; 
     290                        curDigital[id] = dat.button[j]; 
     291                        curDigital[id].setTime(); 
     292                } 
     293                 
     294                // Flystick valuators ('HAT switch' or 'joystick'): 
     295                 
     296                nvt = dat.num_joystick; 
     297                 
     298                if(nvt > VALUATORS_PER_FLYSTICK){ 
     299                        nvt = VALUATORS_PER_FLYSTICK; 
     300                } 
     301                 
     302                for(j=0; j<nvt; j++){ 
     303                        id = j + i * VALUATORS_PER_FLYSTICK;  // VRJuggler id number 
     304                         
     305                        curAnalog[id] = dat.joystick[j] / 2.0 + 0.5;  // normalizing 
     306                        curAnalog[id].setTime(); 
     307                } 
     308        } 
     309 
     310        // get 'measurement tool' data: 
     311 
     312        for(i=0; i<num_meatool; i++){ 
     313                dtrack_meatool_type dat = standalone->get_meatool(i); 
     314                 
     315                if(dat.quality >= 0){  // check if position is tracked 
     316                        id = i + num_flystick;  // VRJuggler id number 
     317                         
     318                        curPosition[id].mPosData = getpos(dat); 
     319                        curPosition[id].setTime(); 
     320                }                      // otherwise keep last valid position 
     321 
     322                // measurement tool buttons: 
     323 
     324                nbt = dat.num_button; 
     325                 
     326                if(nbt > BUTTONS_PER_MEATOOL){ 
     327                        nbt = BUTTONS_PER_MEATOOL; 
     328                } 
     329                 
     330                for(j=0; j<nbt; j++){ 
     331                        id = j + i * BUTTONS_PER_MEATOOL + num_flystick * BUTTONS_PER_FLYSTICK;  // VRJuggler id number 
     332                         
     333                        curDigital[id] = dat.button[j]; 
     334                        curDigital[id].setTime(); 
    332335                } 
    333336        } 
    …… 
    335338        // get 'standard body' data: 
    336339 
    337         for(i=0; i<nbody; i++){ 
    338                 dtracklib_body_type dat = standalone->get_body(i); 
    339  
    340                 unsigned long id = dat.id + nflystick;  // VRJuggler id number of this 'Standard Body' 
    341                 resize_curPosition(id+1); 
    342  
    343                 curPosition[id].mPosData = getpos(dat); 
    344                 curPosition[id].setTime(); 
    345         }  // bodies not in this list are not tracked: keep their last valid position 
     340        for(i=0; i<num_body; i++){ 
     341                dtrack_body_type dat = standalone->get_body(i); 
     342 
     343                if(dat.quality >= 0){  // check if position is tracked 
     344                        id = i + num_flystick + num_meatool;  // VRJuggler id number 
     345 
     346                        curPosition[id].mPosData = getpos(dat); 
     347                        curPosition[id].setTime(); 
     348                }                      // otherwise keep last valid position 
     349        } 
    346350 
    347351        // update buffers: 
    …… 
    373377// n (i): new size 
    374378 
    375 void DTrack::resize_curPosition(unsigned long n) 
    376 { 
    377         unsigned long nsize = curPosition.size(); 
     379void DTrack::resize_curPosition(int n) 
     380{ 
     381        int nsize = (int )curPosition.size(); 
    378382 
    379383        if(n > nsize){ 
    380384                curPosition.resize(n); 
    381385 
    382                 for(unsigned long i=nsize; i<n; i++){  // set default for new elements 
     386                for(int i=nsize; i<n; i++){  // set default for new elements 
    383387                        curPosition[i].mPosData = getpos_default(); 
    384388                        curPosition[i].setTime(); 
    …… 
    387391} 
    388392 
     393// Resize curDigital vector: 
     394// n (i): new size 
     395 
     396void DTrack::resize_curDigital(int n) 
     397{ 
     398        int nsize = (int )curDigital.size(); 
     399 
     400        if(n > nsize){ 
     401                curDigital.resize(n); 
     402 
     403                for(int i=nsize; i<n; i++){  // set default for new elements 
     404                        curDigital[i] = 0; 
     405                        curDigital[i].setTime(); 
     406                } 
     407        } 
     408} 
     409 
     410// Resize curAnalog vector: 
     411// n (i): new size 
     412 
     413void DTrack::resize_curAnalog(int n) 
     414{ 
     415        int nsize = (int )curAnalog.size(); 
     416        float fn; 
     417 
     418        if(n > nsize){ 
     419                curAnalog.resize(n); 
     420 
     421                for(int i=nsize; i<n; i++){  // set default for new elements 
     422                        this->normalizeMinToMax(0, fn); 
     423                        curAnalog[i] = fn; 
     424                        curAnalog[i].setTime(); 
     425                } 
     426        } 
     427} 
     428 
    389429 
    390430// Transfer DTracklib pose into gmtl pose: 
    …… 
    393433// return value (o): gmtl pose 
    394434 
    395 gmtl::Matrix44f DTrack::getpos(dtracklib_body_type& bod) 
     435gmtl::Matrix44f DTrack::getpos(dtrack_body_type& bod) 
    396436{ 
    397437        gmtl::Matrix44f ret_val; 
    …… 
    411451} 
    412452 
    413 gmtl::Matrix44f DTrack::getpos(dtracklib_flystick_type& bod) 
     453gmtl::Matrix44f DTrack::getpos(dtrack_flystick_type& bod) 
    414454{ 
    415455        gmtl::Matrix44f ret_val; 
    …… 
    429469} 
    430470 
     471gmtl::Matrix44f DTrack::getpos(dtrack_meatool_type& bod) 
     472{ 
     473        gmtl::Matrix44f ret_val; 
     474 
     475        for(int i=0; i<3; i++){ 
     476                for(int j=0; j<3; j++){ 
     477                        ret_val[i][j] = bod.rot[i + 3*j]; 
     478                } 
     479 
     480                ret_val[3][i] = 0; 
     481                ret_val[i][3] = bod.loc[i] / 1000;  // convert to meters 
     482        } 
     483 
     484        ret_val[3][3] = 1; 
     485 
     486        return ret_val; 
     487} 
     488 
    431489gmtl::Matrix44f DTrack::getpos_default(void) 
    432490{ 

  • juggler/trunk/modules/gadgeteer/drivers/ART/DTrack/DTrack.h

    r20356 r20359  
    11/* Gadgeteer Driver for 'A.R.T. DTrack' Tracker 
    2  * Copyright (C) 2005, Advanced Realtime Tracking GmbH 
     2 * Copyright (C) 2005-2007, Advanced Realtime Tracking GmbH 
    33 *  
    44 * This library is free software; you can redistribute it and/or 
    …… 
    2222 *          Kurt Achatz, Advanced Realtime Tracking GmbH (http://www.ar-tracking.de) 
    2323 * 
    24  * Last modified: 2005/05/28 
     24 * Last modified: 2007/03/28 
    2525 * 
    26  * DTrack.h,v 1.2 2005/06/16 14:43:31 kurt Exp 
     26 * DTrack.h,v 1.3 2007/06/20 15:10:55 kurt Exp 
    2727 */ 
    2828 
    …… 
    7575 
    7676        DTrackStandalone* standalone; 
    77         unsigned short receive_port; 
     77        int receive_port; 
    7878        bool use_commands; 
    7979        std::string server_name; 
    80         unsigned short command_port; 
     80        int command_port; 
    8181 
    8282        std::vector<PositionData> curPosition; 
    …… 
    8484        std::vector<AnalogData> curAnalog; 
    8585         
    86         void resize_curPosition(unsigned long n); 
     86        void resize_curPosition(int n); 
     87        void resize_curDigital(int n); 
     88        void resize_curAnalog(int n); 
    8789 
    88         gmtl::Matrix44f getpos(dtracklib_body_type& bod); 
    89         gmtl::Matrix44f getpos(dtracklib_flystick_type& bod); 
     90        gmtl::Matrix44f getpos(dtrack_body_type& bod); 
     91        gmtl::Matrix44f getpos(dtrack_flystick_type& bod); 
     92        gmtl::Matrix44f getpos(dtrack_meatool_type& bod); 
    9093        gmtl::Matrix44f getpos_default(void); 
    9194}; 

  • juggler/trunk/modules/gadgeteer/drivers/ART/DTrack/DTrackStandalone.cpp

    r19847 r20359  
    11/* Gadgeteer Driver for 'A.R.T. DTrack' Tracker 
    2  * Copyright (C) 2005, Advanced Realtime Tracking GmbH 
     2 * Copyright (C) 2005-2007, Advanced Realtime Tracking GmbH 
    33 *  
    44 * This library is free software; you can redistribute it and/or 
    …… 
    1818 * 
    1919 * Purpose: standalone driver class; derived from the A.R.T. sample source 
    20  *          code 'DTracklib' (version v1.2.1), just modified for use with VPR 
     20 *          code 'DTrackSDK' (version v1.3.1), just modified for use with VPR 
    2121 * 
    2222 * Authors: Kurt Achatz, Advanced Realtime Tracking GmbH (http://www.ar-tracking.de) 
    2323 * 
    24  * Last modified: 2005/06/14 
     24 * Last modified: 2007/03/27 
    2525 * 
    26  * DTrackStandalone.cpp,v 1.2 2005/06/16 14:44:01 kurt Exp 
     26 * DTrackStandalone.cpp,v 1.4 2007/06/20 15:15:12 kurt Exp 
    2727 */ 
    2828 
    …… 
    4545// Local error codes: 
    4646 
    47 #define DTRACKLIB_ERR_NONE       0 
    48 #define DTRACKLIB_ERR_TIMEOUT    1  // timeout while receiving data 
    49 #define DTRACKLIB_ERR_UDP        2  // UDP receive error 
    50 #define DTRACKLIB_ERR_PARSE      3  // error in UDP packet 
     47#define DTRACK_ERR_NONE       0  // no error 
     48#define DTRACK_ERR_TIMEOUT    1  // timeout while receiving data 
     49#define DTRACK_ERR_UDP        2  // UDP receive error 
     50#define DTRACK_ERR_PARSE      3  // error in UDP packet 
     51 
     52// DTrack remote commands: 
     53 
     54#define DTRACK_CMD_CAMERAS_OFF           1 
     55#define DTRACK_CMD_CAMERAS_ON            2 
     56#define DTRACK_CMD_CAMERAS_AND_CALC_ON   3 
     57 
     58#define DTRACK_CMD_SEND_DATA            11 
     59#define DTRACK_CMD_STOP_DATA            12 
     60#define DTRACK_CMD_SEND_N_DATA          13 
    5161 
    5262 
    5363// Local prototypes: 
    5464 
    55 static char* string_nextline(char* str, char* start, unsigned long len); 
    56 static char* string_get_ul(char* str, unsigned long* ul); 
     65static char* string_nextline(char* str, char* start, int len); 
     66static char* string_get_i(char* str, int* i); 
     67static char* string_get_ui(char* str, unsigned int* ui); 
    5768static char* string_get_d(char* str, double* d); 
    5869static char* string_get_f(char* str, float* f); 
    59 static char* string_get_block(char* str, char* fmt, unsigned long* uldat, float* fdat); 
     70static char* string_get_block(char* str, char* fmt, int* idat, float* fdat); 
    6071 
    6172 
    …… 
    6778// udpport (i): UDP port number to receive data from DTrack 
    6879// 
    69 // remote_ip (i): DTrack remote control: ip address of DTrack PC (NULL if not used) 
     80// remote_host (i): DTrack remote control: hostname or IP address of DTrack PC (NULL if not used) 
    7081// remote_port (i): port number of DTrack remote control (0 if not used) 
    7182// 
    7283// udpbufsize (i): size of buffer for UDP packets (in bytes) 
    73 // udptimeout_us (i): UDP timeout (receiving and sending) in us (micro sec) 
     84// udptimeout_us (i): UDP timeout (receiving and sending) in us (micro second) 
    7485 
    7586DTrackStandalone::DTrackStandalone( 
    76         unsigned short udpport, const char* remote_ip, unsigned short remote_port, 
    77         int udpbufsize, unsigned long udptimeout_us 
     87        int udpport, const char* remote_host, int remote_port, 
     88        int udpbufsize, int udptimeout_us 
    7889) 
    7990{ 
    8091        vpr::InetAddr addr; 
    8192 
     93        d_udpsock = NULL; 
    8294        d_udpbuf = NULL; 
    8395        set_noerror(); 
    …… 
    8597        // creat UDP socket: 
    8698 
    87         if(remote_ip != NULL && remote_port != 0){ 
    88                 try 
    89                 { 
    90                         d_remote.setAddress(remote_ip, remote_port); 
    91                         d_use_remote = true; 
    92                 } 
    93                 catch (vpr::UnknownHostException& ex) 
    94                 { 
    95                         vprDEBUG(vprDBG_ALL, vprDBG_CRITICAL_LVL) 
    96                                 << "[DTrackStandalone] invalid remote address " 
    97                                 << remote_ip << ":" << remote_port 
    98                                 << std::endl << vprDEBUG_FLUSH; 
    99                         vprDEBUG_NEXT(vprDBG_ALL, vprDBG_CRITICAL_LVL) 
    100                                 << ex.what() << std::endl; 
    101                         d_udpsock = NULL; 
    102                         return; 
    103                 } 
    104         }else{ 
    105                 d_use_remote = false; 
     99        if(udpport <= 0 || udpport > 65535){ 
     100                return; 
    106101        } 
    107102 
    108103        addr = vpr::InetAddr::AnyAddr; 
    109         addr.setPort(udpport); 
     104        addr.setPort((unsigned short )udpport); 
    110105 
    111106        d_udpsock = new vpr::SocketDatagram(addr, vpr::InetAddr::AnyAddr); 
    …… 
    130125                } 
    131126 
     127                // DTrack remote control parameters: 
     128         
     129                d_use_remote = false; 
     130         
     131                if(remote_host != NULL && remote_port > 0 && remote_port <= 65535){ 
     132                        try 
     133                        { 
     134                                d_remote.setAddress(remote_host, remote_port); 
     135                                d_use_remote = true; 
     136                        } 
     137                        catch (vpr::UnknownHostException& ex) 
     138                        { 
     139                                vprDEBUG(vprDBG_ALL, vprDBG_CRITICAL_LVL) 
     140                                        << "[DTrackStandalone] invalid remote address " 
     141                                        << remote_host << ":" << remote_port 
     142                                        << std::endl << vprDEBUG_FLUSH; 
     143                                vprDEBUG_NEXT(vprDBG_ALL, vprDBG_CRITICAL_LVL) 
     144                                        << ex.what() << std::endl; 
     145                                free(d_udpbuf); 
     146                                d_udpsock->close(); 
     147                                delete d_udpsock; 
     148                                d_udpsock = NULL; 
     149                                return; 
     150                        } 
     151                } 
     152 
     153                d_remote_cameras = false; 
     154                d_remote_tracking = true; 
     155                d_remote_sending = true; 
     156 
    132157                // reset actual DTrack data: 
    133158 
    134                 act_framenr = 0; 
     159                act_framecounter = 0; 
    135160                act_timestamp = -1; 
    136161 
    137                 act_nbodycal = -1; 
    138                 act_nbody = act_nflystick = act_nmeatool = act_nmarker = act_nglove = 0; 
     162                act_num_body = act_num_flystick = act_num_meatool = act_num_hand = 0; 
     163                act_num_marker = 0; 
    139164        } 
    140165        catch (vpr::Exception& ex) 
    …… 
    164189        // release UDP socket: 
    165190 
    166         if(d_udpsock != NULL){ 
     191        if(d_udpsock){ 
    167192                d_udpsock->close(); 
    168193                delete d_udpsock; 
     194                d_udpsock = NULL; 
    169195        } 
    170196} 
    …… 
    187213bool DTrackStandalone::timeout(void)         // 'timeout' 
    188214{ 
    189         return (d_lasterror == DTRACKLIB_ERR_TIMEOUT); 
     215        return (d_lasterror == DTRACK_ERR_TIMEOUT); 
    190216} 
    191217 
    192218bool DTrackStandalone::udperror(void)        // 'udp error' 
    193219{ 
    194         return (d_lasterror == DTRACKLIB_ERR_UDP); 
     220        return (d_lasterror == DTRACK_ERR_UDP); 
    195221} 
    196222 
    197223bool DTrackStandalone::parseerror(void)      // 'parse error' 
    198224{ 
    199         return (d_lasterror == DTRACKLIB_ERR_PARSE); 
     225        return (d_lasterror == DTRACK_ERR_PARSE); 
    200226} 
    201227 
    …… 
    204230void DTrackStandalone::set_noerror(void)     // 'no error' 
    205231{ 
    206         d_lasterror = DTRACKLIB_ERR_NONE; 
     232        d_lasterror = DTRACK_ERR_NONE; 
    207233} 
    208234 
    209235void DTrackStandalone::set_timeout(void)     // 'timeout' 
    210236{ 
    211         d_lasterror = DTRACKLIB_ERR_TIMEOUT; 
     237        d_lasterror = DTRACK_ERR_TIMEOUT; 
    212238} 
    213239 
    214240void DTrackStandalone::set_udperror(void)    // 'udp error' 
    215241{ 
    216         d_lasterror = DTRACKLIB_ERR_UDP; 
     242        d_lasterror = DTRACK_ERR_UDP; 
    217243} 
    218244 
    219245void DTrackStandalone::set_parseerror(void)  // 'parse error' 
    220246{ 
    221         d_lasterror = DTRACKLIB_ERR_PARSE; 
     247        d_lasterror = DTRACK_ERR_PARSE; 
    222248} 
    223249 
    …… 
    234260         
    235261        char* s; 
    236         int i, j; 
    237         unsigned long ul, ularr[2]; 
    238         float farr[6]; 
     262        int i, j, k, l, n, id; 
     263        char sfmt[20]; 
     264        int iarr[3]; 
     265        float f, farr[6]; 
     266        int loc_num_bodycal, loc_num_handcal, loc_num_flystick1, loc_num_meatool; 
    239267 
    240268        if(!valid()){ 
    …… 
    242270        } 
    243271 
    244         // Defaults: 
    245          
    246         act_framenr = 0; 
     272        // defaults: 
     273         
     274        act_framecounter = 0; 
    247275        act_timestamp = -1;   // i.e. not available 
    248         act_nbodycal = -1;    // i.e. not available 
    249         act_nbody = act_nflystick = act_nmeatool = act_nmarker = act_nglove = 0; 
    250          
    251         // Receive UDP packet: 
     276         
     277        loc_num_bodycal = loc_num_handcal = -1;  // i.e. not available 
     278        loc_num_flystick1 = loc_num_meatool = 0; 
     279         
     280        // receive UDP packet: 
    252281 
    253282        do{ 
    …… 
    272301        s[len] = '\0'; 
    273302 
    274         // Process lines: 
     303        // process lines: 
    275304 
    276305        set_parseerror(); 
    277306 
    278307        do{ 
    279                 // Line for frame counter: 
     308                // line for frame counter: 
    280309 
    281310                if(!strncmp(s, "fr ", 3)){ 
    282311                        s += 3; 
    283312                         
    284                         if(!(s = string_get_ul(s, &ul))){               // get frame counter 
     313                        if(!(s = string_get_ui(s, &act_framecounter))){  // get frame counter 
     314                                act_framecounter = 0; 
    285315                                return false; 
    286316                        } 
    287317 
    288                         act_framenr = ul; 
    289318                        continue; 
    290319                } 
    291320 
    292                 // Line for timestamp: 
     321                // line for timestamp: 
    293322 
    294323                if(!strncmp(s, "ts ", 3)){ 
    295324                        s += 3; 
    296325                         
    297                         if(!(s = string_get_d(s, &act_timestamp))){     // get timestamp 
     326                        if(!(s = string_get_d(s, &act_timestamp))){   // get timestamp 
    298327                                act_timestamp = -1; 
    299328                                return false; 
    300329                        } 
     330 
    301331                        continue; 
    302332                } 
    303333                 
    304                 // Line for additional information about number of calibrated bodies: 
     334                // line for additional information about number of calibrated bodies: 
    305335 
    306336                if(!strncmp(s, "6dcal ", 6)){ 
    307337                        s += 6; 
    308338                         
    309                         if(!(s = string_get_ul(s, &ul))){               // get number of bodies 
     339                        if(!(s = string_get_i(s, &loc_num_bodycal))){  // get number of calibrated bodies 
    310340                                return false; 
    311341                        } 
    312342 
    313                         act_nbodycal = (int )ul; 
    314343                        continue; 
    315344                } 
    316345 
    317                 // Line for 6d data: 
     346                // line for standard body data: 
    318347 
    319348                if(!strncmp(s, "6d ", 3)){ 
    320349                        s += 3; 
    321350                         
    322                         if(!(s = string_get_ul(s, &ul))){               // get number of bodies 
     351                        for(i=0; i<act_num_body; i++){  // disable all existing data 
     352                                memset(&act_body[i], 0, sizeof(dtrack_body_type)); 
     353                                act_body[i].id = i; 
     354                                act_body[i].quality = -1; 
     355                        } 
     356 
     357                        if(!(s = string_get_i(s, &n))){               // get number of standard bodies (in line) 
    323358                                return false; 
    324359                        } 
    325360 
    326                         act_nbody = (int )ul; 
    327                         if(act_nbody > (int )act_body.size()){ 
    328                                 act_body.resize(act_nbody); 
    329                         } 
    330  
    331                         for(i=0; i<act_nbody; i++){                     // get data of body 
    332                                 if(!(s = string_get_block(s, "uf", &act_body[i].id, &act_body[i].quality))){ 
    333                                         return false; 
    334                                 } 
    335                          
    336                                 if(!(s = string_get_block(s, "ffffff", NULL, farr))){ 
    337                                         return false; 
    338                                 } 
    339                                 for(j=0; j<3; j++){ 
    340                                         act_body[i].loc[j] = farr[j]; 
    341                                         act_body[i].ang[j] = farr[j+3]; 
    342                                 } 
    343                          
    344                                 if(!(s = string_get_block(s, "fffffffff", NULL, act_body[i].rot))){ 
     361                        for(i=0; i<n; i++){                           // get data of standard bodies 
     362                                if(!(s = string_get_block(s, "if", &id, &f))){ 
     363                                        return false; 
     364                                } 
     365 
     366                                if(id >= act_num_body){  // adjust length of vector 
     367                                        act_body.resize(id + 1); 
     368 
     369                                        for(j=act_num_body; j<=id; j++){ 
     370                                                memset(&act_body[j], 0, sizeof(dtrack_body_type)); 
     371                                                act_body[j].id = j; 
     372                                                act_body[j].quality = -1; 
     373                                        } 
     374 
     375                                        act_num_body = id + 1; 
     376                                } 
     377                                 
     378                                act_body[id].id = id; 
     379                                act_body[id].quality = f; 
     380                                 
     381                                if(!(s = string_get_block(s, "fff", NULL, act_body[id].loc))){ 
     382                                        return false; 
     383                                } 
     384                         
     385                                if(!(s = string_get_block(s, "fffffffff", NULL, act_body[id].rot))){ 
    345386                                        return false; 
    346387                                } 
    …… 
    350391                } 
    351392                 
    352                 // Line for flystick data: 
     393                // line for Flystick data (older format): 
    353394 
    354395                if(!strncmp(s, "6df ", 4)){ 
    355396                        s += 4; 
    356397                         
    357                         if(!(s = string_get_ul(s, &ul))){               // get number of flysticks 
     398                        if(!(s = string_get_i(s, &n))){               // get number of calibrated Flysticks 
    358399                                return false; 
    359400                        } 
    360401 
    361                         act_nflystick = (int )ul; 
    362                         if(act_nflystick > (int )act_flystick.size()){ 
    363                                 act_flystick.resize(act_nflystick); 
    364                         } 
    365  
    366                         for(i=0; i<act_nflystick; i++){                 // get data of body 
    367                                 if(!(s = string_get_block(s, "ufu", ularr, &act_flystick[i].quality))){ 
     402                        loc_num_flystick1 = n; 
     403                         
     404                        if(n != act_num_flystick){  // adjust length of vector 
     405                                act_flystick.resize(n); 
     406                                 
     407                                act_num_flystick = n; 
     408                        } 
     409                         
     410                        for(i=0; i<n; i++){                           // get data of Flysticks 
     411                                if(!(s = string_get_block(s, "ifi", iarr, &f))){ 
    368412                                        return false; 
    369413                                } 
    370414                                         
    371                                 act_flystick[i].id = ularr[0]; 
    372                                 act_flystick[i].bt = ularr[1]; 
    373                                  
    374                                 if(!(s = string_get_block(s, "ffffff", NULL, farr))){ 
    375                                         return false; 
    376                                 } 
    377                                 for(j=0; j<3; j++){ 
    378                                         act_flystick[i].loc[j] = farr[j]; 
    379                                         act_flystick[i].ang[j] = farr[j+3]; 
     415                                if(iarr[0] != i){  // not expected 
     416                                        return false; 
     417                                } 
     418 
     419                                act_flystick[i].id = iarr[0]; 
     420                                act_flystick[i].quality = f; 
     421 
     422                                act_flystick[i].num_button = 8; 
     423                                k = iarr[1]; 
     424                                for(j=0; j<8; j++){ 
     425                                        act_flystick[i].button[j] = k & 0x01; 
     426                                        k >>= 1; 
     427                                } 
     428                                 
     429                                act_flystick[i].num_joystick = 2;  // additionally to buttons 5-8 
     430                                if(iarr[1] & 0x20){ 
     431                                        act_flystick[i].joystick[0] = -1; 
     432                                }else if(iarr[1] & 0x80){ 
     433                                        act_flystick[i].joystick[0] = 1; 
     434                                }else{ 
     435                                        act_flystick[i].joystick[0] = 0; 
     436                                } 
     437                                if(iarr[1] & 0x10){ 
     438                                        act_flystick[i].joystick[1] = -1; 
     439                                }else if(iarr[1] & 0x40){ 
     440                                        act_flystick[i].joystick[1] = 1; 
     441                                }else{ 
     442                                        act_flystick[i].joystick[1] = 0; 
     443                                } 
     444                                 
     445                                if(!(s = string_get_block(s, "fff", NULL, act_flystick[i].loc))){ 
     446                                        return false; 
    380447                                } 
    381448                                 
    …… 
    388455                } 
    389456                 
    390