Changeset 20354

Show
Ignore:
Timestamp:
06/27/07 06:39:59 (1 year ago)
Author:
patrick
Message:

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/branches/2.0/modules/gadgeteer/drivers/ART/DTrack/DTrack.cpp

    r18152 r20354  
    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 
    …… 
    4040// some constants: 
    4141 
    42 #define VERSION_STRING   "DTrack Driver v0.1.0; (C) 2005, Advanced Realtime Tracking GmbH" 
     42#define VERSION_STRING   "DTrack Driver v0.2.0; (C) 2005-2007, Advanced Realtime Tracking GmbH" 
    4343 
    4444#define BUTTONS_PER_FLYSTICK    8  // number of buttons per 'Flystick' (fixed) 
    4545#define VALUATORS_PER_FLYSTICK  2  // number of valuators per 'Flystick' (fixed) 
     46#define BUTTONS_PER_MEATOOL     2  // number of buttons per 'Measurement Tool' (fixed) 
    4647 
    4748 
    …… 
    9596{ 
    9697 
    97         return "dtrack"; 
     98        return std::string("dtrack"); 
    9899} 
    99100 
    …… 
    163164 
    164165        if(use_commands){ 
    165                 standalone->send(DTRACKLIB_CMD_CAMERAS_AND_CALC_ON); 
    166                 standalone->send(DTRACKLIB_CMD_SEND_DATA); 
     166                standalone->cmd_cameras(true); 
    167167        } 
    168168 
    …… 
    209209 
    210210        if(use_commands){ 
    211                 standalone->send(DTRACKLIB_CMD_STOP_DATA); 
    212                 standalone->send(DTRACKLIB_CMD_CAMERAS_OFF); 
     211                standalone->cmd_cameras(false); 
    213212        } 
    214213 
    …… 
    245244{ 
    246245        bool stat; 
    247         unsigned long i; 
    248         unsigned long nbody, nflystick; 
     246        int i, j, id; 
     247        int nbt, nvt; 
     248        int num_body, num_flystick, num_meatool; 
    249249 
    250250        if(!thrRunning){ 
    …… 
    258258        } 
    259259 
    260         nbody = (unsigned long )standalone->get_nbody(); 
    261         nflystick = (unsigned long )standalone->get_nflystick(); 
    262  
    263         if(standalone->get_nbodycal() > -1){        // number of calibrated bodies available 
    264                 unsigned long nbodycal = (unsigned long )standalone->get_nbodycal(); 
    265                 nbodycal -= standalone->get_nmeatool();  // ignore measurement tools 
     260        num_body = standalone->get_num_body(); 
     261        num_flystick = standalone->get_num_flystick(); 
     262        num_meatool = standalone->get_num_meatool(); 
    266263         
    267                 resize_curPosition(nbodycal); 
    268         } 
     264        resize_curPosition(num_flystick + num_meatool + num_body); 
     265        resize_curDigital(num_flystick * BUTTONS_PER_FLYSTICK + num_meatool * BUTTONS_PER_MEATOOL); 
     266        resize_curAnalog(num_flystick * VALUATORS_PER_FLYSTICK); 
    269267 
    270268        // get 'Flystick' data: 
    271269 
    272         resize_curPosition(nflystick); 
    273  
    274         if(nflystick * BUTTONS_PER_FLYSTICK > curDigital.size()){ 
    275                 curDigital.resize(nflystick * BUTTONS_PER_FLYSTICK);   // all elements will be set later 
    276         } 
    277  
    278         if(nflystick * VALUATORS_PER_FLYSTICK > curAnalog.size()){ 
    279                 curAnalog.resize(nflystick * VALUATORS_PER_FLYSTICK);  // all elements will be set later 
    280         } 
    281  
    282         for(i=0; i<nflystick; i++){ 
    283                 dtracklib_flystick_type dat = standalone->get_flystick(i); 
     270        for(i=0; i<num_flystick; i++){ 
     271                dtrack_flystick_type dat = standalone->get_flystick(i); 
    284272                 
    285273                if(dat.quality >= 0){  // check if Flystick position is tracked 
    …… 
    290278                // Flystick buttons: 
    291279 
    292                 unsigned long bt = dat.bt; 
    293                 for(int j=0; j<BUTTONS_PER_FLYSTICK; j++){ 
    294                         curDigital[i*BUTTONS_PER_FLYSTICK + j] = bt & 0x0001; 
    295                         curDigital[i*BUTTONS_PER_FLYSTICK + j].setTime(); 
     280                nbt = dat.num_button; 
     281                 
     282                if(nbt > BUTTONS_PER_FLYSTICK){ 
     283                        nbt = BUTTONS_PER_FLYSTICK; 
     284                } 
     285                 
     286                for(j=0; j<nbt; j++){ 
     287                        id = j + i * BUTTONS_PER_FLYSTICK;  // VRJuggler id number 
    296288                         
    297                         bt >>= 1; 
    298                 } 
    299                  
    300                 // Flystick 'HAT switch' (valuators simulated using four buttons): 
    301                  
    302                 for(int j=0; j<VALUATORS_PER_FLYSTICK; j++){ 
    303                         curAnalog[i*VALUATORS_PER_FLYSTICK + j] = 0; 
    304                         curAnalog[i*VALUATORS_PER_FLYSTICK + j].setTime(); 
    305                 } 
    306                  
    307                 float f, fn; 
    308  
    309                 f = 0; 
    310                 if(dat.bt & 0x0020){   
    311                         f = -1; 
    312                 }else if(dat.bt & 0x0080){ 
    313                         f = 1; 
    314                 } 
    315  
    316                 if(f != 0){ 
    317                         this->normalizeMinToMax(f, fn); 
    318                         curAnalog[i*VALUATORS_PER_FLYSTICK + 0] = fn; 
    319                 } 
    320  
    321                 f = 0; 
    322                 if(dat.bt & 0x0010){   
    323                         f = -1; 
    324                 }else if(dat.bt & 0x0040){ 
    325                         f = 1; 
    326                 } 
    327  
    328                 if(f != 0){ 
    329                         this->normalizeMinToMax(f, fn); 
    330                         curAnalog[i*VALUATORS_PER_FLYSTICK + 1] = fn; 
     289                        curDigital[id] = dat.button[j]; 
     290                        curDigital[id].setTime(); 
     291                } 
     292                 
     293                // Flystick valuators ('HAT switch' or 'joystick'): 
     294                 
     295                nvt = dat.num_joystick; 
     296                 
     297                if(nvt > VALUATORS_PER_FLYSTICK){ 
     298                        nvt = VALUATORS_PER_FLYSTICK; 
     299                } 
     300                 
     301                for(j=0; j<nvt; j++){ 
     302                        id = j + i * VALUATORS_PER_FLYSTICK;  // VRJuggler id number 
     303                         
     304                        curAnalog[id] = dat.joystick[j] / 2.0 + 0.5;  // normalizing 
     305                        curAnalog[id].setTime(); 
     306                } 
     307        } 
     308 
     309        // get 'measurement tool' data: 
     310 
     311        for(i=0; i<num_meatool; i++){ 
     312                dtrack_meatool_type dat = standalone->get_meatool(i); 
     313                 
     314                if(dat.quality >= 0){  // check if position is tracked 
     315                        id = i + num_flystick;  // VRJuggler id number 
     316                         
     317                        curPosition[id].mPosData = getpos(dat); 
     318                        curPosition[id].setTime(); 
     319                }                      // otherwise keep last valid position 
     320 
     321                // measurement tool buttons: 
     322 
     323                nbt = dat.num_button; 
     324                 
     325                if(nbt > BUTTONS_PER_MEATOOL){ 
     326                        nbt = BUTTONS_PER_MEATOOL; 
     327                } 
     328                 
     329                for(j=0; j<nbt; j++){ 
     330                        id = j + i * BUTTONS_PER_MEATOOL + num_flystick * BUTTONS_PER_FLYSTICK;  // VRJuggler id number 
     331                         
     332                        curDigital[id] = dat.button[j]; 
     333                        curDigital[id].setTime(); 
    331334                } 
    332335        } 
    …… 
    334337        // get 'standard body' data: 
    335338 
    336         for(i=0; i<nbody; i++){ 
    337                 dtracklib_body_type dat = standalone->get_body(i); 
    338  
    339                 unsigned long id = dat.id + nflystick;  // VRJuggler id number of this 'Standard Body' 
    340                 resize_curPosition(id+1); 
    341  
    342                 curPosition[id].mPosData = getpos(dat); 
    343                 curPosition[id].setTime(); 
    344         }  // bodies not in this list are not tracked: keep their last valid position 
     339        for(i=0; i<num_body; i++){ 
     340                dtrack_body_type dat = standalone->get_body(i); 
     341 
     342                if(dat.quality >= 0){  // check if position is tracked 
     343                        id = i + num_flystick + num_meatool;  // VRJuggler id number 
     344 
     345                        curPosition[id].mPosData = getpos(dat); 
     346                        curPosition[id].setTime(); 
     347                }                      // otherwise keep last valid position 
     348        } 
    345349 
    346350        // update buffers: 
    …… 
    372376// n (i): new size 
    373377 
    374 void DTrack::resize_curPosition(unsigned long n) 
    375 { 
    376         unsigned long nsize = curPosition.size(); 
     378void DTrack::resize_curPosition(int n) 
     379{ 
     380        int nsize = (int )curPosition.size(); 
    377381 
    378382        if(n > nsize){ 
    379383                curPosition.resize(n); 
    380384 
    381                 for(unsigned long i=nsize; i<n; i++){  // set default for new elements 
     385                for(int i=nsize; i<n; i++){  // set default for new elements 
    382386                        curPosition[i].mPosData = getpos_default(); 
    383387                        curPosition[i].setTime(); 
    …… 
    386390} 
    387391 
     392// Resize curDigital vector: 
     393// n (i): new size 
     394 
     395void DTrack::resize_curDigital(int n) 
     396{ 
     397        int nsize = (int )curDigital.size(); 
     398 
     399        if(n > nsize){ 
     400                curDigital.resize(n); 
     401 
     402                for(int i=nsize; i<n; i++){  // set default for new elements 
     403                        curDigital[i] = 0; 
     404                        curDigital[i].setTime(); 
     405                } 
     406        } 
     407} 
     408 
     409// Resize curAnalog vector: 
     410// n (i): new size 
     411 
     412void DTrack::resize_curAnalog(int n) 
     413{ 
     414        int nsize = (int )curAnalog.size(); 
     415        float fn; 
     416 
     417        if(n > nsize){ 
     418                curAnalog.resize(n); 
     419 
     420                for(int i=nsize; i<n; i++){  // set default for new elements 
     421                        this->normalizeMinToMax(0, fn); 
     422                        curAnalog[i] = fn; 
     423                        curAnalog[i].setTime(); 
     424                } 
     425        } 
     426} 
     427 
    388428 
    389429// Transfer DTracklib pose into gmtl pose: 
    …… 
    392432// return value (o): gmtl pose 
    393433 
    394 gmtl::Matrix44f DTrack::getpos(dtracklib_body_type& bod) 
     434gmtl::Matrix44f DTrack::getpos(dtrack_body_type& bod) 
    395435{ 
    396436        gmtl::Matrix44f ret_val; 
    …… 
    410450} 
    411451 
    412 gmtl::Matrix44f DTrack::getpos(dtracklib_flystick_type& bod) 
     452gmtl::Matrix44f DTrack::getpos(dtrack_flystick_type& bod) 
    413453{ 
    414454        gmtl::Matrix44f ret_val; 
    …… 
    428468} 
    429469 
     470gmtl::Matrix44f DTrack::getpos(dtrack_meatool_type& bod) 
     471{ 
     472        gmtl::Matrix44f ret_val; 
     473 
     474        for(int i=0; i<3; i++){ 
     475                for(int j=0; j<3; j++){ 
     476                        ret_val[i][j] = bod.rot[i + 3*j]; 
     477                } 
     478 
     479                ret_val[3][i] = 0; 
     480                ret_val[i][3] = bod.loc[i] / 1000;  // convert to meters 
     481        } 
     482 
     483        ret_val[3][3] = 1; 
     484 
     485        return ret_val; 
     486} 
     487 
    430488gmtl::Matrix44f DTrack::getpos_default(void) 
    431489{ 

  • juggler/branches/2.0/modules/gadgeteer/drivers/ART/DTrack/DTrack.h

    r20306 r20354  
    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 
    …… 
    9696 
    9797        DTrackStandalone* standalone; 
    98         unsigned short receive_port; 
     98        int receive_port; 
    9999        bool use_commands; 
    100100        std::string server_name; 
    101         unsigned short command_port; 
     101        int command_port; 
    102102 
    103103        std::vector<PositionData> curPosition; 
    …… 
    105105        std::vector<AnalogData> curAnalog; 
    106106         
    107         void resize_curPosition(unsigned long n); 
     107        void resize_curPosition(int n); 
     108        void resize_curDigital(int n); 
     109        void resize_curAnalog(int n); 
    108110 
    109         gmtl::Matrix44f getpos(dtracklib_body_type& bod); 
    110         gmtl::Matrix44f getpos(dtracklib_flystick_type& bod); 
     111        gmtl::Matrix44f getpos(dtrack_body_type& bod); 
     112        gmtl::Matrix44f getpos(dtrack_flystick_type& bod); 
     113        gmtl::Matrix44f getpos(dtrack_meatool_type& bod); 
    111114        gmtl::Matrix44f getpos_default(void); 
    112115}; 

  • juggler/branches/2.0/modules/gadgeteer/drivers/ART/DTrack/DTrackStandalone.cpp

    r18789 r20354  
    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 
    …… 
    4444// Local error codes: 
    4545 
    46 #define DTRACKLIB_ERR_NONE       0 
    47 #define DTRACKLIB_ERR_TIMEOUT    1  // timeout while receiving data 
    48 #define DTRACKLIB_ERR_UDP        2  // UDP receive error 
    49 #define DTRACKLIB_ERR_PARSE      3  // error in UDP packet 
     46#define DTRACK_ERR_NONE       0  // no error 
     47#define DTRACK_ERR_TIMEOUT    1  // timeout while receiving data 
     48#define DTRACK_ERR_UDP        2  // UDP receive error 
     49#define DTRACK_ERR_PARSE      3  // error in UDP packet 
     50 
     51// DTrack remote commands: 
     52 
     53#define DTRACK_CMD_CAMERAS_OFF           1 
     54#define DTRACK_CMD_CAMERAS_ON            2 
     55#define DTRACK_CMD_CAMERAS_AND_CALC_ON   3 
     56 
     57#define DTRACK_CMD_SEND_DATA            11 
     58#define DTRACK_CMD_STOP_DATA            12 
     59#define DTRACK_CMD_SEND_N_DATA          13 
    5060 
    5161 
    5262// Local prototypes: 
    5363 
    54 static char* string_nextline(char* str, char* start, unsigned long len); 
    55 static char* string_get_ul(char* str, unsigned long* ul); 
     64static char* string_nextline(char* str, char* start, int len); 
     65static char* string_get_i(char* str, int* i); 
     66static char* string_get_ui(char* str, unsigned int* ui); 
    5667static char* string_get_d(char* str, double* d); 
    5768static char* string_get_f(char* str, float* f); 
    58 static char* string_get_block(char* str, char* fmt, unsigned long* uldat, float* fdat); 
     69static char* string_get_block(char* str, char* fmt, int* idat, float* fdat); 
    5970 
    6071 
    …… 
    6677// udpport (i): UDP port number to receive data from DTrack 
    6778// 
    68 // remote_ip (i): DTrack remote control: ip address of DTrack PC (NULL if not used) 
     79// remote_host (i): DTrack remote control: hostname or IP address of DTrack PC (NULL if not used) 
    6980// remote_port (i): port number of DTrack remote control (0 if not used) 
    7081// 
    7182// udpbufsize (i): size of buffer for UDP packets (in bytes) 
    72 // udptimeout_us (i): UDP timeout (receiving and sending) in us (micro sec) 
     83// udptimeout_us (i): UDP timeout (receiving and sending) in us (micro second) 
    7384 
    7485DTrackStandalone::DTrackStandalone( 
    75         unsigned short udpport, const char* remote_ip, unsigned short remote_port, 
    76         int udpbufsize, unsigned long udptimeout_us 
     86        int udpport, const char* remote_host, int remote_port, 
     87        int udpbufsize, int udptimeout_us 
    7788) 
    7889{ 
    …… 
    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                 stat = d_remote.setAddress(remote_ip, remote_port); 
    89                  
    90                 if(!stat.success()){ 
    91                         vprDEBUG(vprDBG_ALL, vprDBG_CRITICAL_LVL) 
    92                                 << "[DTrackStandalone] invalid remote address " << remote_ip << ":" << remote_port 
    93                                 << std::endl << vprDEBUG_FLUSH; 
    94                                          
    95                         d_udpsock = NULL; 
    96                         return; 
    97                 } 
    98                  
    99                 d_use_remote = true; 
    100         }else{ 
    101                 d_use_remote = false; 
     99        if(udpport <= 0 || udpport > 65535){ 
     100                return; 
    102101        } 
    103102 
    104103        addr = vpr::InetAddr::AnyAddr; 
    105         addr.setPort(udpport); 
     104        addr.setPort((unsigned short )udpport); 
    106105 
    107106        d_udpsock = new vpr::SocketDatagram(addr, vpr::InetAddr::AnyAddr); 
    …… 
    141140        } 
    142141 
     142        // DTrack remote control parameters: 
     143         
     144        d_use_remote = false; 
     145         
     146        if(remote_host != NULL && remote_port > 0 && remote_port <= 65535){ 
     147                stat = d_remote.setAddress(remote_host, remote_port); 
     148         
     149                if(!stat.success()){ 
     150                        vprDEBUG(vprDBG_ALL, vprDBG_CRITICAL_LVL) 
     151                                << "[DTrackStandalone] invalid remote address " << remote_host << ":" << remote_port 
     152                                << std::endl << vprDEBUG_FLUSH; 
     153                         
     154                        free(d_udpbuf); 
     155                        d_udpsock->close(); 
     156                        delete d_udpsock; 
     157                        d_udpsock = NULL; 
     158                        return; 
     159                } 
     160 
     161                d_use_remote = true; 
     162        } 
     163 
     164        d_remote_cameras = false; 
     165        d_remote_tracking = true; 
     166        d_remote_sending = true; 
     167 
    143168        // reset actual DTrack data: 
    144169 
    145         act_framenr = 0; 
     170        act_framecounter = 0; 
    146171        act_timestamp = -1; 
    147172 
    148         act_nbodycal = -1; 
    149         act_nbody = act_nflystick = act_nmeatool = act_nmarker = act_nglove = 0; 
     173        act_num_body = act_num_flystick = act_num_meatool = act_num_hand = 0; 
     174        act_num_marker = 0; 
    150175} 
    151176 
    …… 
    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 
    …… 
    235261         
    236262        char* s; 
    237         int i, j; 
    238         unsigned long ul, ularr[2]; 
    239         float farr[6]; 
     263        int i, j, k, l, n, id; 
     264        char sfmt[20]; 
     265        int iarr[3]; 
     266        float f, farr[6]; 
     267        int loc_num_bodycal, loc_num_handcal, loc_num_flystick1, loc_num_meatool; 
    240268 
    241269        if(!valid()){ 
    …… 
    243271        } 
    244272 
    245         // Defaults: 
    246          
    247         act_framenr = 0; 
     273        // defaults: 
     274         
     275        act_framecounter = 0; 
    248276        act_timestamp = -1;   // i.e. not available 
    249         act_nbodycal = -1;    // i.e. not available 
    250         act_nbody = act_nflystick = act_nmeatool = act_nmarker = act_nglove = 0; 
    251          
    252         // Receive UDP packet: 
     277         
     278        loc_num_bodycal = loc_num_handcal = -1;  // i.e. not available 
     279        loc_num_flystick1 = loc_num_meatool = 0; 
     280         
     281        // receive UDP packet: 
    253282 
    254283        do{ 
    …… 
    269298        s[len] = '\0'; 
    270299 
    271         // Process lines: 
     300        // process lines: 
    272301 
    273302        set_parseerror(); 
    274303 
    275304        do{ 
    276                 // Line for frame counter: 
     305                // line for frame counter: 
    277306 
    278307                if(!strncmp(s, "fr ", 3)){ 
    279308                        s += 3; 
    280309                         
    281                         if(!(s = string_get_ul(s, &ul))){               // get frame counter 
     310                        if(!(s = string_get_ui(s, &act_framecounter))){  // get frame counter 
     311                                act_framecounter = 0; 
    282312                                return false; 
    283313                        } 
    284314 
    285                         act_framenr = ul; 
    286315                        continue; 
    287316                } 
    288317 
    289                 // Line for timestamp: 
     318                // line for timestamp: 
    290319 
    291320                if(!strncmp(s, "ts ", 3)){ 
    292321                        s += 3; 
    293322                         
    294                         if(!(s = string_get_d(s, &act_timestamp))){     // get timestamp 
     323                        if(!(s = string_get_d(s, &act_timestamp))){   // get timestamp 
    295324                                act_timestamp = -1; 
    296325                                return false; 
    297326                        } 
     327 
    298328                        continue; 
    299329                } 
    300330                 
    301                 // Line for additional information about number of calibrated bodies: 
     331                // line for additional information about number of calibrated bodies: 
    302332 
    303333                if(!strncmp(s, "6dcal ", 6)){ 
    304334                        s += 6; 
    305335                         
    306                         if(!(s = string_get_ul(s, &ul))){               // get number of bodies 
     336                        if(!(s = string_get_i(s, &loc_num_bodycal))){  // get number of calibrated bodies 
    307337                                return false; 
    308338                        } 
    309339 
    310                         act_nbodycal = (int )ul; 
    311340                        continue; 
    312341                } 
    313342 
    314                 // Line for 6d data: 
     343                // line for standard body data: 
    315344 
    316345                if(!strncmp(s, "6d ", 3)){ 
    317346                        s += 3; 
    318347                         
    319                         if(!(s = string_get_ul(s, &ul))){               // get number of bodies 
     348                        for(i=0; i<act_num_body; i++){  // disable all existing data 
     349                                memset(&act_body[i], 0, sizeof(dtrack_body_type)); 
     350                                act_body[i].id = i; 
     351                                act_body[i].quality = -1; 
     352                        } 
     353 
     354                        if(!(s = string_get_i(s, &n))){               // get number of standard bodies (in line) 
    320355                                return false; 
    321356                        } 
    322357 
    323                         act_nbody = (int )ul; 
    324                         if(act_nbody > (int )act_body.size()){ 
    325                                 act_body.resize(act_nbody); 
    326                         } 
    327  
    328                         for(i=0; i<act_nbody; i++){                     // get data of body 
    329                                 if(!(s = string_get_block(s, "uf", &act_body[i].id, &act_body[i].quality))){ 
    330                                         return false; 
    331                                 } 
    332                          
    333                                 if(!(s = string_get_block(s, "ffffff", NULL, farr))){ 
    334                                         return false; 
    335                                 } 
    336                                 for(j=0; j<3; j++){ 
    337                                         act_body[i].loc[j] = farr[j]; 
    338                                         act_body[i].ang[j] = farr[j+3]; 
    339                                 } 
    340                          
    341                                 if(!(s = string_get_block(s, "fffffffff", NULL, act_body[i].rot))){ 
     358                        for(i=0; i<n; i++){                           // get data of standard bodies 
     359                                if(!(s = string_get_block(s, "if", &id, &f))){ 
     360                                        return false; 
     361                                } 
     362 
     363                                if(id >= act_num_body){  // adjust length of vector 
     364                                        act_body.resize(id + 1); 
     365 
     366                                        for(j=act_num_body; j<=id; j++){ 
     367                                                memset(&act_body[j], 0, sizeof(dtrack_body_type)); 
     368                                                act_body[j].id = j; 
     369                                                act_body[j].quality = -1; 
     370                                        } 
     371 
     372                                        act_num_body = id + 1; 
     373                                } 
     374                                 
     375                                act_body[id].id = id; 
     376                                act_body[id].quality = f; 
     377                                 
     378                                if(!(s = string_get_block(s, "fff", NULL, act_body[id].loc))){ 
     379                                        return false; 
     380                                } 
     381                         
     382                                if(!(s = string_get_block(s, "fffffffff", NULL, act_body[id].rot))){ 
    342383                                        return false; 
    343384                                } 
    …… 
    347388                } 
    348389                 
    349                 // Line for flystick data: 
     390                // line for Flystick data (older format): 
    350391 
    351392                if(!strncmp(s, "6df ", 4)){ 
    352393                        s += 4; 
    353394                         
    354                         if(!(s = string_get_ul(s, &ul))){               // get number of flysticks 
     395                        if(!(s = string_get_i(s, &n))){               // get number of calibrated Flysticks 
    355396                                return false; 
    356397                        } 
    357398 
    358                         act_nflystick = (int )ul; 
    359                         if(act_nflystick > (int )act_flystick.size()){ 
    360                                 act_flystick.resize(act_nflystick); 
    361                         } 
    362  
    363                         for(i=0; i<act_nflystick; i++){                 // get data of body 
    364                                 if(!(s = string_get_block(s, "ufu", ularr, &act_flystick[i].quality))){ 
     399                        loc_num_flystick1 = n; 
     400                         
     401                        if(n != act_num_flystick){  // adjust length of vector 
     402                                act_flystick.resize(n); 
     403                                 
     404                                act_num_flystick = n; 
     405                        } 
     406                         
     407                        for(i=0; i<n; i++){                           // get data of Flysticks 
     408                                if(!(s = string_get_block(s, "ifi", iarr, &f))){ 
    365409                                        return false; 
    366410                                } 
    367411                                         
    368                                 act_flystick[i].id = ularr[0]; 
    369                                 act_flystick[i].bt = ularr[1]; 
    370                                  
    371                                 if(!(s = string_get_block(s, "ffffff", NULL, farr))){ 
    372                                         return false; 
    373                                 } 
    374                                 for(j=0; j<3; j++){ 
    375                                         act_flystick[i].loc[j] = farr[j]; 
    376                                         act_flystick[i].ang[j] = farr[j+3]; 
     412                                if(iarr[0] != i){  // not expected 
     413                                        return false; 
     414                                } 
     415 
     416                                act_flystick[i].id = iarr[0]; 
     417                                act_flystick[i].quality = f; 
     418 
     419                                act_flystick[i].num_button = 8; 
     420                                k = iarr[1]; 
     421                                for(j=0; j<8; j++){ 
     422                                        act_flystick[i].button[j] = k & 0x01; 
     423                                        k >>= 1; 
     424                                } 
     425                                 
     426                                act_flystick[i].num_joystick = 2;  // additionally to buttons 5-8 
     427                                if(iarr[1] & 0x20){ 
     428                                        act_flystick[i].joystick[0] = -1; 
     429                                }else if(iarr[1] & 0x80){ 
     430                                        act_flystick[i].joystick[0] = 1; 
     431                                }else{ 
     432                                        act_flystick[i].joystick[0] = 0; 
     433                                } 
     434                                if(iarr[1] & 0x10){ 
     435                                        act_flystick[i].joystick[1] = -1; 
     436                                }else if(iarr[1] & 0x40){ 
     437                                        act_flystick[i].joystick[1] = 1; 
     438                                }else{ 
     439                                        act_flystick[i].joystick[1] = 0; 
     440                                } 
     441                                 
     442                                if(!(s = string_get_block(s, "fff", NULL, act_flystick[i].loc))){ 
     443                                        return false; 
    377444                                } 
    378445                                 
    …… 
    385452                } 
    386453                 
    387                 // Line for measurement tool data: 
     454                // line for Flystick data (newer format): 
     455 
     456                if(!strncmp(s, "6df2 ", 5)){ 
     457                        s += 5; 
     458                         
     459                        if(!(s = string_get_i(s, &n))){               // get number of calibrated Flysticks 
     460                                return false; 
     461                        } 
     462 
     463                        if(n != act_num_flystick){  // adjust length of vector 
     464                                act_flystick.resize(n); 
     465                                 
     466                                act_num_flystick = n; 
     467                        } 
     468              &