vcs.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/Examples/Linux/Navigation/Sources/UI/gamepad.c

   1 /**
   2  *  \brief    gamepad handling implementation
   3  *  \author   Sylvain Gaeremynck <sylvain.gaeremynck@parrot.fr>
   4  *  \version  1.0
   5  *  \date     04/06/2007
   6  *  \warning  Subject to completion
   7  */
   8
   9 #include <errno.h>
  10 #include <fcntl.h>
  11 #include <string.h>
  12 #include <stddef.h>
  13 #include <stdio.h>
  14 #include <unistd.h>
  15
  16 #include <linux/joystick.h>
  17
  18 #include <ardrone_api.h>
  19 #include <VP_Os/vp_os_print.h>
  20 #include "gamepad.h"
  21
  22 typedef struct {
  23   int32_t bus;
  24   int32_t vendor;
  25   int32_t product;
  26   int32_t version;
  27   char    name[MAX_NAME_LENGTH];
  28   char    handlers[MAX_NAME_LENGTH];
  29 } device_t;
  30
  31 extern int32_t MiscVar[];
  32
  33 static C_RESULT add_device(device_t* device, const int32_t id);
  34
  35 static C_RESULT parse_proc_input_devices(FILE* f, const int32_t id);
  36
  37 input_device_t gamepad = {
  38   "Gamepad",
  39   open_gamepad,
  40   update_gamepad,
  41   close_gamepad
  42 };
  43
  44 static int32_t joy_dev = 0;
  45
  46 input_device_t radioGP = {
  47   "GreatPlanes",
  48   open_radioGP,
  49   update_radioGP,
  50   close_radioGP
  51 };
  52
  53
  54 input_device_t ps3pad = {
  55   "PS3Gamepad",
  56   open_ps3pad,
  57   update_ps3pad,
  58   close_ps3pad
  59 };
  60
  61 input_device_t joystick = {
  62         "Joystick",
  63         open_joystick,
  64         update_joystick,
  65         close_joystick
  66 };
  67
  68 int current_joystick = JOYSTICK_DEVICE_MAX;
  69
  70 int32_t joystick_device_ids[JOYSTICK_DEVICE_MAX] = {
  71         [JOYSTICK_DEVICE_SIDEWINDER]    = 0x045e001b,
  72         [JOYSTICK_DEVICE_CYBORG]                = 0x06a30836,
  73 };
  74
  75 int32_t joystick_table_buttons[JOYSTICK_DEVICE_MAX][JOYBTN_NUM] = {
  76         [JOYSTICK_DEVICE_SIDEWINDER] = {
  77                 [JOYBTN_FIRE] = 0,
  78                 [JOYBTN_SELECT] = 2,
  79                 [JOYBTN_START]  = 3,
  80         },
  81         [JOYSTICK_DEVICE_CYBORG]         = {
  82                 [JOYBTN_FIRE] = 0,
  83                 [JOYBTN_SELECT] = 1,
  84                 [JOYBTN_START]  = 2,
  85         },
  86 };
  87
  88 int32_t joystick_table_axis[JOYSTICK_DEVICE_MAX][JOYAXIS_NUM] = {
  89         [JOYSTICK_DEVICE_SIDEWINDER] = {
  90                 [JOYAXIS_ROLL]  = 0,
  91                 [JOYAXIS_PITCH] = 1,
  92                 [JOYAXIS_YAW]   = 2,
  93                 [JOYAXIS_GAZ]   = 3,
  94         },
  95         [JOYSTICK_DEVICE_CYBORG]         = {
  96                 [JOYAXIS_ROLL]  = 0,
  97                 [JOYAXIS_PITCH] = 1,
  98                 [JOYAXIS_YAW]   = 3,
  99                 [JOYAXIS_GAZ]   = 6,
 100         },
 101 };
 102 ///////////////////////////////
 103 //  RadioGP input functions  //
 104 ///////////////////////////////
 105 C_RESULT open_radioGP(void)
 106 {
 107   C_RESULT res = C_FAIL;
 108
 109   FILE* f = fopen("/proc/bus/input/devices", "r");
 110
 111   if( f != NULL )
 112   {
 113     res = parse_proc_input_devices( f,  RADIO_GP_ID);
 114
 115     fclose( f );
 116
 117     if( SUCCEED( res ) && strcmp(radioGP.name, "GreatPlanes")!=0)
 118     {
 119                         char dev_path[20]="/dev/input/";
 120                         strcat(dev_path, radioGP.name);
 121       joy_dev = open(dev_path, O_NONBLOCK | O_RDONLY);
 122
 123                         return res;
 124     }
 125                 else
 126                 {
 127                         return C_FAIL;
 128                 }
 129   }
 130
 131   return res;
 132 }
 133
 134 C_RESULT update_radioGP(void)
 135 {
 136   static float32_t roll = 0, pitch = 0, gaz=0, yaw=0;
 137   static bool_t refresh_values = FALSE;
 138   ssize_t res;
 139   static struct js_event js_e_buffer[64];
 140
 141   res = read(joy_dev, js_e_buffer, sizeof(struct js_event) * 64);
 142
 143   if( !res || (res < 0 && errno == EAGAIN) )
 144     return C_OK;
 145
 146   if( res < 0 )
 147     return C_FAIL;
 148
 149   if (res < (int) sizeof(struct js_event))// If non-complete bloc: ignored
 150     return C_OK;
 151
 152   // Buffer d�composition in blocs (if the last is incomplete, it's ignored)
 153   int32_t idx = 0;
 154   refresh_values = FALSE;
 155   for (idx = 0; idx < res / sizeof(struct js_event); idx++)
 156   {
 157     if(js_e_buffer[idx].type & JS_EVENT_INIT )// If Init, the first values are ignored
 158     {
 159       break;
 160     }
 161     else if(js_e_buffer[idx].type & JS_EVENT_BUTTON )// Event Button detected
 162     {
 163       switch( js_e_buffer[idx].number )
 164       {
 165         case GP_BOARD_LEFT :
 166                                         ardrone_tool_set_ui_pad_start(js_e_buffer[idx].value);
 167                                         break;
 168         case GP_SIDE_RIGHT :
 169                                         ardrone_tool_set_ui_pad_r2(js_e_buffer[idx].value);
 170                                         break;
 171         case GP_IMPULSE :
 172                                         ardrone_tool_set_ui_pad_select(js_e_buffer[idx].value);
 173                                         break;
 174         case GP_SIDE_LEFT_DOWN :
 175                                         ardrone_tool_set_ui_pad_ad(js_e_buffer[idx].value);
 176                                         break;
 177         case GP_SIDE_LEFT_UP :
 178                                         ardrone_tool_set_ui_pad_ad(js_e_buffer[idx].value);
 179                                         break;
 180         default: break;
 181       }
 182     }
 183     else if(js_e_buffer[idx].type & JS_EVENT_AXIS )// Event Axis detected
 184     {
 185       refresh_values = TRUE;
 186       switch( js_e_buffer[idx].number )
 187       {
 188         case GP_PITCH:
 189           pitch = js_e_buffer[idx].value;
 190           break;
 191         case GP_GAZ:
 192           gaz = js_e_buffer[idx].value;
 193           break;
 194         case GP_ROLL:
 195           roll = js_e_buffer[idx].value;
 196           break;
 197         case GP_PID:
 198           break;
 199         case GP_YAW:
 200           yaw = js_e_buffer[idx].value;
 201           break;
 202         default:
 203           break;
 204       }
 205     }
 206     else
 207     {// TODO: default: ERROR (non-supported)
 208     }
 209   }
 210
 211   if(refresh_values)// Axis values to refresh
 212     {
 213        ardrone_at_set_progress_cmd( 1,
 214                                                roll/25000.0,
 215                                                pitch/25000.0,
 216                                                -gaz/25000.0,
 217                                                yaw/25000.0);
 218     }
 219
 220   return C_OK;
 221 }
 222
 223 C_RESULT close_radioGP(void)
 224 {
 225   close( joy_dev );
 226
 227   return C_OK;
 228 }
 229
 230
 231 ///////////////////////////////
 232 //  GamePad input functions  //
 233 ///////////////////////////////
 234
 235 C_RESULT open_gamepad(void)
 236 {
 237   C_RESULT res = C_FAIL;
 238
 239   FILE* f = fopen("/proc/bus/input/devices", "r");
 240
 241   if( f != NULL )
 242   {
 243     res = parse_proc_input_devices( f, GAMEPAD_LOGICTECH_ID);
 244
 245     fclose( f );
 246
 247     if( SUCCEED( res ) && strcmp(gamepad.name, "Gamepad")!=0)
 248     {
 249                         char dev_path[20]="/dev/input/";
 250                         strcat(dev_path, gamepad.name);
 251             joy_dev = open(dev_path, O_NONBLOCK | O_RDONLY);
 252     }
 253                 else
 254                 {
 255                         return C_FAIL;
 256                 }
 257   }
 258
 259   return res;
 260 }
 261
 262 C_RESULT update_gamepad(void)
 263 {
 264   static int32_t x = 0, y = 0;
 265   static bool_t refresh_values = FALSE;
 266   ssize_t res;
 267   static struct js_event js_e_buffer[64];
 268   static int32_t start = 0;
 269   input_state_t* input_state;
 270
 271   static int32_t theta_trim = 0;
 272   static int32_t phi_trim   = 0;
 273   static int32_t yaw_trim   = 0;
 274
 275
 276   res = read(joy_dev, js_e_buffer, sizeof(struct js_event) * 64);
 277
 278   if( !res || (res < 0 && errno == EAGAIN) )
 279     return C_OK;
 280
 281   if( res < 0 )
 282     return C_FAIL;
 283
 284   if (res < (int) sizeof(struct js_event))// If non-complete bloc: ignored
 285     return C_OK;
 286
 287   // Buffer d�composition in blocs (if the last is incomplete, it's ignored)
 288   int32_t idx = 0;
 289   refresh_values = FALSE;
 290   input_state = ardrone_tool_get_input_state();
 291   for (idx = 0; idx < res / sizeof(struct js_event); idx++)
 292   {
 293     if(js_e_buffer[idx].type & JS_EVENT_INIT )// If Init, the first values are ignored
 294     {
 295       break;
 296     }
 297     else if(js_e_buffer[idx].type & JS_EVENT_BUTTON )// Event Button detected
 298     {
 299       switch( js_e_buffer[idx].number )
 300       {
 301         case PAD_AG :
 302                 ardrone_tool_set_ui_pad_ag(js_e_buffer[idx].value);
 303                 break;
 304         case PAD_AB :
 305                 ardrone_tool_set_ui_pad_ab(js_e_buffer[idx].value);
 306                 break;
 307         case PAD_AD :
 308                 ardrone_tool_set_ui_pad_ad(js_e_buffer[idx].value);
 309                 break;
 310         case PAD_AH :
 311                 ardrone_tool_set_ui_pad_ah(js_e_buffer[idx].value);
 312                 break;
 313         case PAD_L1 :
 314                 if( js_e_buffer[idx].value )
 315                 {
 316                         yaw_trim = 0;
 317
 318                         if( input_state->r2 )
 319                         {
 320                                 yaw_trim = -1;
 321                         }
 322                         else
 323                         {
 324                                 ardrone_tool_set_ui_pad_l1(1);
 325                         }
 326          ui_pad_yaw_trim( yaw_trim );
 327                 }
 328       else
 329       {
 330          yaw_trim = 0;
 331          ardrone_tool_set_ui_pad_l1(0);
 332          ui_pad_yaw_trim( yaw_trim );
 333       }
 334                 break;
 335         case PAD_R1 :
 336                 if( js_e_buffer[idx].value )
 337                 {
 338                         yaw_trim = 0;
 339
 340                         if( input_state->r2 )
 341                         {
 342                                 yaw_trim = 1;
 343                         }
 344                         else
 345                         {
 346                                 ardrone_tool_set_ui_pad_r1(1);
 347                         }
 348          ui_pad_yaw_trim( yaw_trim );
 349                 }
 350       else
 351       {
 352          yaw_trim = 0;
 353          ardrone_tool_set_ui_pad_r1(0);
 354          ui_pad_yaw_trim( yaw_trim );
 355       }
 356                 break;
 357         case PAD_L2 :
 358                 ardrone_tool_set_ui_pad_l2(js_e_buffer[idx].value);
 359                 if( !js_e_buffer[idx].value )
 360                 {
 361                         ardrone_at_set_pmode( MiscVar[0] );
 362                         ardrone_at_set_ui_misc( MiscVar[0], MiscVar[1], MiscVar[2], MiscVar[3] );
 363                 }
 364                 break;
 365         case PAD_R2 :
 366                 ardrone_tool_set_ui_pad_r2(js_e_buffer[idx].value);
 367       if( !js_e_buffer[idx].value )
 368          ardrone_at_set_flat_trim();
 369                 break;
 370         case PAD_SELECT :
 371                 ardrone_tool_set_ui_pad_select(js_e_buffer[idx].value);
 372                 break;
 373         case PAD_START :
 374       if( js_e_buffer[idx].value )
 375       {
 376          start ^= 1;
 377                    ardrone_tool_set_ui_pad_start( start );
 378                 }
 379       break;
 380         default:
 381                 break;
 382       }
 383
 384     }
 385     else if(js_e_buffer[idx].type & JS_EVENT_AXIS )// Event Axis detected
 386     {
 387       refresh_values = TRUE;
 388       switch( js_e_buffer[idx].number )
 389       {
 390         case PAD_X:
 391           x = ( js_e_buffer[idx].value + 1 ) >> 15;
 392           break;
 393         case PAD_Y:
 394           y = ( js_e_buffer[idx].value + 1 ) >> 15;
 395           break;
 396         default:
 397           break;
 398       }
 399     }
 400     else
 401     {// TODO: default: ERROR (non-supported)
 402     }
 403   }
 404
 405   if(refresh_values)// Axis values to refresh
 406     {
 407
 408                  phi_trim    = 0;
 409                  theta_trim  = 0;
 410
 411                  if( input_state->r2 )
 412                  {
 413                          switch(x + 1)
 414                          {
 415                                  case 0:
 416                                          phi_trim = -1;
 417                                          break;
 418
 419                                  case 2:
 420                                          phi_trim = 1;
 421                                          break;
 422                          }
 423
 424                          switch(y + 1)
 425                          {
 426                                  case 0:
 427                                          theta_trim = -1;
 428                                          break;
 429
 430                                  case 2:
 431                                          theta_trim = 1;
 432                                          break;
 433                          }
 434                          // We are triming so we don't want to update ardrone position
 435                          x = 0;
 436                          y = 0;
 437                  }
 438                  ardrone_tool_set_ui_pad_xy( x, y );
 439        ui_pad_phi_trim( phi_trim );
 440        ui_pad_theta_trim( theta_trim );
 441     }
 442   return C_OK;
 443 }
 444
 445 C_RESULT close_gamepad(void)
 446 {
 447   close( joy_dev );
 448
 449   return C_OK;
 450 }
 451
 452 ///////////////////////////////
 453 //  Joystick input functions  //
 454 ///////////////////////////////
 455
 456 C_RESULT open_joystick(void)
 457 {
 458         PRINT("Searching Joystick device ...\n");
 459         C_RESULT res = C_FAIL;
 460         int device;
 461
 462         for(device = 0 ; (device < JOYSTICK_DEVICE_MAX) && FAILED(res) ; device++)
 463         {
 464                 FILE* f = fopen("/proc/bus/input/devices", "r");
 465
 466                 if( f != NULL )
 467                 {
 468                         res = parse_proc_input_devices( f,  joystick_device_ids[device]);
 469
 470                         fclose( f );
 471
 472                         if( SUCCEED( res ) && strcmp(joystick.name, "Joystick")!=0)
 473                         {
 474                                 char dev_path[20]="/dev/input/";
 475                                 strcat(dev_path, joystick.name);
 476                                 joy_dev = open(dev_path, O_NONBLOCK | O_RDONLY);
 477
 478                                 if (joy_dev)
 479                                 {
 480                                         printf("Joydev %s ouvert\n",dev_path);
 481                                         res = C_OK;
 482                                 }
 483                                 else{
 484                                         printf("Joydev %s pas ouvert\n",dev_path);
 485                                 }
 486                         }
 487                         else
 488                         {
 489                                 PRINT("Joystick device not found.\n");
 490                                 res = C_FAIL;
 491                         }
 492                 }
 493         }
 494
 495         if(res == C_OK)
 496                 current_joystick = device - 1;
 497
 498         return res;
 499 }
 500
 501 C_RESULT update_joystick(void)
 502 {
 503           static int32_t stick1LR = 0, stick1UD = 0, stick2LR = 0, stick2UD = 0;
 504           static bool_t refresh_values = FALSE;
 505           ssize_t res;
 506           static struct js_event js_e_buffer[64];
 507           static int32_t start = 0;
 508           input_state_t* input_state;
 509
 510           static int center_x1=0;
 511           static int center_y1=0;
 512           static int center_x2=0;
 513           static int center_y2=0;
 514
 515           res = read(joy_dev, js_e_buffer, sizeof(struct js_event) * 64);
 516
 517           if( !res || (res < 0 && errno == EAGAIN) )
 518             return C_OK;
 519
 520
 521           if( res < 0 )
 522             return C_FAIL;
 523
 524
 525           if (res < (int) sizeof(struct js_event))// If non-complete bloc: ignored
 526             return C_OK;
 527
 528
 529           int32_t idx = 0;
 530           refresh_values = FALSE;
 531           input_state = ardrone_tool_get_input_state();
 532           for (idx = 0; idx < res / sizeof(struct js_event); idx++)
 533           {
 534             if(js_e_buffer[idx].type & JS_EVENT_INIT )// If Init, the first values are ignored
 535             {
 536               break;
 537             }
 538             else if(js_e_buffer[idx].type & JS_EVENT_BUTTON )// Event Button detected
 539             {
 540 //            printf("Button event : %d => %d\n", js_e_buffer[idx].number, js_e_buffer[idx].value);
 541
 542               if(js_e_buffer[idx].number == joystick_table_buttons[current_joystick][JOYBTN_START])
 543               {
 544                   if( js_e_buffer[idx].value )
 545                   {
 546                           start ^= 1;
 547                           ardrone_tool_set_ui_pad_start( start );
 548                   }
 549               }
 550               else if(js_e_buffer[idx].number == joystick_table_buttons[current_joystick][JOYBTN_SELECT])
 551               {
 552                   start = 0;
 553                   ardrone_tool_set_ui_pad_start( start );
 554                   ardrone_tool_set_ui_pad_select(js_e_buffer[idx].value);
 555               }
 556             }
 557             else if(js_e_buffer[idx].type & JS_EVENT_AXIS )// Event Axis detected
 558             {
 559              // printf("Axes event : %d => %d\n", js_e_buffer[idx].number, js_e_buffer[idx].value);
 560               refresh_values = TRUE;
 561               if(js_e_buffer[idx].number == joystick_table_axis[current_joystick][JOYAXIS_ROLL])
 562                   stick1LR = ( js_e_buffer[idx].value ) ;
 563               else if(js_e_buffer[idx].number == joystick_table_axis[current_joystick][JOYAXIS_PITCH])
 564                   stick1UD = ( js_e_buffer[idx].value ) ;
 565               else if(js_e_buffer[idx].number == joystick_table_axis[current_joystick][JOYAXIS_YAW])
 566                       stick2LR = ( js_e_buffer[idx].value ) ;
 567               else if(js_e_buffer[idx].number == joystick_table_axis[current_joystick][JOYAXIS_GAZ])
 568                         stick2UD = ( js_e_buffer[idx].value ) ;
 569             }
 570             else
 571             {
 572                 // TODO: default: ERROR (non-supported)
 573             }
 574           }
 575
 576           if(refresh_values)// Axis values to refresh
 577             {
 578                   int enable = ((stick1LR-center_x1) != 0) || ((stick1UD-center_y1) != 0) || ((stick2UD-center_x2) != 0) || ((stick2LR-center_y2) != 0);
 579               ardrone_at_set_progress_cmd( enable,
 580                                             /*roll*/(float)(stick1LR-center_x1)/32767.0f,
 581                                             /*pitch*/(float)(stick1UD-center_y1)/32767.0f,
 582                                             /*gaz*/-(float)(stick2UD-center_x2)/32767.0f,
 583                                             /*yaw*/(float)(stick2LR-center_y2)/32767.0f );
 584             }
 585           return C_OK;
 586 }
 587
 588 C_RESULT close_joystick(void)
 589 {
 590   close( joy_dev );
 591
 592   return C_OK;
 593 }
 594
 595
 596
 597
 598
 599
 600 ///////////////////////////////
 601 //  Playstation 3 Gamepad input functions  //
 602 ///////////////////////////////
 603
 604 C_RESULT open_ps3pad(void)
 605 {
 606     PRINT("Searching PS3 Pad device ...\n");
 607
 608   C_RESULT res = C_FAIL;
 609
 610   FILE* f = fopen("/proc/bus/input/devices", "r");
 611
 612   if( f != NULL )
 613   {
 614     res = parse_proc_input_devices( f, GAMEPAD_PLAYSTATION3_ID );
 615
 616     fclose( f );
 617
 618     if( SUCCEED( res ) && strcmp(ps3pad.name, "PS3Gamepad")!=0)
 619     {
 620                         char dev_path[20]="/dev/input/";
 621                         strcat(dev_path, ps3pad.name);
 622             joy_dev = open(dev_path, O_NONBLOCK | O_RDONLY);
 623
 624             if (joy_dev)
 625             {
 626                 printf("Joydev %s ouvert\n",dev_path);
 627             }
 628             else{
 629                 printf("Joydev %s pas ouvert\n",dev_path);
 630             }
 631     }
 632                 else
 633                 {
 634                     PRINT("PS3 Pad device not found.\n");
 635                         return C_FAIL;
 636
 637                 }
 638   }
 639
 640   return res;
 641 }
 642
 643 C_RESULT update_ps3pad(void)
 644 {
 645
 646   static int32_t stick1LR = 0, stick1UD = 0 , stick2LR = 0 , stick2UD = 0;
 647   static bool_t refresh_values = FALSE;
 648   ssize_t res;
 649   static struct js_event js_e_buffer[64];
 650   static int32_t start = 0;
 651   input_state_t* input_state;
 652
 653   static int center_x1=0;
 654   static int center_y1=0;
 655   static int center_x2=0;
 656   static int center_y2=0;
 657
 658   res = read(joy_dev, js_e_buffer, sizeof(struct js_event) * 64);
 659
 660
 661   if( !res || (res < 0 && errno == EAGAIN) )
 662     return C_OK;
 663
 664
 665   if( res < 0 )
 666     return C_FAIL;
 667
 668
 669   if (res < (int) sizeof(struct js_event))// If non-complete bloc: ignored
 670     return C_OK;
 671
 672
 673   int32_t idx = 0;
 674   refresh_values = FALSE;
 675   input_state = ardrone_tool_get_input_state();
 676   for (idx = 0; idx < res / sizeof(struct js_event); idx++)
 677   {
 678     if(js_e_buffer[idx].type & JS_EVENT_INIT )// If Init, the first values are ignored
 679     {
 680       break;
 681     }
 682     else if(js_e_buffer[idx].type & JS_EVENT_BUTTON )// Event Button detected
 683     {
 684       switch( js_e_buffer[idx].number )
 685       {
 686         case PS3BTN_LEFTARROW :
 687                 ardrone_tool_set_ui_pad_ag(js_e_buffer[idx].value);
 688                 break;
 689         case PS3BTN_DOWNARROW :
 690                 ardrone_tool_set_ui_pad_ab(js_e_buffer[idx].value);
 691                 break;
 692         case PS3BTN_RIGHTARROW :
 693                 ardrone_tool_set_ui_pad_ad(js_e_buffer[idx].value);
 694                 break;
 695         case PS3BTN_UPARROW :
 696                 ardrone_tool_set_ui_pad_ah(js_e_buffer[idx].value);
 697                 break;
 698         case PS3BTN_L1 :
 699                 ardrone_tool_set_ui_pad_l1(js_e_buffer[idx].value);
 700                 break;
 701         case PS3BTN_R1 :
 702                 ardrone_tool_set_ui_pad_r1(js_e_buffer[idx].value);
 703                 break;
 704         case PS3BTN_L2 :
 705                 ardrone_tool_set_ui_pad_l2(js_e_buffer[idx].value);
 706                 break;
 707         case PS3BTN_R2 :
 708                 ardrone_tool_set_ui_pad_r2(js_e_buffer[idx].value);
 709                 break;
 710         case PS3BTN_SELECT :
 711                 ardrone_tool_set_ui_pad_select(js_e_buffer[idx].value);
 712                 break;
 713         case PS3BTN_START :
 714             if( js_e_buffer[idx].value )   { start ^= 1;  ardrone_tool_set_ui_pad_start( start );   }
 715                 break;
 716         case PS3BTN_PS3:
 717             /* Calibrate joystick */
 718            /* center_x1 = stick1LR;
 719             center_y1 = stick1UD;
 720             center_x2 = stick2UD;
 721             center_y2 = stick2LR;*/
 722         break;
 723         default:
 724                 break;
 725       }
 726     }
 727     else if(js_e_buffer[idx].type & JS_EVENT_AXIS )// Event Axis detected
 728     {
 729       refresh_values = TRUE;
 730       switch( js_e_buffer[idx].number )
 731       {
 732         case PS3AXIS_STICK1_LR:
 733           stick1LR = ( js_e_buffer[idx].value ) ;
 734           break;
 735         case PS3AXIS_STICK1_UD:
 736           stick1UD = ( js_e_buffer[idx].value ) ;
 737           break;
 738
 739         case PS3AXIS_STICK2_LR:
 740           stick2LR = ( js_e_buffer[idx].value ) ;
 741           break;
 742         case PS3AXIS_STICK2_UD:
 743           stick2UD = ( js_e_buffer[idx].value ) ;
 744           break;
 745
 746         default:
 747           break;
 748       }
 749     }
 750     else
 751     {// TODO: default: ERROR (non-supported)
 752     }
 753   }
 754
 755   if(refresh_values)// Axis values to refresh
 756     {
 757       ardrone_at_set_progress_cmd( 1,
 758                                     /*roll*/(float)(stick1LR-center_x1)/32767.0f,
 759                                     /*pitch*/(float)(stick1UD-center_y1)/32767.0f,
 760                                     /*gaz*/-(float)(stick2UD-center_x2)/32767.0f,
 761                                     /*yaw*/(float)(stick2LR-center_y2)/32767.0f );
 762     }
 763   return C_OK;
 764 }
 765
 766
 767
 768
 769
 770 C_RESULT close_ps3pad(void)
 771 {
 772   close( joy_dev );
 773
 774   return C_OK;
 775 }
 776
 777
 778
 779
 780
 781 static int32_t make_id(device_t* device)
 782 {
 783   return ( (device->vendor << 16) & 0xffff0000) | (device->product & 0xffff);
 784 }
 785
 786 static C_RESULT add_device(device_t* device, const int32_t id_wanted)
 787 {
 788   int32_t id = make_id(device);
 789   int device_id;
 790   if( id_wanted == GAMEPAD_LOGICTECH_ID && id == id_wanted)
 791   {
 792                 PRINT("Input device %s found\n", device->name);
 793     strncpy(gamepad.name, device->handlers, MAX_NAME_LENGTH);
 794     return C_OK;
 795         }
 796
 797   for(device_id = 0 ; device_id < JOYSTICK_DEVICE_MAX ; device_id++)
 798   {
 799           if( id_wanted == joystick_device_ids[device_id] && id == id_wanted)
 800           {
 801                   PRINT("Input device %s found\n", device->name);
 802                   strncpy(joystick.name, device->handlers, MAX_NAME_LENGTH);
 803                   return C_OK;
 804           }
 805   }
 806
 807   if(id_wanted == RADIO_GP_ID && id==id_wanted)
 808         {
 809                 PRINT("Input device %s found\n", device->name);
 810     strncpy(radioGP.name, device->handlers, MAX_NAME_LENGTH);
 811     return C_OK;
 812   }
 813
 814     if(id_wanted == GAMEPAD_PLAYSTATION3_ID && id==id_wanted)
 815         {
 816                 PRINT("PS3 : Input device %s found\n", device->name);
 817         strncpy(ps3pad.name, device->handlers, MAX_NAME_LENGTH);
 818         return C_OK;
 819   }
 820
 821   return C_FAIL;
 822 }
 823
 824
 825
 826
 827
 828 /** simple /proc/bus/input/devices generic LL(1) parser **/
 829
 830 #define KW_MAX_LEN 64
 831
 832 typedef enum {
 833   KW_BUS,
 834   KW_VENDOR,
 835   KW_PRODUCT,
 836   KW_VERSION,
 837   KW_NAME,
 838   KW_HANDLERS,
 839   KW_MAX
 840 } keyword_t;
 841
 842 typedef enum {
 843   INT,
 844   STRING,
 845 } value_type_t;
 846
 847 typedef struct {
 848   const char*   name;
 849   value_type_t  value_type;
 850   int32_t       value_offset;
 851 } kw_tab_entry_t;
 852
 853 static int current_c;
 854 static int next_c; // look ahead buffer
 855
 856 static device_t current_device;
 857
 858 static const int separators[] = { ' ',  ':', '=', '\"', '\n' };
 859 static const int quote = '\"';
 860 static const int eol = '\n';
 861
 862 static kw_tab_entry_t kw_tab[] = {
 863   [KW_BUS]      = {  "Bus",      INT,    offsetof(device_t, bus)       },
 864   [KW_VENDOR]   = {  "Vendor",   INT,    offsetof(device_t, vendor)    },
 865   [KW_PRODUCT]  = {  "Product",  INT,    offsetof(device_t, product)   },
 866   [KW_VERSION]  = {  "Version",  INT,    offsetof(device_t, version)   },
 867   [KW_NAME]     = {  "Name",     STRING, offsetof(device_t, name)      },
 868   [KW_HANDLERS] = {  "Handlers", STRING, offsetof(device_t, handlers)  }
 869 };
 870
 871 static const char* handler_names[] = {
 872   "js0",
 873   "js1",
 874   "js2",
 875   "js3",
 876   0
 877 };
 878
 879 static bool_t is_separator(int c)
 880 {
 881   int32_t i;
 882   bool_t found = FALSE;
 883
 884   for( i = 0; i < sizeof separators && !found; i++ )
 885   {
 886     found = ( c == separators[i] );
 887   }
 888
 889   return found;
 890 }
 891
 892 static bool_t is_quote(int c)
 893 {
 894   return c == quote;
 895 }
 896
 897 static bool_t is_eol(int c)
 898 {
 899   return c == eol;
 900 }
 901
 902 static C_RESULT fetch_char(FILE* f)
 903 {
 904   C_RESULT res = C_FAIL;
 905
 906   current_c = next_c;
 907
 908   if( !feof(f) )
 909   {
 910     next_c = fgetc(f);
 911     res = C_OK;
 912   }
 913
 914   // PRINT("current_c = %c, next_c = %c\n", current_c, next_c );
 915
 916   return res;
 917 }
 918
 919 static C_RESULT parse_string(FILE* f, char* str, int32_t maxlen)
 920 {
 921   int32_t i = 0;
 922   bool_t is_quoted = is_quote(current_c);
 923
 924   if( is_quoted )
 925   {
 926     while( SUCCEED(fetch_char(f)) && ! ( is_separator(current_c) && is_quote(current_c) ) )  {
 927       str[i] = current_c;
 928       i++;
 929     }
 930   }
 931   else
 932   {
 933     while( SUCCEED(fetch_char(f)) && !is_separator(current_c) )  {
 934       str[i] = current_c;
 935       i++;
 936     }
 937   }
 938
 939   str[i] = '\0';
 940   // PRINT("parse_string: %s\n", str);
 941
 942   return is_eol( current_c ) ? C_FAIL : C_OK;
 943 }
 944
 945 static C_RESULT parse_int(FILE* f, int32_t* i)
 946 {
 947   C_RESULT res = C_OK;
 948   int value;
 949
 950   *i = 0;
 951
 952   while( !is_separator(next_c) && SUCCEED(fetch_char(f)) && res == C_OK )  {
 953     value = current_c - '0';
 954
 955     if (value > 9 || value < 0)
 956     {
 957       value = current_c - 'a' + 10;
 958       res = (value > 0xF || value < 0xa) ? C_FAIL : C_OK;
 959     }
 960
 961     *i *= 16;
 962     *i += value;
 963   }
 964
 965   return res;
 966 }
 967
 968 static C_RESULT skip_line(FILE* f)
 969 {
 970   while( !is_eol(next_c) && SUCCEED(fetch_char(f)) );
 971
 972   return C_OK;
 973 }
 974
 975 static C_RESULT match_keyword( const char* keyword, keyword_t* kw )
 976 {
 977   int32_t i;
 978   C_RESULT res = C_FAIL;
 979
 980   for( i = 0; i < KW_MAX && res != C_OK; i++ )
 981   {
 982     res = ( strcmp( keyword, kw_tab[i].name ) == 0 ) ? C_OK : C_FAIL;
 983   }
 984
 985   *kw = i-1;
 986
 987   return res;
 988 }
 989
 990 static C_RESULT match_handler( void )
 991 {
 992   int32_t i = 0;
 993   bool_t found = FALSE;
 994
 995   while( !found && handler_names[i] != 0 )
 996   {
 997     found = strcmp( (char*)((char*)&current_device + kw_tab[KW_HANDLERS].value_offset), handler_names[i] ) == 0;
 998
 999     i ++;
1000   }
1001
1002         if(found)
1003         {
1004                 strcpy(current_device.handlers, handler_names[i-1]);
1005         }
1006
1007   return found ? C_OK : C_FAIL;
1008 }
1009
1010 static C_RESULT parse_keyword( FILE* f, keyword_t kw )
1011 {
1012   C_RESULT res = C_OK;
1013
1014   while( is_separator(next_c) && SUCCEED(fetch_char(f)) );
1015
1016   switch( kw_tab[kw].value_type ) {
1017     case INT:
1018       parse_int( f, (int32_t*)((char*)&current_device + kw_tab[kw].value_offset) );
1019       //PRINT("%s = %x\n", kw_tab[kw].name, *(int32_t*)((char*)&current_device + kw_tab[kw].value_offset) );
1020       break;
1021
1022     case STRING:
1023       parse_string( f, (char*)((char*)&current_device + kw_tab[kw].value_offset), KW_MAX_LEN );
1024       //PRINT("%s = %s\n", kw_tab[kw].name, (char*)((char*)&current_device + kw_tab[kw].value_offset) );
1025       break;
1026
1027     default:
1028       res = C_FAIL;
1029       break;
1030   }
1031
1032   return res;
1033 }
1034
1035 static C_RESULT parse_I(FILE* f)
1036 {
1037   char keyword[KW_MAX_LEN];
1038
1039   while( SUCCEED(fetch_char(f)) && is_separator(next_c) );
1040
1041   while( !is_eol(next_c) ) {
1042     keyword_t kw;
1043
1044     parse_string( f, keyword, KW_MAX_LEN );
1045     if( SUCCEED( match_keyword( keyword, &kw ) ) )
1046     {
1047       parse_keyword( f, kw );
1048     }
1049   }
1050
1051   return C_OK;
1052 }
1053
1054 static C_RESULT parse_N(FILE* f)
1055 {
1056   char keyword[KW_MAX_LEN];
1057
1058   while( SUCCEED(fetch_char(f)) && is_separator(next_c) );
1059
1060   while( !is_eol(next_c) ) {
1061     keyword_t kw;
1062
1063     parse_string( f, keyword, KW_MAX_LEN );
1064     if( SUCCEED( match_keyword( keyword, &kw ) ) )
1065     {
1066       parse_keyword( f, kw );
1067     }
1068   }
1069
1070
1071   return C_OK;
1072 }
1073
1074 static C_RESULT parse_H(FILE* f)
1075 {
1076   C_RESULT res = C_FAIL;
1077   char keyword[KW_MAX_LEN];
1078
1079   while( SUCCEED(fetch_char(f)) && is_separator(next_c) );
1080
1081   while( !is_eol(next_c) ) {
1082     parse_string( f, keyword, KW_MAX_LEN );
1083     if( strcmp( keyword, kw_tab[KW_HANDLERS].name ) == 0 )
1084     {
1085       while( FAILED(res) && SUCCEED( parse_string(f,
1086                                                   (char*)((char*)&current_device + kw_tab[KW_HANDLERS].value_offset),
1087                                                   KW_MAX_LEN ) ) )
1088       {
1089         res = match_handler();
1090       }
1091     }
1092   }
1093
1094   return res;
1095 }
1096
1097 static C_RESULT end_device(const int32_t id)
1098 {
1099   C_RESULT res = C_FAIL;
1100   res=add_device(&current_device, id);
1101   vp_os_memset( &current_device, 0, sizeof(device_t) );
1102
1103   return res;
1104 }
1105
1106 static C_RESULT parse_proc_input_devices(FILE* f, const int32_t id)
1107 {
1108   C_RESULT res = C_FAIL;
1109
1110   next_c = '\0';
1111   vp_os_memset( &current_device, 0, sizeof(device_t) );
1112
1113   while( res != C_OK && SUCCEED( fetch_char(f) ) )
1114   {
1115     switch( next_c )
1116     {
1117       case 'I': parse_I(f); break;
1118       case 'N': parse_N(f); break;
1119       case 'H': if( SUCCEED( parse_H(f) ) ) res = end_device(id); break;
1120       case 'P':
1121       case 'S':
1122       case 'B':
1123       default: skip_line(f); break;
1124     }
1125   }
1126
1127   return res;
1128 }