vcs.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/Examples/Win32/sdk_demo/Sources/ardrone_tool_win32.c

   1 /********************************************************************
   2  *                    COPYRIGHT PARROT 2010
   3  ********************************************************************
   4  *       PARROT - A.R.Drone SDK Windows Client Example
   5  *-----------------------------------------------------------------*/
   6 /**
   7  * @file ardrone_tool_win32.c
   8  * @brief Main program of all client programs using ARDroneTool.
   9  *
  10  * @author Stephane Piskorski <stephane.piskorski.ext@parrot.fr>
  11  * @date   Sept, 8. 2010
  12  * @warning  This file is a modified version of the 'ardrone_tool.c'
  13  *                       of the A.R. Drone SDK
  14  *
  15  *******************************************************************/
  16
  17
  18 #pragma warning( disable : 4996 ) // disable deprecation warning
  19
  20 #include <stdlib.h>
  21
  22 #include <VP_Os/vp_os_malloc.h>
  23 #include <VP_Os/vp_os_print.h>
  24 #include <VP_Api/vp_api_thread_helper.h>
  25
  26 #include <VP_Com/vp_com.h>
  27
  28 #include <ardrone_tool/ardrone_tool.h>
  29 #include <ardrone_tool/ardrone_time.h>
  30 #include <ardrone_tool/Control/ardrone_control.h>
  31 #include <ardrone_tool/Control/ardrone_control_ack.h>
  32 #include <ardrone_tool/Navdata/ardrone_navdata_client.h>
  33 #include <ardrone_tool/UI/ardrone_input.h>
  34 #include <ardrone_tool/Com/config_com.h>
  35
  36
  37 int32_t MiscVar[NB_MISC_VARS] = {
  38                DEFAULT_MISC1_VALUE,
  39                DEFAULT_MISC2_VALUE,
  40                DEFAULT_MISC3_VALUE,
  41                DEFAULT_MISC4_VALUE
  42                                 };
  43
  44 static bool_t need_update   = TRUE;
  45 static ardrone_timer_t ardrone_tool_timer;
  46 static int ArdroneToolRefreshTimeInMs = ARDRONE_REFRESH_MS;
  47
  48 /* Those should be defined in ArDroneLib, but there are compilation issues to solve */
  49 unsigned long bswap(unsigned long x) { return _byteswap_ulong(x); }
  50 /*
  51 #include <cmnintrin.h>
  52 int clz(unsigned long x) { return _CountLeadingZeros(x); }
  53 */
  54
  55 int clz(unsigned long x)
  56 {
  57         /* Barbarian counting method if no instrinsic is available */
  58         int i; const int L=sizeof(x)*8-1;
  59         const unsigned long mask = ( 1 << L );
  60         if (x==0) { return L+1; }
  61         for (i=0;i<L;i++) { if (x&mask) return i; x<<=1; }
  62         return i;
  63 }
  64
  65
  66 char wifi_ardrone_ip[256] = { WIFI_ARDRONE_IP };
  67
  68
  69 /*---------------------------------------------------------------------------------------------------------------------
  70 Array containing parameters to be sent to the drone before starting flight.
  71 Those parameters are sent one by one with the use of an acknowledgment to make sure they
  72 were properly set on the drone.
  73 ---------------------------------------------------------------------------------------------------------------------*/
  74 /// Remote data configuration ///
  75 ardrone_tool_configure_data_t configure_data[] = {
  76   { "general:navdata_demo", "FALSE" },
  77   { NULL, NULL }
  78 };
  79
  80
  81
  82 static int32_t configure_index = 0;
  83 static ardrone_control_ack_event_t ack_config;
  84 static bool_t send_com_watchdog = FALSE;
  85
  86
  87
  88 /*---------------------------------------------------------------------------------------------------------------------
  89 Makes ardrone tool send a keepalive command to the drone, when the drone detects inactivity for too long.
  90 ---------------------------------------------------------------------------------------------------------------------*/
  91 void ardrone_tool_send_com_watchdog( void )
  92 {
  93   send_com_watchdog = TRUE;
  94 }
  95
  96
  97 /*---------------------------------------------------------------------------------------------------------------------
  98 Callback function used by 'ardrone_tool_configure' and by itself.
  99 This function uses the drone control socket and its acknowlegment system to securely
 100 set some parameters on the drone.
 101 The parameters to set are stored in the 'configure_data' array.
 102 ---------------------------------------------------------------------------------------------------------------------*/
 103
 104 static void ardrone_tool_end_configure( struct _ardrone_control_event_t* event )
 105 {
 106   if( event->status == ARDRONE_CONTROL_EVENT_FINISH_SUCCESS )
 107     configure_index ++;
 108
 109   if( configure_data[configure_index].var != NULL && configure_data[configure_index].value != NULL )
 110   {
 111     ack_config.event                        = ACK_CONTROL_MODE;
 112     ack_config.num_retries                  = 20;
 113     ack_config.status                       = ARDRONE_CONTROL_EVENT_WAITING;
 114     ack_config.ardrone_control_event_start  = NULL;
 115     ack_config.ardrone_control_event_end    = ardrone_tool_end_configure;
 116     ack_config.ack_state                    = ACK_COMMAND_MASK_TRUE;
 117
 118     ardrone_at_set_toy_configuration( configure_data[configure_index].var, configure_data[configure_index].value );
 119     ardrone_at_send();
 120
 121     ardrone_control_send_event( (ardrone_control_event_t*)&ack_config );
 122   }
 123 }
 124
 125
 126 /*---------------------------------------------------------------------------------------------------------------------
 127 See 'ardrone_tool_end_configure'
 128 ---------------------------------------------------------------------------------------------------------------------*/
 129
 130 static C_RESULT ardrone_tool_configure()
 131 {
 132   if( configure_data[configure_index].var != NULL && configure_data[configure_index].value != NULL )
 133   {
 134     ack_config.event                        = ACK_CONTROL_MODE;
 135     ack_config.num_retries                  = 20;
 136     ack_config.status                       = ARDRONE_CONTROL_EVENT_WAITING;
 137     ack_config.ardrone_control_event_start  = NULL;
 138     ack_config.ardrone_control_event_end    = ardrone_tool_end_configure;
 139     ack_config.ack_state                    = ACK_COMMAND_MASK_TRUE;
 140
 141     ardrone_at_set_toy_configuration( configure_data[configure_index].var, configure_data[configure_index].value );
 142     ardrone_at_send();
 143
 144     ardrone_control_send_event( (ardrone_control_event_t*)&ack_config );
 145   }
 146
 147   return C_OK;
 148 }
 149
 150
 151 /*---------------------------------------------------------------------------------------------------------------------
 152 ---------------------------------------------------------------------------------------------------------------------*/
 153 static void ardrone_toy_network_adapter_cb( const char* name )
 154 {
 155   strcpy( COM_CONFIG_NAVDATA()->itfName, name );
 156 }
 157
 158
 159 /*---------------------------------------------------------------------------------------------------------------------
 160 ---------------------------------------------------------------------------------------------------------------------*/
 161 C_RESULT ardrone_tool_setup_com( const char* ssid )
 162 {
 163   C_RESULT res = C_OK;
 164
 165   vp_com_init(COM_NAVDATA());
 166   vp_com_local_config(COM_NAVDATA(), COM_CONFIG_NAVDATA());
 167   vp_com_connect(COM_NAVDATA(), COM_CONNECTION_NAVDATA(), NUM_ATTEMPTS);
 168   ((vp_com_wifi_connection_t*)wifi_connection())->is_up=1;
 169   return res;
 170 }
 171
 172
 173 /*---------------------------------------------------------------------------------------------------------------------
 174 ---------------------------------------------------------------------------------------------------------------------*/
 175 C_RESULT ardrone_tool_init(int argc, char **argv)
 176 {
 177         C_RESULT res;
 178
 179         //Fill structure AT codec and built the library AT commands.
 180         ardrone_at_init( wifi_ardrone_ip, strlen( wifi_ardrone_ip) );
 181
 182         // Init subsystems
 183         ardrone_timer_reset(&ardrone_tool_timer);
 184
 185         ardrone_tool_input_init();
 186         ardrone_control_init();
 187         ardrone_navdata_client_init();
 188
 189         // Init custom tool
 190         res = ardrone_tool_init_custom(argc, argv);
 191
 192    //Opens a connection to AT port.
 193         ardrone_at_open();
 194
 195
 196         START_THREAD(navdata_update, 0);
 197         START_THREAD(ardrone_control, 0);
 198
 199         ardrone_tool_configure();
 200
 201         // Send start up configuration
 202         ardrone_at_set_pmode( MiscVar[0] );
 203         ardrone_at_set_ui_misc( MiscVar[0], MiscVar[1], MiscVar[2], MiscVar[3] );
 204
 205         return res;
 206 }
 207
 208
 209 C_RESULT ardrone_tool_set_refresh_time(int refresh_time_in_ms)
 210 {
 211   ArdroneToolRefreshTimeInMs = refresh_time_in_ms;
 212
 213   return C_OK;
 214 }
 215
 216 C_RESULT ardrone_tool_pause( void )
 217 {
 218    ardrone_navdata_client_suspend();
 219
 220    return C_OK;
 221 }
 222
 223 C_RESULT ardrone_tool_resume( void )
 224 {
 225    ardrone_navdata_client_resume();
 226
 227    return C_OK;
 228 }
 229
 230
 231
 232 /*---------------------------------------------------------------------------------------------------------------------
 233 Updates the AT command client by flushing pending commands.
 234 ---------------------------------------------------------------------------------------------------------------------*/
 235 C_RESULT ardrone_tool_update()
 236 {
 237         int delta;
 238
 239         C_RESULT res = C_OK;
 240
 241         // Update subsystems & custom tool
 242         if( need_update )
 243         {
 244                 ardrone_timer_update(&ardrone_tool_timer);
 245
 246                 ardrone_tool_input_update();
 247                 res = ardrone_tool_update_custom();
 248
 249                 if( send_com_watchdog == TRUE )
 250                 {
 251                         ardrone_at_reset_com_watchdog();
 252                         send_com_watchdog = FALSE;
 253                 }
 254                 // Send all pushed messages
 255                 ardrone_at_send();
 256
 257                 need_update = FALSE;
 258         }
 259
 260         delta = ardrone_timer_delta_ms(&ardrone_tool_timer);
 261         if( delta >= ArdroneToolRefreshTimeInMs)
 262         {
 263                 // Render frame
 264                 res = ardrone_tool_display_custom();
 265                 need_update = TRUE;
 266         }
 267         else
 268         {
 269                 Sleep((ArdroneToolRefreshTimeInMs - delta));
 270         }
 271
 272         return res;
 273 }
 274
 275
 276
 277
 278 /*---------------------------------------------------------------------------------------------------------------------
 279 Stops the drone controlling subsystem.
 280 ---------------------------------------------------------------------------------------------------------------------*/
 281 C_RESULT ardrone_tool_shutdown()
 282 {
 283   C_RESULT res = C_OK;
 284
 285 #ifndef NO_ARDRONE_MAINLOOP
 286   res = ardrone_tool_shutdown_custom();
 287 #endif
 288
 289   // Shutdown subsystems
 290   ardrone_navdata_client_shutdown();
 291   ardrone_control_shutdown();
 292   ardrone_tool_input_shutdown();
 293
 294   JOIN_THREAD(ardrone_control);
 295   JOIN_THREAD(navdata_update);
 296
 297   // Shutdown AT Commands
 298   ATcodec_exit_thread();
 299   ATcodec_Shutdown_Library();
 300
 301   vp_com_disconnect(COM_NAVDATA());
 302   vp_com_shutdown(COM_NAVDATA());
 303
 304   PRINT("Custom ardrone tool ended\n");
 305
 306   return res;
 307 }
 308
 309
 310
 311 /*---------------------------------------------------------------------------------------------------------------------
 312 Tests the network connection to the drone by fetching the drone version number
 313 through the FTP server embedded on the drone.
 314 This is how FreeFlight checks if a drone sofware update is required.
 315
 316 The FTP connection process is a quick and (very)dirty one. It uses FTP passive mode.
 317 ---------------------------------------------------------------------------------------------------------------------*/
 318 int test_drone_connection()
 319 {
 320         const char * passivdeModeHeader = "\r\n227 PASV ok (";
 321         vp_com_socket_t ftp_client,ftp_client2;
 322         char buffer[1024];
 323         static Write ftp_write = NULL;
 324         static Read  ftp_read = NULL;
 325         int bytes_to_send,received_bytes;
 326         int i,L,x[6],port;
 327         int timeout_windows = 1000; /*milliseconds*/
 328
 329         vp_os_memset(buffer,0,sizeof(buffer));
 330
 331         /* Connects to the FTP server */
 332                 wifi_config_socket(&ftp_client,VP_COM_CLIENT,FTP_PORT,WIFI_ARDRONE_IP);
 333                 ftp_client.protocol = VP_COM_TCP;
 334                 if(VP_FAILED(vp_com_init(wifi_com()))) return -1;
 335                 if(VP_FAILED(vp_com_open(wifi_com(), &ftp_client, &ftp_read, &ftp_write))) return -2;
 336                 setsockopt((int32_t)ftp_client.priv,
 337                                                                 SOL_SOCKET,
 338                                                                 SO_RCVTIMEO,
 339                                                                 (const char*)&timeout_windows, sizeof(timeout_windows)
 340                                                                 );
 341
 342         /* Request version file */
 343                 bytes_to_send = _snprintf(buffer,sizeof(buffer),"%s",\r                  "USER anonymous\r\nCWD /\r\nPWD\r\nTYPE A\r\nPASV\r\nRETR version.txt\r\n");\r           ftp_write(&ftp_client,buffer,&bytes_to_send);
 344                 /* Dirty. We should wait for data to arrive with some kind of synchronization
 345                 or make the socket blocking.*/
 346                 Sleep(1000);
 347
 348         /* Gets the data port */
 349                 received_bytes = sizeof(buffer);
 350                 ftp_read(&ftp_client,buffer,&received_bytes);
 351                 if (received_bytes<1) { vp_com_close(wifi_com(), &ftp_client); return -3; }
 352                 L=received_bytes-strlen(passivdeModeHeader);
 353
 354         /* Searches for the passive mode acknowlegment from the FTP server */
 355                 for (i=0;i<L;i++) {
 356                         if (strncmp((buffer+i),passivdeModeHeader,strlen(passivdeModeHeader))==0)  break;
 357                 }
 358                 if (i==L) {
 359                         vp_com_close(wifi_com(), &ftp_client); return -4;
 360                 }
 361                 i+=strlen(passivdeModeHeader);
 362                 if (sscanf(buffer+i,"%i,%i,%i,%i,%i,%i)",&x[0],&x[1],&x[2],&x[3],&x[4],&x[5])!=6)
 363                         { vp_com_close(wifi_com(), &ftp_client); return -5; }
 364                 port=(x[4]<<8)+x[5];
 365
 366         /* Connects to the FTP server data port */
 367                 wifi_config_socket(&ftp_client2,VP_COM_CLIENT,port,"192.168.1.1");
 368                 ftp_client2.protocol = VP_COM_TCP;
 369                 if(VP_FAILED(vp_com_init(wifi_com())))
 370                                 { vp_com_close(wifi_com(), &ftp_client2); return -6; }
 371                 if(VP_FAILED(vp_com_open(wifi_com(), &ftp_client2, &ftp_read, &ftp_write)))
 372                         { vp_com_close(wifi_com(), &ftp_client2); return -7; }
 373
 374         /* Gets the data */
 375                 received_bytes = sizeof(buffer);
 376                 ftp_read(&ftp_client2,buffer,&received_bytes);
 377                 if (received_bytes>0) {
 378                         buffer[min(received_bytes,sizeof(buffer)-1)]=0;
 379                         printf("Drone version %s detected ... press <Enter> to start the application.\n",buffer);
 380                         getchar();
 381                 }
 382
 383         /* Clean up */
 384                 vp_com_close(wifi_com(), &ftp_client);
 385                 vp_com_close(wifi_com(), &ftp_client2);
 386
 387         return 0;
 388 }
 389
 390
 391
 392
 393 /*---------------------------------------------------------------------------------------------------------------------
 394 Main application function
 395 ---------------------------------------------------------------------------------------------------------------------*/
 396
 397 int sdk_demo_stop=0;
 398
 399 int main(int argc, char **argv)
 400 {
 401           C_RESULT res;                                 // functions return value
 402           //const char* old_locale=NULL;
 403           const char* appname = argv[0];
 404           //int argc_backup = argc;
 405           //char** argv_backup = argv;
 406
 407           WSADATA wsaData = {0};
 408           int iResult = 0;
 409
 410
 411         /* Initializes Windows socket subsystem */
 412                 iResult = WSAStartup(MAKEWORD(2, 2), &wsaData);
 413                 if (iResult != 0) {     wprintf(L"WSAStartup failed: %d\n", iResult);   return 1;       }
 414
 415         /* Includes the Pthread for Win32 Library if necessary */
 416                 #include <VP_Os/vp_os_signal.h>
 417                         #if defined USE_PTHREAD_FOR_WIN32
 418                         #pragma comment (lib,"pthreadVC2.lib")
 419                         #endif
 420
 421         /*      Initializes communication sockets       */
 422                 res = test_drone_connection();
 423                 if(res!=0){
 424                         printf("%s","Could not detect the drone version ... press <Enter> to try connecting anyway.\n");
 425                         getchar();
 426                         //WSACleanup(); exit(-1);
 427                 }
 428
 429                 res = ardrone_tool_setup_com( NULL );
 430                 if( FAILED(res) ){  PRINT("Wifi initialization failed.\n");  return -1; }
 431
 432         /* Initialises ARDroneTool */
 433            res = ardrone_tool_init(argc, argv);
 434
 435    /* Keeps sending AT commands to control the drone as long as
 436                 everything is OK */
 437       while( VP_SUCCEEDED(res) && ardrone_tool_exit() == FALSE ) {
 438         res = ardrone_tool_update();     }
 439
 440    /**/
 441       res = ardrone_tool_shutdown();
 442
 443           WSACleanup();
 444
 445   /* Bye bye */
 446         system("cls");
 447           printf("End of SDK Demo for Windows\n");
 448           getchar();
 449           return VP_SUCCEEDED(res) ? 0 : -1;
 450 }
 451
 452
 453
 454
 455 // Default implementation for weak functions
 456         C_RESULT ardrone_tool_update_custom() { return C_OK; }
 457         C_RESULT ardrone_tool_display_custom() { return C_OK; }
 458         C_RESULT ardrone_tool_check_argc_custom( int32_t argc) { return C_OK; }
 459         void ardrone_tool_display_cmd_line_custom( void ) {}
 460         bool_t ardrone_tool_parse_cmd_line_custom( const char* cmd ) { return TRUE; }
 461
 462