vcs.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/Examples/Linux/Testbenches/navdata_selection/Sources/ardrone_testing_tool.c

   1 /**
   2  * @file main.c
   3  * @author sylvain.gaeremynck@parrot.com
   4  * @date 2009/07/01
   5  */
   6 #include <ardrone_testing_tool.h>
   7
   8 //ARDroneLib
   9 #include <ardrone_tool/ardrone_time.h>
  10 #include <ardrone_tool/Navdata/ardrone_navdata_client.h>
  11 #include <ardrone_tool/Control/ardrone_control.h>
  12 #include <ardrone_tool/UI/ardrone_input.h>
  13 #include <ardrone_tool/ardrone_tool_configuration.h>
  14
  15 //Common
  16 #include <config.h>
  17 #include <ardrone_api.h>
  18
  19 //VP_SDK
  20 #include <ATcodec/ATcodec_api.h>
  21 #include <VP_Os/vp_os_print.h>
  22 #include <VP_Os/vp_os_thread.h>
  23 #include <VP_Api/vp_api_thread_helper.h>
  24 #include <VP_Os/vp_os_signal.h>
  25 #include <VP_Os/vp_os_types.h>
  26 #include <VP_Os/vp_os_delay.h>
  27 //#include <VP_Os/vp_os_signal_dep.h>
  28
  29 //Local project
  30 #include <UI/gamepad.h>
  31 #include <Video/video_stage.h>
  32
  33 static int32_t exit_ihm_program = 1;
  34
  35 /* Implementing Custom methods for the main function of an ARDrone application */
  36
  37 /* The delegate object calls this method during initialization of an ARDrone application */
  38 C_RESULT ardrone_tool_init_custom(int argc, char **argv)
  39 {
  40   /* Registering for a new device of game controller */
  41   ardrone_tool_input_add( &gamepad );
  42   ardrone_tool_input_add( &ps3pad );
  43
  44   /* Start all threads of your application */
  45   START_THREAD( video_stage, NULL );
  46   START_THREAD( main_application_thread , NULL );
  47   return C_OK;
  48 }
  49
  50 /* The delegate object calls this method when the event loop exit */
  51 C_RESULT ardrone_tool_shutdown_custom()
  52 {
  53   /* Relinquish all threads of your application */
  54   JOIN_THREAD( video_stage );
  55
  56   /* Unregistering for the current device */
  57   ardrone_tool_input_remove( &gamepad );
  58
  59   return C_OK;
  60 }
  61
  62 /* The event loop calls this method for the exit condition */
  63 bool_t ardrone_tool_exit()
  64 {
  65   return exit_ihm_program == 0;
  66 }
  67
  68 C_RESULT signal_exit()
  69 {
  70   exit_ihm_program = 0;
  71
  72   return C_OK;
  73 }
  74
  75 PROTO_THREAD_ROUTINE(main_application_thread, data);
  76
  77 /* Implementing thread table in which you add routines of your application and those provided by the SDK */
  78 BEGIN_THREAD_TABLE
  79   THREAD_TABLE_ENTRY( ardrone_control, 20 )
  80   THREAD_TABLE_ENTRY( navdata_update, 20 )
  81   THREAD_TABLE_ENTRY( video_stage, 20 )
  82   THREAD_TABLE_ENTRY( main_application_thread, 20 )
  83 END_THREAD_TABLE
  84
  85
  86 vp_os_mutex_t test_mutex;
  87 vp_os_cond_t  test_condition;
  88
  89 vp_os_mutex_t navdata_mutex;
  90 vp_os_cond_t  navdata_condition;
  91
  92 navdata_unpacked_t last_navdata;
  93
  94  C_RESULT demo_navdata_client_init( void* data ) { return C_OK; }
  95
  96  char anim[]={'-','\\','|','/'};
  97
  98  C_RESULT demo_navdata_client_process( const navdata_unpacked_t* const navdata )
  99  {
 100          static int counter=0;
 101          printf("%c\n\033[1A",anim[(counter++)%sizeof(anim)]);
 102          vp_os_memcpy(&last_navdata,navdata,sizeof(last_navdata));
 103          vp_os_cond_signal(&navdata_condition);
 104          return C_OK;
 105  }
 106
 107  C_RESULT demo_navdata_client_release( void ) {return C_OK;}
 108
 109
 110 void wait_for_next_navdata()
 111 {
 112         vp_os_mutex_lock(&navdata_mutex);
 113         vp_os_cond_wait(&navdata_condition);
 114         vp_os_mutex_unlock(&navdata_mutex);
 115 }
 116
 117
 118 void test_callback(int res)
 119 {
 120         PRINT("<ArdroneTool callback>\n");
 121         /* Make the test program continue */
 122         if (res)
 123         {
 124                 vp_os_cond_signal(&test_condition);
 125         }
 126         else
 127         {
 128                 printf("  -- Configuration command is taking time to succeed ... ---\n");
 129         }
 130 }
 131
 132 void wait_for_setting_to_be_acknowlegded()
 133 {
 134         vp_os_mutex_lock(&test_mutex);
 135         vp_os_cond_wait(&test_condition);
 136         vp_os_mutex_unlock(&test_mutex);
 137 }
 138
 139
 140 #define BLUE { printf("%s","\033[34;01m"); }
 141 #define RED { printf("%s","\033[31;01m"); }
 142 #define GREEN { printf("%s","\033[32;01m"); }
 143
 144 #define RAZ { printf("%s","\033[0m"); }
 145
 146 #define title(x) { BLUE; printf("[%i] %s",__LINE__,x); RAZ; }
 147
 148 #define ok(x)      { GREEN; printf("[%i] %s",__LINE__,x); RAZ; }
 149 #define failure(x) { RED; printf("[%i] %s",__LINE__,x); RAZ; }
 150
 151 void mypause()
 152 {
 153         //char buffer[128];
 154         char c;
 155         printf("Press a key to continue...\n");
 156         vp_os_delay(2000);
 157         return;
 158         c=fgetc(stdin); //getchar_unlocked();
 159         perror("Erreur");
 160         printf(" Key : %i\n",(int)c);
 161 }
 162
 163 //#define USE_STEP_1
 164 //#define USE_STEP_2
 165 #define USE_STEP_3
 166
 167
 168 DEFINE_THREAD_ROUTINE(main_application_thread, data)
 169 {
 170         int i;
 171         int expected_mask=0;
 172         int mask_to_test = 0;
 173         int32_t i_value;
 174
 175         /* Mutexes for synchronisation */
 176                 vp_os_mutex_init(&test_mutex);
 177                 vp_os_cond_init(&test_condition,&test_mutex);
 178
 179                 vp_os_mutex_init(&navdata_mutex);
 180                 vp_os_cond_init(&navdata_condition,&navdata_mutex);
 181
 182         vp_os_delay(1000);
 183
 184         //fflush(stdin);
 185
 186         title("[IMPORTANT]  Make sure no client application connected to the drone before.\n");
 187
 188         printf("Navdata full option mask : <%x>\n",NAVDATA_OPTION_FULL_MASK);
 189         printf("Navdata nb options : <%i>\n",NAVDATA_NUM_TAGS);
 190
 191
 192         mypause();
 193
 194 #ifdef USE_STEP_1
 195
 196         /* Check that the drone is in bootstrap mode */
 197
 198 /*Ardronetool sets the navdata full mode at startup */
 199 #ifdef NOARDRONETOOL
 200         title("-- Checking drone state at bootstrap -- \n");
 201
 202                 wait_for_next_navdata();
 203
 204                 if (ardrone_get_mask_from_state(last_navdata.ardrone_state,ARDRONE_NAVDATA_BOOTSTRAP))
 205                 { ok("Bootstrap bit is set.\n"); }
 206                 else
 207                 { failure("Bootstrap bit is not set.\n"); }
 208
 209                 if (!ardrone_get_mask_from_state(last_navdata.ardrone_state,ARDRONE_NAVDATA_DEMO_MASK))
 210                 { ok("Demo mask bit is not set\n"); }
 211                 else
 212                 { failure("Demo mask bit is set.\n"); }
 213 #endif
 214
 215         title("-- Checking drone state in navdata DEMO mode -- \n");
 216
 217                 i_value = 1;
 218                 ARDRONE_TOOL_CONFIGURATION_ADDEVENT(navdata_demo, &i_value, NULL);
 219
 220                 ARDRONE_TOOL_CONFIGURATION_GET(test_callback);
 221                 wait_for_setting_to_be_acknowlegded();
 222
 223                 wait_for_next_navdata();
 224
 225                 if (ardrone_get_mask_from_state(last_navdata.ardrone_state,ARDRONE_NAVDATA_BOOTSTRAP))
 226                 { failure("Bootstrap bit is set.\n"); }
 227                 else
 228                 { ok("Bootstrap bit is not set.\n"); }
 229
 230                 if (!ardrone_get_mask_from_state(last_navdata.ardrone_state,ARDRONE_NAVDATA_DEMO_MASK))
 231                 { failure("Demo mask bit is not set\n"); }
 232                 else
 233                 { ok("Demo mask bit is set.\n"); }
 234
 235                 if (last_navdata.last_navdata_refresh == ( NAVDATA_OPTION_MASK(NAVDATA_DEMO_TAG) | NAVDATA_OPTION_MASK(NAVDATA_VISION_DETECT_TAG) ) )
 236                 {       ok("Received the right options in the navdata\n");              }
 237                 else
 238                 {   failure("Received the wrong navdata options"); printf(" - mask : %x\n",last_navdata.last_navdata_refresh); }
 239
 240                 if (ardrone_control_config.navdata_options == ( NAVDATA_OPTION_MASK(NAVDATA_DEMO_TAG) | NAVDATA_OPTION_MASK(NAVDATA_VISION_DETECT_TAG) ) )
 241                 {       ok("Navdata option mask is ok in the configuration\n");         }
 242                 else
 243                 {   failure("Wrong navdata option mask in the configuration"); printf(" - mask : %x\n",ardrone_control_config.navdata_options); }
 244
 245                 mypause();
 246
 247
 248
 249         title("-- Checking drone state in navdata FULL mode -- \n");
 250
 251                 i_value = 0;
 252                 ARDRONE_TOOL_CONFIGURATION_ADDEVENT(navdata_demo, &i_value, NULL);
 253
 254                 ARDRONE_TOOL_CONFIGURATION_GET(test_callback);
 255                 wait_for_setting_to_be_acknowlegded();
 256
 257                 wait_for_next_navdata();
 258
 259                 if (ardrone_get_mask_from_state(last_navdata.ardrone_state,ARDRONE_NAVDATA_BOOTSTRAP))
 260                 { failure("Bootstrap bit is set.\n"); }
 261                 else
 262                 { ok("Bootstrap bit is not set.\n"); }
 263
 264                 if (!ardrone_get_mask_from_state(last_navdata.ardrone_state,ARDRONE_NAVDATA_DEMO_MASK))
 265                 { ok("Demo mask bit is not set\n"); }
 266                 else
 267                 { failure("Demo mask bit is set.\n"); }
 268
 269                 if ( (last_navdata.last_navdata_refresh  & (NAVDATA_OPTION_FULL_MASK)) == NAVDATA_OPTION_FULL_MASK )
 270                 {       ok("Received the right options in the navdata\n");              }
 271                 else
 272                 {   failure("Received the wrong navdata options"); printf("mask : %x\n",last_navdata.last_navdata_refresh); }
 273
 274                 if ( (ardrone_control_config.navdata_options & (NAVDATA_OPTION_FULL_MASK)) == NAVDATA_OPTION_FULL_MASK )
 275                 {   ok("Navdata option mask part is ok\n");             }
 276                 else
 277                 {   failure("Wrong navdata option mask"); printf("mask : %x\n",ardrone_control_config.navdata_options); }
 278
 279                 mypause();
 280
 281 #endif
 282
 283         /*----------------------------*/
 284 #ifdef USE_STEP_2
 285
 286                 /* Set the demo mode to get 15 navdatas per second */
 287                 i_value = 1;
 288                 ARDRONE_TOOL_CONFIGURATION_ADDEVENT(navdata_demo, &ivalue, NULL);
 289
 290                 for (i=NAVDATA_DEMO_TAG ; i<NAVDATA_NUM_TAGS ; i++)
 291                 {
 292                         title("-- Checking navdata tag "); printf("[%d]",i);
 293
 294                                         expected_mask = ( NAVDATA_OPTION_MASK(i) | NAVDATA_OPTION_MASK(NAVDATA_DEMO_TAG) |  NAVDATA_OPTION_MASK(NAVDATA_VISION_DETECT_TAG) );
 295                                         i_value = NAVDATA_OPTION_MASK(i);
 296                                         ARDRONE_TOOL_CONFIGURATION_ADDEVENT(navdata_options, &i_value, NULL);
 297                                         ARDRONE_TOOL_CONFIGURATION_GET(test_callback);
 298
 299                                         wait_for_setting_to_be_acknowlegded();
 300                                         wait_for_next_navdata();
 301
 302                                         if (ardrone_get_mask_from_state(last_navdata.ardrone_state,ARDRONE_NAVDATA_BOOTSTRAP))
 303                                         { failure("Bootstrap bit is set.\n"); }
 304                                         else
 305                                         { ok("Bootstrap bit is not set.\n"); }
 306
 307                                         if (ardrone_get_mask_from_state(last_navdata.ardrone_state,ARDRONE_NAVDATA_DEMO_MASK))
 308                                         { ok("Demo mask bit is set\n"); }
 309                                         else
 310                                         { failure("Demo mask bit is not set.\n"); }
 311
 312                                         if ( (last_navdata.last_navdata_refresh  & (NAVDATA_OPTION_FULL_MASK)) == expected_mask )
 313                                         {       ok("Received the right options in the navdata\n");              }
 314                                         else
 315                                         {   failure("Received the wrong navdata options"); printf("mask : %x\n",last_navdata.last_navdata_refresh); }
 316
 317                                         if ( (ardrone_control_config.navdata_options & (NAVDATA_OPTION_FULL_MASK)) == expected_mask )
 318                                         {   ok("Navdata option mask in the configuration is ok\n");             }
 319                                         else
 320                                         {   failure("Wrong navdata option mask in the configuration"); printf("mask : %x\n",ardrone_control_config.navdata_options); }
 321
 322                                         //mypause();
 323
 324                 }
 325
 326 #endif
 327
 328 #ifdef USE_STEP_3
 329
 330                 /* Set the demo mode to get 15 navdatas per second */
 331                 i_value = 1;
 332                 ARDRONE_TOOL_CONFIGURATION_ADDEVENT(navdata_demo, &i_value, NULL);
 333
 334                 while(1)
 335                 {
 336                         i = mask_to_test = rand() & NAVDATA_OPTION_FULL_MASK;
 337
 338                         title("-- Checking navdata tag "); printf("[%x]",i);
 339
 340                                                                 expected_mask = ( mask_to_test | NAVDATA_OPTION_MASK(NAVDATA_DEMO_TAG) |  NAVDATA_OPTION_MASK(NAVDATA_VISION_DETECT_TAG) );
 341                                                                 i_value = (int32_t)mask_to_test;
 342                                                                 ARDRONE_TOOL_CONFIGURATION_ADDEVENT(navdata_options, &i_value, NULL);
 343                                                                 ARDRONE_TOOL_CONFIGURATION_GET(test_callback);
 344
 345                                                                 wait_for_setting_to_be_acknowlegded();
 346                                                                 wait_for_next_navdata();
 347
 348                                                                 if (ardrone_get_mask_from_state(last_navdata.ardrone_state,ARDRONE_NAVDATA_BOOTSTRAP))
 349                                                                 { failure("Bootstrap bit is set.\n"); }
 350                                                                 else
 351                                                                 { ok("Bootstrap bit is not set.\n"); }
 352
 353                                                                 if (ardrone_get_mask_from_state(last_navdata.ardrone_state,ARDRONE_NAVDATA_DEMO_MASK))
 354                                                                 { ok("Demo mask bit is set\n"); }
 355                                                                 else
 356                                                                 { failure("Demo mask bit is not set.\n"); }
 357
 358                                                                 if ( (last_navdata.last_navdata_refresh  & (NAVDATA_OPTION_FULL_MASK)) == expected_mask )
 359                                                                 {       ok("Received the right options in the navdata\n");              }
 360                                                                 else
 361                                                                 {   failure("Received the wrong navdata options"); printf("mask : %x  expected : %x\n",last_navdata.last_navdata_refresh,expected_mask); }
 362
 363                                                                 if ( (ardrone_control_config.navdata_options & (NAVDATA_OPTION_FULL_MASK)) == expected_mask )
 364                                                                 {   ok("Navdata option mask in the configuration is ok\n");             }
 365                                                                 else
 366                                                                 {   failure("Wrong navdata option mask in the configuration"); printf("mask : %x  epxpected : %x\n",ardrone_control_config.navdata_options,expected_mask); }
 367
 368                 }
 369
 370 #endif
 371
 372
 373         title("\nEnd of the test.\n");
 374
 375 }