vcs.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/ARDroneLib/Soft/Lib/ardrone_tool/AT/ardrone_at_mutex.c

   1 /********************************************************************
   2  *                    COPYRIGHT PARROT 2010
   3  ********************************************************************
   4  *       PARROT                MODULES
   5  *-----------------------------------------------------------------*/
   6 /**
   7  * @file   ardrone_at.c
   8  * @brief  .
   9  *
  10  * @author K. Leplat <karl.leplat.ext@parrot.com>
  11  * @date   Mon Feb 1 10:30:50 2010
  12  *
  13  *
  14  *******************************************************************/
  15 #include <VP_Os/vp_os_assert.h>
  16 #include <VP_Os/vp_os_print.h>
  17 #include <ardrone_tool/Com/config_com.h>
  18 #include <ardrone_tool/ardrone_tool.h>
  19
  20 //Common
  21 #include <ardrone_api.h>
  22 #include <at_msgs_ids.h>
  23 #include <config.h>
  24
  25 //SDK
  26 #include <VP_Com/vp_com.h>
  27 #include <VP_Com/vp_com_socket.h>
  28
  29 //GNU STANDARD C LIBRARY
  30 #include <stdio.h>
  31
  32 /********************************************************************
  33  * Constants
  34  *******************************************************************/
  35 #define MAX_BUF_SIZE 256
  36
  37 /********************************************************************
  38  * Static variables and types
  39  *******************************************************************/
  40 AT_CODEC_MSG_IDS ids;
  41 static uint32_t at_init = 0;
  42 static vp_os_mutex_t at_mutex;
  43 static uint32_t nb_sequence = 0;
  44 static vp_com_socket_t at_socket;
  45 static AT_CODEC_FUNCTIONS_PTRS func_ptrs;
  46 static Write atcodec_write = NULL;
  47 static Read atcodec_read = NULL;
  48
  49 // Navdata
  50 float32_t nd_iphone_gaz=0;
  51 float32_t nd_iphone_yaw=0;
  52 int32_t nd_iphone_flag = 0;
  53 float32_t nd_iphone_phi=0;
  54 float32_t nd_iphone_theta=0;
  55
  56 /********************************************************************
  57  * Static function declarations
  58  *******************************************************************/
  59 static void atcodec_init( AT_CODEC_FUNCTIONS_PTRS *funcs );
  60 void ardrone_at_shutdown ( void );
  61 AT_CODEC_ERROR_CODE atresu_error(ATcodec_Memory_t *mem, ATcodec_Memory_t *output, int *id);
  62 AT_CODEC_ERROR_CODE atresu_ok(ATcodec_Memory_t *mem, ATcodec_Memory_t *output, int *id);
  63 AT_CODEC_ERROR_CODE host_init( void );
  64 AT_CODEC_ERROR_CODE host_shutdown( void );
  65 AT_CODEC_ERROR_CODE host_enable( void );
  66 AT_CODEC_ERROR_CODE host_open( void );
  67 AT_CODEC_ERROR_CODE host_close( void );
  68 AT_CODEC_ERROR_CODE host_write(int8_t *buffer, int32_t *len);
  69 AT_CODEC_ERROR_CODE host_read(int8_t *buffer, int32_t *len);
  70
  71
  72 /********************************************************************
  73  * Static functions
  74  *******************************************************************/
  75 AT_CODEC_ERROR_CODE atresu_ok(ATcodec_Memory_t *mem, ATcodec_Memory_t *output, int *id)
  76 {
  77   return AT_CODEC_GENERAL_OK;
  78 }
  79
  80 AT_CODEC_ERROR_CODE atresu_error(ATcodec_Memory_t *mem, ATcodec_Memory_t *output, int *id)
  81 {
  82   return AT_CODEC_GENERAL_OK;
  83 }
  84
  85 AT_CODEC_ERROR_CODE host_init( void )
  86 {
  87   if( func_ptrs.init != NULL )
  88      func_ptrs.init();
  89
  90 # undef ATCODEC_DEFINE_AT_CMD
  91 # define ATCODEC_DEFINE_AT_CMD(ID,Str,From,Cb,Prio) \
  92     if((ids.ID = ATcodec_Add_Defined_Message(Str)) == -1) \
  93       { \
  94         return AT_CODEC_INIT_ERROR; \
  95       }
  96
  97 # undef ATCODEC_DEFINE_AT_RESU
  98 # define ATCODEC_DEFINE_AT_RESU(ID,Str,From,Cb) \
  99     if((ids.ID = ATcodec_Add_Hashed_Message(Str,ids.From,Cb,0)) == -1) \
 100       { \
 101         return AT_CODEC_INIT_ERROR; \
 102       }
 103
 104 # include <at_msgs.h>
 105
 106   return AT_CODEC_INIT_OK;
 107 }
 108
 109 AT_CODEC_ERROR_CODE host_shutdown( void )
 110 {
 111   if( func_ptrs.shutdown != NULL )
 112      func_ptrs.shutdown();
 113
 114   ardrone_at_shutdown();
 115
 116   return AT_CODEC_SHUTDOWN_OK;
 117 }
 118
 119 AT_CODEC_ERROR_CODE host_enable( void )
 120 {
 121    if( func_ptrs.enable != NULL )
 122       return func_ptrs.enable();
 123
 124    /* Only used with ARDrone */
 125    return AT_CODEC_ENABLE_OK;
 126 }
 127
 128 AT_CODEC_ERROR_CODE host_open( void )
 129 {
 130         static bool_t init_ok = FALSE;
 131
 132    if( func_ptrs.open != NULL )
 133       return func_ptrs.open();
 134
 135         if( !init_ok )
 136         {
 137                 COM_CONFIG_SOCKET_AT(&at_socket, VP_COM_CLIENT, AT_PORT, wifi_ardrone_ip);
 138                 at_socket.protocol = VP_COM_UDP;
 139
 140                 if(VP_FAILED(vp_com_init(COM_AT())))
 141                 {
 142          PRINT ("Failed to init AT\n");
 143                         vp_com_shutdown( COM_AT() );
 144                         return AT_CODEC_OPEN_ERROR;
 145                 }
 146
 147       if(VP_FAILED(vp_com_open(COM_AT(), &at_socket, &atcodec_read, &atcodec_write)))
 148                 {
 149          PRINT ("Failed to open AT\n");
 150                         return AT_CODEC_OPEN_ERROR;
 151                 }
 152
 153                 init_ok = TRUE;
 154         }
 155
 156         return AT_CODEC_OPEN_OK;
 157 }
 158
 159 AT_CODEC_ERROR_CODE host_close( void )
 160 {
 161    if( func_ptrs.close != NULL )
 162       return func_ptrs.close();
 163
 164   vp_com_close(COM_AT(), &at_socket);
 165
 166   return AT_CODEC_CLOSE_OK;
 167 }
 168
 169 AT_CODEC_ERROR_CODE host_write(int8_t *buffer, int32_t *len)
 170 {
 171   if( func_ptrs.write != NULL )
 172      return func_ptrs.write( buffer, len );
 173
 174   if( atcodec_write != NULL )
 175   {
 176     return VP_FAILED(atcodec_write(&at_socket, buffer, len)) ? AT_CODEC_WRITE_ERROR : AT_CODEC_WRITE_OK;
 177   }
 178
 179   return AT_CODEC_WRITE_OK;
 180 }
 181
 182 AT_CODEC_ERROR_CODE host_read(int8_t *buffer, int32_t *len)
 183 {
 184   if( func_ptrs.read != NULL )
 185      return func_ptrs.read( buffer, len );
 186
 187   return AT_CODEC_READ_OK;
 188 }
 189
 190 static void atcodec_init( AT_CODEC_FUNCTIONS_PTRS *funcs )
 191 {
 192         if( funcs != NULL)
 193         {
 194                 memcpy(&func_ptrs, funcs, sizeof(*funcs));
 195         }
 196
 197 #if defined (_MSC_VER)
 198
 199         AT_CODEC_FUNCTIONS_PTRS ptrs =
 200         {
 201                 /*init*/host_init,
 202                 /*shutdown*/host_shutdown,
 203                 /*enable*/host_enable,
 204                 /*open*/host_open,
 205                 /*close*/host_close,
 206                 /*write*/host_write,
 207                 /*read*/host_read
 208         };
 209 #else
 210         AT_CODEC_FUNCTIONS_PTRS ptrs =
 211         {
 212                 .init     = host_init,
 213                 .shutdown = host_shutdown,
 214                 .enable   = host_enable,
 215                 .open     = host_open,
 216                 .close    = host_close,
 217                 .read     = host_read,
 218                 .write    = host_write,
 219         };
 220 #endif
 221         vp_os_mutex_init(&at_mutex);
 222         ATcodec_Init_Library( &ptrs );
 223 }
 224
 225 /********************************************************************
 226  * Public functions
 227  *******************************************************************/
 228 void ardrone_at_set_ui_value( uint32_t value )
 229 {
 230   if (!at_init)
 231      return;
 232
 233   vp_os_mutex_lock(&at_mutex);
 234   ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_RC_REF_EXE,
 235                                 ++nb_sequence,
 236                                 value );
 237   vp_os_mutex_unlock(&at_mutex);
 238 }
 239
 240 void ardrone_at_set_pmode( int32_t pmode )
 241 {
 242   if (!at_init)
 243      return;
 244
 245   vp_os_mutex_lock(&at_mutex);
 246   ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_PMODE_EXE,
 247                                 ++nb_sequence,
 248                                 pmode );
 249   vp_os_mutex_unlock(&at_mutex);
 250 }
 251
 252 void ardrone_at_set_ui_misc(int32_t m1, int32_t m2, int32_t m3, int32_t m4)
 253 {
 254   if (!at_init)
 255      return;
 256
 257   vp_os_mutex_lock(&at_mutex);
 258   ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_MISC_EXE,
 259                                 ++nb_sequence,
 260                                 m1,
 261                                 m2,
 262                                 m3,
 263                                 m4 );
 264   vp_os_mutex_unlock(&at_mutex);
 265 }
 266
 267 void ardrone_at_set_anim( anim_mayday_t type, int32_t timeout )
 268 {
 269         int32_t animtype = type;
 270
 271         if (!at_init)
 272                 return;
 273
 274         vp_os_mutex_lock(&at_mutex);
 275         ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_ANIM_EXE,
 276                         ++nb_sequence,
 277                         animtype,
 278                         timeout );
 279         vp_os_mutex_unlock(&at_mutex);
 280 }
 281
 282 void ardrone_at_set_flat_trim(void)
 283 {
 284   if (!at_init)
 285      return;
 286
 287   vp_os_mutex_lock(&at_mutex);
 288         ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_FTRIM_EXE,
 289                                  ++nb_sequence );
 290   vp_os_mutex_unlock(&at_mutex);
 291 }
 292
 293 void ardrone_at_set_manual_trims(float32_t trim_pitch, float32_t trim_roll, float32_t trim_yaw)
 294 {
 295   float_or_int_t _trim_pitch, _trim_roll, _trim_yaw;
 296   if (!at_init)
 297      return;
 298
 299   _trim_pitch.f = trim_pitch;
 300   _trim_roll.f = trim_roll;
 301   _trim_yaw.f = trim_yaw;
 302
 303   vp_os_mutex_lock(&at_mutex);
 304   ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_MTRIM_EXE,
 305                                 ++nb_sequence,
 306                                 _trim_pitch.i,
 307                                 _trim_roll.i,
 308                                 _trim_yaw.i);
 309   vp_os_mutex_unlock(&at_mutex);
 310 }
 311
 312 void ardrone_at_set_control_gains( api_control_gains_t* gains )
 313 {
 314   if (!at_init)
 315      return;
 316
 317   vp_os_mutex_lock(&at_mutex);
 318   ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_GAIN_EXE,
 319                                 ++nb_sequence,
 320                                 gains->pq_kp, gains->r_kp, gains->r_ki, gains->ea_kp, gains->ea_ki,
 321                                 gains->alt_kp, gains->alt_ki, gains->vz_kp, gains->vz_ki,
 322                                 gains->hovering_kp, gains->hovering_ki,
 323                                 gains->hovering_b_kp, gains->hovering_b_ki);
 324   vp_os_mutex_unlock(&at_mutex);
 325 }
 326
 327 void ardrone_at_set_vision_track_params( api_vision_tracker_params_t* params )
 328 {
 329   if (!at_init)
 330      return;
 331
 332   vp_os_mutex_lock(&at_mutex);
 333   ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_VISP_EXE,
 334                                 ++nb_sequence,
 335                                 params->coarse_scale,
 336                                 params->nb_pair,
 337                                 params->loss_per,
 338                                 params->nb_tracker_width,
 339                                 params->nb_tracker_height,
 340                                 params->scale,
 341                                 params->trans_max,
 342                                 params->max_pair_dist,
 343                                 params->noise );
 344   vp_os_mutex_unlock(&at_mutex);
 345 }
 346
 347 void ardrone_at_start_raw_capture(void)
 348 {
 349   if (!at_init)
 350      return;
 351
 352   vp_os_mutex_lock(&at_mutex);
 353   ATcodec_Queue_Message_valist(ids.AT_MSG_ATCMD_RAWC_EXE,
 354                                ++nb_sequence);
 355   vp_os_mutex_unlock(&at_mutex);
 356 }
 357
 358 void ardrone_at_zap( ZAP_VIDEO_CHANNEL channel )
 359 {
 360   if (!at_init)
 361      return;
 362
 363   vp_os_mutex_lock(&at_mutex);
 364   ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_ZAP_EXE,
 365                                 ++nb_sequence,
 366                                 channel );
 367   vp_os_mutex_unlock(&at_mutex);
 368 }
 369
 370 void ardrone_at_cad( CAD_TYPE type, float32_t tag_size )
 371 {
 372   float_or_int_t size;
 373   size.f = tag_size;
 374
 375   if (!at_init)
 376      return;
 377
 378   vp_os_mutex_lock(&at_mutex);
 379   ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_CAD_EXE,
 380                                 ++nb_sequence,
 381                                 type,
 382                                 size.i);
 383   vp_os_mutex_unlock(&at_mutex);
 384 }
 385
 386 /********************************************************************
 387  * ardrone_at_set_progress_cmd:
 388  *-----------------------------------------------------------------*/
 389 /**
 390  * @param enable 1,with pitch,roll and 0,without pitch,roll.
 391  * @param pitch Using floating value between -1 to +1.
 392  * @param roll Using floating value between -1 to +1.
 393  * @param gaz Using floating value between -1 to +1.
 394  * @param yaw Using floating value between -1 to +1.
 395  *
 396  * @brief
 397  *
 398  * @DESCRIPTION
 399  *
 400  *******************************************************************/
 401 void ardrone_at_set_progress_cmd( int32_t flag, float32_t phi, float32_t theta, float32_t gaz, float32_t yaw )
 402 {
 403    float_or_int_t _phi, _theta, _gaz, _yaw;
 404
 405         if (!at_init)
 406                 return;
 407
 408    _phi.f = phi;
 409    _theta.f = theta;
 410    _gaz.f = gaz;
 411    _yaw.f = yaw;
 412
 413         // Saving values to set them in navdata_file
 414    nd_iphone_flag=flag;
 415         nd_iphone_phi=phi;
 416         nd_iphone_theta=theta;
 417         nd_iphone_gaz=gaz;
 418         nd_iphone_yaw=yaw;
 419
 420   vp_os_mutex_lock(&at_mutex);
 421         ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_PCMD_EXE,
 422                                  ++nb_sequence,
 423                                  flag,
 424                                  _phi.i,
 425                                  _theta.i,
 426                                  _gaz.i,
 427                                  _yaw.i );
 428   vp_os_mutex_unlock(&at_mutex);
 429 }
 430
 431 void ardrone_at_set_led_animation ( LED_ANIMATION_IDS anim_id, float32_t freq, uint32_t duration_sec )
 432 {
 433         float_or_int_t _freq;
 434
 435         if (!at_init)
 436      return;
 437
 438         _freq.f = freq;
 439
 440   vp_os_mutex_lock(&at_mutex);
 441         ATcodec_Queue_Message_valist(ids.AT_MSG_ATCMD_LED_EXE,
 442                                 ++nb_sequence,
 443                                 anim_id,
 444                                 _freq.i,
 445                                 duration_sec);
 446   vp_os_mutex_unlock(&at_mutex);
 447 }
 448
 449 void ardrone_at_set_vision_update_options(int32_t user_vision_option)
 450 {
 451   if (!at_init)
 452      return;
 453
 454   vp_os_mutex_lock(&at_mutex);
 455   ATcodec_Queue_Message_valist(ids.AT_MSG_ATCMD_VISO_EXE,
 456                                ++nb_sequence,
 457                                user_vision_option);
 458   vp_os_mutex_unlock(&at_mutex);
 459 }
 460
 461 /********************************************************************
 462  * ardrone_at_set_polaris_pos:
 463  *-----------------------------------------------------------------*/
 464 void ardrone_at_set_polaris_pos( float32_t fx, float32_t fy, float32_t fpsi, bool_t defined, int32_t time_us )
 465 {
 466   float_or_int_t x, y, psi;
 467
 468   if (!at_init)
 469      return;
 470
 471   x.f   = fx;
 472   y.f   = fy;
 473   psi.f = fpsi;
 474
 475   vp_os_mutex_lock(&at_mutex);
 476   ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_POLARIS_EXE,
 477                                 ++nb_sequence,
 478                                 x.i,
 479                                 y.i,
 480                                 psi.i,
 481                                 defined,
 482                                 time_us );
 483   vp_os_mutex_unlock(&at_mutex);
 484 }
 485
 486 void ardrone_at_set_vicon_data(struct timeval time, int32_t frame_number, float32_t latency, vector31_t global_translation, vector31_t global_rotation_euler)
 487 {
 488         float_or_int_t _latency, _global_translation_x, _global_translation_y, _global_translation_z, _global_rotation_euler_x, _global_rotation_euler_y,
 489                                         _global_rotation_euler_z;
 490
 491         if (!at_init)
 492          return;
 493
 494         _latency.f = latency;
 495         _global_translation_x.f = global_translation.x;
 496         _global_translation_y.f = global_translation.y;
 497         _global_translation_z.f = global_translation.z;
 498         _global_rotation_euler_x.f = global_rotation_euler.x;
 499         _global_rotation_euler_y.f = global_rotation_euler.y;
 500         _global_rotation_euler_z.f = global_rotation_euler.z;
 501
 502         vp_os_mutex_lock(&at_mutex);
 503         ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_VICON_EXE,
 504                                                                 ++nb_sequence,
 505                                                                 (int)time.tv_sec,
 506                                                                 (int)time.tv_usec,
 507                                                                 (int)frame_number,
 508                                                                 _latency.i,
 509                                                                 _global_translation_x.i,
 510                                                                 _global_translation_y.i,
 511                                                                 _global_translation_z.i,
 512                                                                 _global_rotation_euler_x.i,
 513                                                                 _global_rotation_euler_y.i,
 514                                                                 _global_rotation_euler_z.i);
 515
 516         vp_os_mutex_unlock(&at_mutex);
 517 }
 518
 519 /********************************************************************
 520  * ardrone_at_set_toy_configuration_ids:
 521  *-----------------------------------------------------------------*/
 522 /**
 523  * @param param A key as read from an ini file is given as "section:key".
 524  *
 525  * @param value
 526  *
 527  * @brief identified ardrone_at_set_toy_configuration
 528  *
 529  * @DESCRIPTION
 530  *
 531  *******************************************************************/
 532 void ardrone_at_set_toy_configuration_ids(const char* param,char* ses_id, char* usr_id, char* app_id,  const char* value)
 533 {
 534   if (!at_init)
 535      return;
 536
 537   vp_os_mutex_lock(&at_mutex);
 538   ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_CONFIG_IDS,
 539                                 ++nb_sequence,
 540                                 ses_id,
 541                                 usr_id,
 542                                 app_id );
 543   ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_CONFIG_EXE,
 544                                 ++nb_sequence,
 545                                 param,
 546                                 value );
 547   vp_os_mutex_unlock(&at_mutex);
 548 }
 549
 550 /********************************************************************
 551  * ardrone_at_reset_com_watchdog:
 552  *-----------------------------------------------------------------*/
 553 /**
 554  * @brief Re-connect with the ARDrone.
 555  *
 556  * @DESCRIPTION
 557  *
 558  *******************************************************************/
 559 void ardrone_at_reset_com_watchdog(void)
 560 {
 561   if (!at_init)
 562      return;
 563
 564   vp_os_mutex_lock(&at_mutex);
 565   ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_RESET_COM_WATCHDOG,
 566                                 ++nb_sequence );
 567   vp_os_mutex_unlock(&at_mutex);
 568 }
 569
 570 /********************************************************************
 571  * ardrone_at_update_control_mode:
 572  *-----------------------------------------------------------------*/
 573 /**
 574  * @param what_to_do
 575  *
 576  * @param filesize
 577  *
 578  * @brief .
 579  *
 580  * @DESCRIPTION
 581  *
 582  *******************************************************************/
 583 void ardrone_at_update_control_mode(uint32_t what_to_do, uint32_t filesize)
 584 {
 585   if (!at_init)
 586      return;
 587
 588   vp_os_mutex_lock(&at_mutex);
 589   ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_CTRL_EXE,
 590                                 ++nb_sequence,
 591                                 what_to_do,
 592                                 filesize );
 593   vp_os_mutex_unlock(&at_mutex);
 594 }
 595
 596 /********************************************************************
 597  * ardrone_at_configuration_get_ctrl_mode:
 598  *-----------------------------------------------------------------*/
 599 /**
 600  * @brief Request to receive configuration file.
 601  *
 602  * @DESCRIPTION
 603  *
 604  *******************************************************************/
 605 void ardrone_at_configuration_get_ctrl_mode(void)
 606 {
 607   if (!at_init)
 608      return;
 609
 610  ardrone_at_update_control_mode( CFG_GET_CONTROL_MODE, 0 );
 611 }
 612
 613 /* Stephane - multiconfiguration support */
 614 void ardrone_at_custom_configuration_get_ctrl_mode(void)
 615 {
 616         printf("%s %s %i\n",__FILE__,__FUNCTION__,__LINE__);
 617   if (!at_init)
 618      return;
 619
 620  ardrone_at_update_control_mode( CUSTOM_CFG_GET_CONTROL_MODE, 0 );
 621 }
 622
 623 /********************************************************************
 624  * ardrone_at_configuration_ack_ctrl_mode:
 625  *-----------------------------------------------------------------*/
 626 /**
 627  * @brief Signal to the drone that the file has been received.
 628  *
 629  * @DESCRIPTION
 630  *
 631  *******************************************************************/
 632 void ardrone_at_configuration_ack_ctrl_mode(void)
 633 {
 634   if (!at_init)
 635      return;
 636
 637   ardrone_at_update_control_mode( ACK_CONTROL_MODE, 0 );
 638 }
 639
 640 /********************************************************************
 641  * ardrone_at_set_pwm: .
 642  *-----------------------------------------------------------------*/
 643 /**
 644  * @param p1
 645  *
 646  * @param p2
 647  *
 648  * @param p3
 649  *
 650  * @param p4
 651  *
 652  * @brief .
 653  *
 654  * @DESCRIPTION
 655  *
 656  *******************************************************************/
 657 void ardrone_at_set_pwm(int32_t p1, int32_t p2, int32_t p3, int32_t p4)
 658 {
 659   if (!at_init)
 660      return;
 661
 662   vp_os_mutex_lock(&at_mutex);
 663   ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_PWM_EXE,
 664                                 ++nb_sequence,
 665                                 p1,
 666                                 p2,
 667                                 p3,
 668                                 p4 );
 669   vp_os_mutex_unlock(&at_mutex);
 670 }
 671
 672 /********************************************************************
 673  * ardrone_at_set_autonomous_flight: Enables / disables the autopilot.
 674  *-----------------------------------------------------------------*/
 675 /**
 676  * @param isActive Integer set to 1 to enable the autopilot.
 677  *
 678  * @brief Enables / disables the autopilot.
 679  *
 680  * @DESCRIPTION
 681  *
 682  *******************************************************************/
 683 void ardrone_at_set_autonomous_flight( int32_t isActive )
 684 {
 685   if (!at_init)
 686      return;
 687
 688   vp_os_mutex_lock(&at_mutex);
 689    ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_AUTONOMOUS_FLIGHT_EXE,
 690                                 ++nb_sequence,
 691                                 isActive );
 692   vp_os_mutex_unlock(&at_mutex);
 693 }
 694
 695 static inline void strtolower(char *str)
 696 {
 697         int i;
 698         for(i = 0 ; str[i] != '\0' ; i++)
 699                 str[i] = (char)tolower((int)str[i]);
 700 }
 701
 702 #undef ARDRONE_CONFIG_KEY_IMM
 703 #undef ARDRONE_CONFIG_KEY_REF
 704 #undef ARDRONE_CONFIG_KEY_STR
 705 #define ARDRONE_CONFIG_KEY_IMM(KEY, NAME, INI_TYPE, C_TYPE, C_TYPE_PTR, RW, DEFAULT, CALLBACK)  \
 706 ARDRONE_CONFIGURATION_PROTOTYPE(NAME)                                                                                                                   \
 707 {                                                                                                                                                                                               \
 708         C_RESULT res = C_FAIL;                                                                                                                                          \
 709     char msg[64];                                                                               \
 710     if(strcmp("" #INI_TYPE, "INI_FLOAT") == 0)                                                  \
 711     {                                                                                           \
 712         sprintf( &msg[0], "%f", *((float*)value));                                              \
 713         res = C_OK;                                                                             \
 714     }                                                                                           \
 715     if(strcmp("" #INI_TYPE, "INI_DOUBLE") == 0)                                                 \
 716     {                                                                                           \
 717     sprintf( &msg[0], "%lf", *((double*)value));                                                \
 718     res = C_OK;                                                                                 \
 719     }                                                                                           \
 720     else if(strcmp("" #INI_TYPE, "INI_INT") == 0)                                               \
 721     {                                                                                           \
 722         sprintf( &msg[0], "%d", *((int*)value));                                                \
 723         res = C_OK;                                                                             \
 724     }                                                                                           \
 725     else if(strcmp("" #INI_TYPE, "INI_BOOLEAN") == 0)                                           \
 726     {                                                                                           \
 727         sprintf( &msg[0], *((bool_t*)value) ? "TRUE" : "FALSE");                                \
 728         res = C_OK;                                                                             \
 729     }                                                                                           \
 730     if(res == C_OK)                                                                             \
 731     {                                                                                           \
 732         char tmp[64];                                                                           \
 733         strcpy(&tmp[0], KEY);                                                                   \
 734         strtolower(&tmp[0]);                                                                    \
 735         strcat(tmp, ":" #NAME);                                                                 \
 736         ardrone_at_set_toy_configuration_ids( &tmp[0], ses_id, usr_id, app_id, &msg[0] );       \
 737     }                                                                                           \
 738         return res;                                                                                                                                                                     \
 739 }
 740
 741 #define ARDRONE_CONFIG_KEY_REF(KEY, NAME, INI_TYPE, C_TYPE, C_TYPE_PTR, RW, DEFAULT, CALLBACK)
 742
 743 #define ARDRONE_CONFIG_KEY_STR(KEY, NAME, INI_TYPE, C_TYPE, C_TYPE_PTR, RW, DEFAULT, CALLBACK)  \
 744 ARDRONE_CONFIGURATION_PROTOTYPE(NAME)                                                                                                                   \
 745 {                                                                                                                                                                                               \
 746         C_RESULT res = C_FAIL;                                                                                                                                          \
 747         if(value != NULL)                                                                           \
 748         {                                                                                                                                                                                       \
 749                 char tmp[64];                                                                                                                                                   \
 750                 strcpy(&tmp[0], KEY);                                                                                                                                   \
 751                 strtolower(&tmp[0]);                                                                                                                                    \
 752                 strcat(&tmp[0], ":" #NAME);                                                                                                                             \
 753                 ardrone_at_set_toy_configuration_ids( &tmp[0], ses_id, usr_id, app_id, ((C_TYPE_PTR)value) ); \
 754                 res = C_OK;                                                                                                                                                             \
 755         }                                                                                                                                                                                       \
 756         return res;                                                                                                                                                                     \
 757 }
 758
 759 #include <config_keys.h> // must be included before to have types
 760
 761
 762
 763 /********************************************************************
 764  * ardrone_at_init_with_funcs: Init at command with ATCodec funcs
 765  *-----------------------------------------------------------------*/
 766 /**
 767  * @param void
 768  *
 769  * @brief Fill structure AT codec
 770  *        and built the library AT commands.
 771  *
 772  * @DESCRIPTION
 773  *
 774  *******************************************************************/
 775 void ardrone_at_init_with_funcs ( const char* ip, size_t ip_len, AT_CODEC_FUNCTIONS_PTRS *funcs)
 776 {
 777    if ( at_init )
 778       return;
 779
 780    VP_OS_ASSERT( ip_len < MAX_BUF_SIZE );
 781
 782    vp_os_memcpy( &wifi_ardrone_ip[0], ip, ip_len);
 783    wifi_ardrone_ip[ip_len]='\0';
 784
 785    atcodec_init (funcs);
 786
 787    at_init = 1;
 788 }
 789
 790 /********************************************************************
 791  * ardrone_at_shutdown: Close at command.
 792  *-----------------------------------------------------------------*/
 793 /**
 794  * @param void
 795  *
 796  * @brief
 797  *
 798  * @DESCRIPTION
 799  *
 800  *******************************************************************/
 801 void ardrone_at_shutdown ( void )
 802 {
 803         if ( !at_init )
 804                 return;
 805
 806         ATcodec_Shutdown_Library();
 807         vp_os_mutex_destroy(&at_mutex);
 808
 809         at_init = 0;
 810 }
 811
 812 /********************************************************************
 813  * ardrone_at_init: Init at command.
 814  *-----------------------------------------------------------------*/
 815 /**
 816  * @param void
 817  *
 818  * @brief Fill structure AT codec
 819  *        and built the library AT commands.
 820  *
 821  * @DESCRIPTION
 822  *
 823  *******************************************************************/
 824 void ardrone_at_init ( const char* ip, size_t ip_len)
 825 {
 826    if ( at_init )
 827       return;
 828
 829    VP_OS_ASSERT( ip_len < MAX_BUF_SIZE );
 830
 831    if(wifi_ardrone_ip!=ip){
 832            vp_os_memcpy( &wifi_ardrone_ip[0], ip, ip_len);
 833    }
 834    wifi_ardrone_ip[ip_len]='\0';
 835
 836    atcodec_init (NULL);
 837
 838    at_init = 1;
 839 }
 840
 841 /********************************************************************
 842  * ardrone_at_open: Open at command socket.
 843  *-----------------------------------------------------------------*/
 844 /**
 845  * @param void
 846  *
 847  * @brief Open at command socket.
 848  *
 849  * @DESCRIPTION
 850  *
 851  *******************************************************************/
 852 ATCODEC_RET ardrone_at_open ( void )
 853 {
 854    if ( !at_init )
 855       return ATCODEC_FALSE;
 856
 857    return host_open()==AT_CODEC_OPEN_OK?ATCODEC_TRUE:ATCODEC_FALSE;
 858 }
 859
 860 /********************************************************************
 861  * ardrone_at_send: Send all pushed messages.
 862  *-----------------------------------------------------------------*/
 863 /**
 864  * @param void
 865  *
 866  * @brief Send all pushed messages.
 867  *
 868  * @DESCRIPTION
 869  *
 870  *******************************************************************/
 871 ATCODEC_RET ardrone_at_send ( void )
 872 {
 873         C_RESULT res;
 874
 875     if ( !at_init )
 876       return ATCODEC_FALSE;
 877
 878         res = ATcodec_Send_Messages();
 879
 880    return res;
 881 }
 882