git.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/ARDroneLib/Soft/Lib/ardrone_tool/ardrone_tool.c

   1 #include <VP_Os/vp_os_malloc.h>
   2 #include <VP_Os/vp_os_print.h>
   3 #include <VP_Api/vp_api_thread_helper.h>
   4
   5 #include <ardrone_tool/ardrone_tool.h>
   6 #include <ardrone_tool/ardrone_time.h>
   7 #include <ardrone_tool/ardrone_tool_configuration.h>
   8 #include <ardrone_tool/Navdata/ardrone_navdata_client.h>
   9 #include <ardrone_tool/UI/ardrone_input.h>
  10 #include <ardrone_tool/Com/config_com.h>
  11
  12 #include <utils/ardrone_gen_ids.h>
  13
  14 int32_t MiscVar[NB_MISC_VARS] = {
  15                DEFAULT_MISC1_VALUE,
  16                DEFAULT_MISC2_VALUE,
  17                DEFAULT_MISC3_VALUE,
  18                DEFAULT_MISC4_VALUE
  19                                 };
  20
  21 //static bool_t need_update   = TRUE;
  22 static ardrone_timer_t ardrone_tool_timer;
  23 static int ArdroneToolRefreshTimeInUs = ARDRONE_REFRESH_MS * 1000;
  24 static vp_os_mutex_t ardrone_tool_mutex;
  25 static bool_t ardrone_tool_in_pause = FALSE;
  26 char wifi_ardrone_ip[256] = { WIFI_ARDRONE_IP };
  27 char app_id [MULTICONFIG_ID_SIZE] = "00000000"; // Default application ID.
  28 char app_name [APPLI_NAME_SIZE] = "Default application"; // Default application name.
  29 char usr_id [MULTICONFIG_ID_SIZE] = "00000000"; // Default user ID.
  30 char usr_name [USER_NAME_SIZE] = "Default user"; // Default user name.
  31 char ses_id [MULTICONFIG_ID_SIZE] = "00000000"; // Default session ID.
  32 char ses_name [SESSION_NAME_SIZE] = "Default session"; // Default session name.
  33
  34 #ifndef __SDK_VERSION__
  35 #define __SDK_VERSION__ "1.8" // TEMPORARY LOCATION OF __SDK_VERSION__ !!!
  36 #endif
  37
  38
  39 int usleep(unsigned int usec);
  40
  41 static bool_t send_com_watchdog = FALSE;
  42
  43 void ardrone_tool_send_com_watchdog( void )
  44 {
  45   send_com_watchdog = TRUE;
  46 }
  47
  48 #ifndef NO_ARDRONE_MAINLOOP
  49 static void ardrone_tool_usage( const char* appname )
  50 {
  51   printf("%s based on ARDrone Tool\n", appname);
  52   printf("Be aware to not insert space in your options\n");
  53
  54   ardrone_tool_display_cmd_line_custom();
  55 }
  56 #endif
  57
  58 static void ardrone_toy_network_adapter_cb( const char* name )
  59 {
  60         strcpy( COM_CONFIG_NAVDATA()->itfName, name );
  61 }
  62
  63 C_RESULT ardrone_tool_setup_com( const char* ssid )
  64 {
  65   C_RESULT res = C_OK;
  66
  67 #ifdef CHECK_WIFI_CONFIG
  68   if( FAILED(vp_com_init(COM_NAVDATA())) )
  69   {
  70           DEBUG_PRINT_SDK("VP_Com : Failed to init com for navdata\n");
  71           vp_com_shutdown(COM_NAVDATA());
  72           res = C_FAIL;
  73   }
  74
  75   vp_com_network_adapter_lookup(COM_NAVDATA(), ardrone_toy_network_adapter_cb);
  76
  77   if( SUCCEED(res) && FAILED(vp_com_local_config(COM_NAVDATA(), COM_CONFIG_NAVDATA())) )
  78   {
  79           DEBUG_PRINT_SDK("VP_Com : Failed to configure com for navdata\n");
  80           vp_com_shutdown(COM_NAVDATA());
  81           res = C_FAIL;
  82   }
  83
  84   if( ssid != NULL )
  85   {
  86           strcpy( ((vp_com_wifi_connection_t*)wifi_connection())->networkName, ssid );
  87   }
  88
  89   if( SUCCEED(res) && FAILED(vp_com_connect(COM_NAVDATA(), COM_CONNECTION_NAVDATA(), NUM_ATTEMPTS)))
  90   {
  91           DEBUG_PRINT_SDK("VP_Com: Failed to connect for navdata\n");
  92           vp_com_shutdown(COM_NAVDATA());
  93           res = C_FAIL;
  94   }
  95 #else
  96   vp_com_init(COM_NAVDATA());
  97   vp_com_network_adapter_lookup(COM_NAVDATA(), ardrone_toy_network_adapter_cb);
  98   vp_com_local_config(COM_NAVDATA(), COM_CONFIG_NAVDATA());
  99
 100   if( ssid != NULL )
 101   {
 102           strcpy( ((vp_com_wifi_connection_t*)wifi_connection())->networkName, ssid );
 103   }
 104
 105   vp_com_connect(COM_NAVDATA(), COM_CONNECTION_NAVDATA(), NUM_ATTEMPTS);
 106   ((vp_com_wifi_connection_t*)wifi_connection())->is_up=1;
 107 #endif
 108
 109   return res;
 110 }
 111
 112 #ifdef NO_ARDRONE_MAINLOOP
 113 C_RESULT ardrone_tool_init( const char* ardrone_ip, size_t n, AT_CODEC_FUNCTIONS_PTRS *ptrs, const char *appname, const char *usrname)
 114 {
 115         // Initalize mutex and condition
 116         vp_os_mutex_init(&ardrone_tool_mutex);
 117         ardrone_tool_in_pause = FALSE;
 118
 119         // Initialize ardrone_control_config structures;
 120         ardrone_tool_reset_configuration();
 121         // ardrone_control_config_default initialisation. Sould not be modified after that !
 122         vp_os_memcpy ((void *)&ardrone_control_config_default, (const void *)&ardrone_control_config, sizeof (ardrone_control_config_default));
 123         // initialization of application defined default values
 124         vp_os_memcpy ((void *)&ardrone_application_default_config, (const void *)&ardrone_control_config, sizeof (ardrone_application_default_config));
 125
 126         //Fill structure AT codec and built the library AT commands.
 127    if( ptrs != NULL )
 128            ardrone_at_init_with_funcs( ardrone_ip, n, ptrs );
 129    else
 130       ardrone_at_init( ardrone_ip, n );
 131
 132         // Save appname/appid for reconnections
 133         if (NULL != appname)
 134         {
 135           ardrone_gen_appid (appname, __SDK_VERSION__, app_id, app_name, sizeof (app_name));
 136         }
 137         // Save usrname/usrid for reconnections
 138         if (NULL != usrname)
 139         {
 140                 ardrone_gen_usrid (usrname, usr_id, usr_name, sizeof (usr_name));
 141         }
 142         // Create pseudorandom session id
 143         ardrone_gen_sessionid (ses_id, ses_name, sizeof (ses_name));
 144
 145         // Init subsystems
 146         ardrone_timer_reset(&ardrone_tool_timer);
 147         ardrone_timer_update(&ardrone_tool_timer);
 148
 149         ardrone_tool_input_init();
 150         ardrone_control_init();
 151         ardrone_tool_configuration_init();
 152         ardrone_navdata_client_init();
 153
 154
 155    //Opens a connection to AT port.
 156         ardrone_at_open();
 157
 158         START_THREAD(navdata_update, 0);
 159         START_THREAD(ardrone_control, 0);
 160
 161         // Send start up configuration
 162         ardrone_at_set_pmode( MiscVar[0] );
 163         ardrone_at_set_ui_misc( MiscVar[0], MiscVar[1], MiscVar[2], MiscVar[3] );
 164
 165         return C_OK;
 166 }
 167 #else
 168 C_RESULT ardrone_tool_init(int argc, char **argv)
 169 {
 170         C_RESULT res;
 171         int32_t b_value = FALSE;
 172
 173         // Initalize mutex and condition
 174         vp_os_mutex_init(&ardrone_tool_mutex);
 175         ardrone_tool_in_pause = FALSE;
 176
 177         // Initialize ardrone_control_config structures;
 178         ardrone_tool_reset_configuration();
 179         // ardrone_control_config_default initialisation. Sould not be modified after that !
 180         vp_os_memcpy ((void *)&ardrone_control_config_default, (const void *)&ardrone_control_config, sizeof (ardrone_control_config_default));
 181         // initialization of application defined default values
 182         vp_os_memcpy ((void *)&ardrone_application_default_config, (const void *)&ardrone_control_config, sizeof (ardrone_application_default_config));
 183         ardrone_application_default_config.navdata_demo = b_value;
 184
 185         // Save appname/appid for reconnections
 186         if (NULL != argv[0])
 187         {
 188           char *appname = NULL;
 189           int lastSlashPos;
 190           /* Cut the invoking name to the last / or \ character on the command line
 191            * This avoids using differents app_id for applications called from different directories
 192            * e.g. if argv[0] is "Build/Release/ardrone_navigation", appname will point to "ardrone_navigation" only
 193            */
 194           for (lastSlashPos = strlen (argv[0])-1;
 195                lastSlashPos > 0 &&
 196                  argv[0][lastSlashPos] != '/' &&
 197                  argv[0][lastSlashPos] != '\\';
 198                lastSlashPos--);
 199           appname = &argv[0][lastSlashPos+1];
 200           ardrone_gen_appid (appname, __SDK_VERSION__, app_id, app_name, sizeof (app_name));
 201         }
 202
 203         // Create pseudorandom session id
 204         ardrone_gen_sessionid (ses_id, ses_name, sizeof (ses_name));
 205
 206         //Fill structure AT codec and built the library AT commands.
 207         ardrone_at_init( wifi_ardrone_ip, strlen( wifi_ardrone_ip) );
 208
 209         // Init subsystems
 210         ardrone_timer_reset(&ardrone_tool_timer);
 211         ardrone_timer_update(&ardrone_tool_timer);
 212
 213         ardrone_tool_input_init();
 214         ardrone_control_init();
 215         ardrone_tool_configuration_init();
 216         ardrone_navdata_client_init();
 217
 218         // Init custom tool
 219         res = ardrone_tool_init_custom(argc, argv);
 220
 221    //Opens a connection to AT port.
 222         ardrone_at_open();
 223
 224         START_THREAD(navdata_update, 0);
 225         START_THREAD(ardrone_control, 0);
 226
 227         // Send start up configuration
 228         ardrone_at_set_pmode( MiscVar[0] );
 229         ardrone_at_set_ui_misc( MiscVar[0], MiscVar[1], MiscVar[2], MiscVar[3] );
 230
 231         return res;
 232 }
 233 #endif
 234
 235 C_RESULT ardrone_tool_set_refresh_time(int refresh_time_in_ms)
 236 {
 237   ArdroneToolRefreshTimeInUs = refresh_time_in_ms * 1000;
 238
 239   return C_OK;
 240 }
 241
 242 C_RESULT ardrone_tool_pause( void )
 243 {
 244         ardrone_navdata_client_suspend();
 245
 246         vp_os_mutex_lock(&ardrone_tool_mutex);
 247         ardrone_tool_in_pause = TRUE;
 248         vp_os_mutex_unlock(&ardrone_tool_mutex);
 249
 250         return C_OK;
 251 }
 252
 253 C_RESULT ardrone_tool_resume( void )
 254 {
 255    ardrone_navdata_client_resume();
 256
 257         vp_os_mutex_lock(&ardrone_tool_mutex);
 258         ardrone_tool_in_pause = FALSE;
 259         vp_os_mutex_unlock(&ardrone_tool_mutex);
 260
 261    return C_OK;
 262 }
 263
 264 C_RESULT ardrone_tool_update()
 265 {
 266         int delta;
 267
 268         C_RESULT res = C_OK;
 269
 270         delta = ardrone_timer_delta_us(&ardrone_tool_timer);
 271         if( delta >= ArdroneToolRefreshTimeInUs)
 272         {
 273                 // Render frame
 274                 ardrone_timer_update(&ardrone_tool_timer);
 275
 276                 if(!ardrone_tool_in_pause)
 277                 {
 278                         ardrone_tool_input_update();
 279                         res = ardrone_tool_update_custom();
 280                 }
 281
 282                 if( send_com_watchdog == TRUE )
 283                 {
 284                         ardrone_at_reset_com_watchdog();
 285                         send_com_watchdog = FALSE;
 286                 }
 287
 288                 // Send all pushed messages
 289                 ardrone_at_send();
 290
 291                 res = ardrone_tool_display_custom();
 292         }
 293         else
 294         {
 295                 usleep(ArdroneToolRefreshTimeInUs - delta);
 296         }
 297
 298         return res;
 299 }
 300
 301 C_RESULT ardrone_tool_shutdown()
 302 {
 303   C_RESULT res = C_OK;
 304
 305 #ifndef NO_ARDRONE_MAINLOOP
 306   res = ardrone_tool_shutdown_custom();
 307 #endif
 308
 309   // Shutdown subsystems
 310   ardrone_navdata_client_shutdown();
 311   ardrone_control_shutdown();
 312   ardrone_tool_input_shutdown();
 313
 314   JOIN_THREAD(ardrone_control);
 315   JOIN_THREAD(navdata_update);
 316
 317   // Shutdown AT Commands
 318   ATcodec_exit_thread();
 319   ATcodec_Shutdown_Library();
 320
 321   vp_com_disconnect(COM_NAVDATA());
 322   vp_com_shutdown(COM_NAVDATA());
 323
 324   PRINT("Custom ardrone tool ended\n");
 325
 326   return res;
 327 }
 328 #ifndef NO_ARDRONE_MAINLOOP
 329
 330 #include <locale.h>
 331
 332 int main(int argc, char **argv)
 333 {
 334   C_RESULT res;
 335   const char* old_locale;
 336   const char* appname = argv[0];
 337   int argc_backup = argc;
 338   char** argv_backup = argv;
 339
 340   bool_t show_usage = FAILED( ardrone_tool_check_argc_custom(argc) ) ? TRUE : FALSE;
 341
 342   argc--; argv++;
 343   while( argc && *argv[0] == '-' )
 344   {
 345     if( !strcmp(*argv, "-?") || !strcmp(*argv, "-h") || !strcmp(*argv, "-help") || !strcmp(*argv, "--help") )
 346     {
 347       ardrone_tool_usage( appname );
 348       exit( 0 );
 349     }
 350     else if( !ardrone_tool_parse_cmd_line_custom( *argv ) )
 351     {
 352       printf("Option %s not recognized\n", *argv);
 353       show_usage = TRUE;
 354     }
 355
 356     argc--; argv++;
 357   }
 358
 359   if( show_usage || (argc != 0) )
 360   {
 361     ardrone_tool_usage( appname );
 362     exit(-1);
 363   }
 364
 365   /* After a first analysis, the arguments are restored so they can be passed to the user-defined functions */
 366   argc=argc_backup;
 367   argv=argv_backup;
 368
 369   old_locale = setlocale(LC_NUMERIC, "en_GB.UTF-8");
 370
 371   if( old_locale == NULL )
 372   {
 373     PRINT("You have to install new locales in your dev environment! (avoid the need of conv_coma_to_dot)\n");
 374     PRINT("As root, do a \"dpkg-reconfigure locales\" and add en_GB.UTF8 to your locale settings\n");
 375     PRINT("If you have any problem, feel free to contact Pierre Eline (pierre.eline@parrot.com)\n");
 376   }
 377   else
 378   {
 379     PRINT("Setting locale to %s\n", old_locale);
 380   }
 381
 382   vp_com_wifi_config_t *config = (vp_com_wifi_config_t*)wifi_config();
 383   if(config)
 384   {
 385           vp_os_memset( &wifi_ardrone_ip[0], 0, sizeof(wifi_ardrone_ip) );
 386           printf("===================+> %s\n", config->server);
 387           strcpy( &wifi_ardrone_ip[0], config->server);
 388   }
 389
 390   if( &custom_main )
 391   {
 392     return custom_main(argc, argv);
 393   }
 394   else
 395   {
 396         res = ardrone_tool_setup_com( NULL );
 397
 398     if( FAILED(res) )
 399     {
 400       PRINT("Wifi initialization failed. It means either:\n");
 401       PRINT("\t* you're not root (it's mandatory because you can set up wifi connection only as root)\n");
 402       PRINT("\t* wifi device is not present (on your pc or on your card)\n");
 403       PRINT("\t* you set the wrong name for wifi interface (for example rausb0 instead of wlan0) \n");
 404       PRINT("\t* ap is not up (reboot card or remove wifi usb dongle)\n");
 405       PRINT("\t* wifi device has no antenna\n");
 406     }
 407     else
 408     {
 409       res = ardrone_tool_init(argc, argv);
 410
 411       while( SUCCEED(res) && ardrone_tool_exit() == FALSE )
 412       {
 413         res = ardrone_tool_update();
 414       }
 415
 416       res = ardrone_tool_shutdown();
 417     }
 418   }
 419
 420   if( old_locale != NULL )
 421   {
 422     setlocale(LC_NUMERIC, old_locale);
 423   }
 424
 425   return SUCCEED(res) ? 0 : -1;
 426 }
 427 #endif // ! WITH_ARDRONE_MAIN_LOOP
 428
 429 // Default implementation for weak functions
 430 #ifndef _WIN32
 431         C_RESULT ardrone_tool_init_custom(int argc, char **argv) { return C_OK; }
 432         C_RESULT ardrone_tool_update_custom() { return C_OK; }
 433         C_RESULT ardrone_tool_display_custom() { return C_OK; }
 434         C_RESULT ardrone_tool_shutdown_custom() { return C_OK; }
 435         bool_t   ardrone_tool_exit() { return FALSE; }
 436         C_RESULT ardrone_tool_check_argc_custom( int32_t argc) { return C_OK; }
 437         void ardrone_tool_display_cmd_line_custom( void ) {}
 438         bool_t ardrone_tool_parse_cmd_line_custom( const char* cmd ) { return TRUE; }
 439 #endif
 440
 441