/*---------------------------------------------------------------------------------------------------------------------
Tests the network connection to the drone by fetching the drone version number
through the FTP server embedded on the drone.
This is how FreeFlight checks if a drone sofware update is required.

The FTP connection process is a quick and (very)dirty one. It uses FTP passive mode.
---------------------------------------------------------------------------------------------------------------------*/
int test_drone_connection()
{
	vp_com_socket_t ftp_client;
	static Write ftp_write = NULL;
	static Read  ftp_read = NULL;
	int timeout_windows = 1000; /*milliseconds*/

	/* Connects to the FTP server */
	wifi_config_socket(&ftp_client,VP_COM_CLIENT,FTP_PORT,WIFI_ARDRONE_IP);
	ftp_client.protocol = VP_COM_TCP;
	if(VP_FAILED(vp_com_init(wifi_com())))
		return -1;
	if(VP_FAILED(vp_com_open(wifi_com(), &ftp_client, &ftp_read, &ftp_write)))
		return -2;
	setsockopt((int32_t)ftp_client.priv, 
								SOL_SOCKET, 
								SO_RCVTIMEO, 
								(const char*)&timeout_windows, sizeof(timeout_windows)
								);

	/* Clean up */
	vp_com_close(wifi_com(), &ftp_client);

	return 0;
}
Exemplo n.º 2
0
void videoServer_destroy() {
	uint32_t iterator;
	TClient *curClient;

	/* Do not accept more clients */
	vp_com_close(&globalCom, &videoServerSocket);

	/* Close all client sockets */
	if (videoServer_clientList != NULL) {
		clientList_startIterator(&iterator);
		while((curClient = clientList_getNextClient(videoServer_clientList, &iterator)) != NULL) {
			closeClient(curClient);
		}
		clientList_destroy(videoServer_clientList);
		videoServer_clientList = NULL;
	}

	/* Free resources */
	vp_os_cond_destroy(&frameBufferCond);
	vp_os_mutex_destroy(&frameBufferMutex);
	vp_os_free(frameBuffer);

	videoTranscoder_destroy();
	vp_os_mutex_destroy(&settingsMutex);

	videoServerStarted = FALSE;
}
C_RESULT
vp_stages_output_com_stage_close(vp_stages_output_com_config_t *cfg)
{
  // \todo Faire ca dans le transform en detectant la fin d'une connection
  vp_com_close(cfg->com, &cfg->socket_client);

  if(cfg->socket.type == VP_COM_SERVER)
    vp_com_close(cfg->com, &cfg->socket);

  // \todo test
  if(cfg->connection && cfg->connection->is_up)
     vp_com_disconnect(cfg->com);

  vp_com_shutdown(cfg->com);

  return (VP_SUCCESS);
}
Exemplo n.º 4
0
AT_CODEC_ERROR_CODE host_close( void )
{
   if( func_ptrs.close != NULL )
      return func_ptrs.close();

  vp_com_close(COM_AT(), &at_socket);

  return AT_CODEC_CLOSE_OK;
}
/*---------------------------------------------------------------------------------------------------------------------
Tests the network connection to the drone by fetching the drone version number
through the FTP server embedded on the drone.
This is how FreeFlight checks if a drone sofware update is required.

The FTP connection process is a quick and (very)dirty one. It uses FTP passive mode.
---------------------------------------------------------------------------------------------------------------------*/
int test_drone_connection()
{
	const char * passivdeModeHeader = "\r\n227 PASV ok (";
	vp_com_socket_t ftp_client,ftp_client2;
	char buffer[1024];
	static Write ftp_write = NULL;
	static Read  ftp_read = NULL;
	int bytes_to_send,received_bytes;
	int i,L,x[6],port;
	
	vp_os_memset(buffer,0,sizeof(buffer));

	/* Connects to the FTP server */
		wifi_config_socket(&ftp_client,VP_COM_CLIENT,FTP_PORT,WIFI_ARDRONE_IP);
		ftp_client.protocol = VP_COM_TCP;
		if(VP_FAILED(vp_com_init(wifi_com()))) return -1;
		if(VP_FAILED(vp_com_open(wifi_com(), &ftp_client, &ftp_read, &ftp_write))) return -2;

	/* Request version file */
		bytes_to_send = _snprintf(buffer,sizeof(buffer),"%s",
			"USER anonymous\r\nCWD /\r\nPWD\r\nTYPE A\r\nPASV\r\nRETR version.txt\r\n");
		ftp_write(&ftp_client, (const int8_t*)buffer,&bytes_to_send);
		/* Dirty. We should wait for data to arrive with some kind of synchronization
		or make the socket blocking.*/
		Sleep(1000);

	/* Gets the data port */
		received_bytes = sizeof(buffer);
		ftp_read(&ftp_client,(int8_t*)buffer,&received_bytes);
		if (received_bytes<1) { vp_com_close(wifi_com(), &ftp_client); return -3; }
		L=received_bytes-strlen(passivdeModeHeader);

	/* Searches for the passive mode acknowlegment from the FTP server */
		for (i=0;i<L;i++) {
			if (strncmp((buffer+i),passivdeModeHeader,strlen(passivdeModeHeader))==0)  break; 
		}
		if (i==L) {
			vp_com_close(wifi_com(), &ftp_client); return -4; 
		}
		i+=strlen(passivdeModeHeader);
		if (sscanf(buffer+i,"%i,%i,%i,%i,%i,%i)",&x[0],&x[1],&x[2],&x[3],&x[4],&x[5])!=6)
			{ vp_com_close(wifi_com(), &ftp_client); return -5; }
		port=(x[4]<<8)+x[5];

	/* Connects to the FTP server data port */
		wifi_config_socket(&ftp_client2,VP_COM_CLIENT,port,"192.168.1.1");
		ftp_client2.protocol = VP_COM_TCP;
		if(VP_FAILED(vp_com_init(wifi_com()))) 
				{ vp_com_close(wifi_com(), &ftp_client2); return -6; }
		if(VP_FAILED(vp_com_open(wifi_com(), &ftp_client2, &ftp_read, &ftp_write)))
			{ vp_com_close(wifi_com(), &ftp_client2); return -7; }

	/* Clean up */
		vp_com_close(wifi_com(), &ftp_client);
		vp_com_close(wifi_com(), &ftp_client2);

	return 0;
}
C_RESULT video_com_multisocket_stage_close(video_com_multisocket_config_t *cfg)
{
  int i;

  for (i=0;i<cfg->nb_sockets;i++)
    {
      vp_com_close(cfg->configs[i]->com, &cfg->configs[i]->socket);
    }

  return C_OK;
}
C_RESULT ardrone_control_connect_to_drone()
{
	int res_open_socket;

#ifdef _WIN32
	int timeout_windows=1000;/*milliseconds*/
#endif

	struct timeval tv;

	vp_com_close(COM_CONTROL(), &control_socket);

	res_open_socket = vp_com_open(COM_CONTROL(), &control_socket, &control_read, &control_write);
	if( VP_SUCCEEDED(res_open_socket) )
	{
		tv.tv_sec   = 1;
		tv.tv_usec  = 0;

		setsockopt((int32_t)control_socket.priv,
					SOL_SOCKET,
					SO_RCVTIMEO,
					#ifdef _WIN32
						(const char*)&timeout_windows, sizeof(timeout_windows)
					#else
						(const char*)&tv, sizeof(tv)
					#endif
					);

		control_socket.is_disable = FALSE;
		return C_OK;
	}
	else
	{
		DEBUG_PRINT_SDK("VP_Com : Failed to open socket for control\n");
		perror("FTOSFC");
		return C_FAIL;
	}
}
C_RESULT video_com_stage_connect (video_com_config_t *cfg)
{
#ifdef _WIN32
  int timeout_for_windows=1000; /* timeout in milliseconds */
  int sizeinit;
#else
  struct timeval tv;
  // 1 second timeout
  tv.tv_sec   = 1;
  tv.tv_usec  = 0;
#endif

  C_RESULT res = C_FAIL;
	
  if (TRUE == cfg->connected)
    {
      PDBG ("Will close");
      res = vp_com_close (cfg->com, &cfg->socket);
      cfg->connected = FALSE;
      if (VP_FAILED (res))
        {
          PDBG ("Close failed");
          return res;
        }
    }

  if( cfg->protocol == VP_COM_PROBE)
    {
      PRINT("\n\nPROBING\n");

      cfg->socket.protocol = VP_COM_TCP;
      res = vp_com_open(cfg->com, &cfg->socket, &cfg->read, &cfg->write);

      if( VP_SUCCEEDED(res) )
        {
          PRINT("\n\nTCP\n");
          vp_com_close (cfg->com, &cfg->socket);
          cfg->protocol = VP_COM_TCP;
        }
      else
        {
          PRINT("\n\nUDP\n");
          cfg->protocol = VP_COM_UDP;
        }
    }


  if( cfg->protocol == VP_COM_UDP )
    {
      cfg->socket.protocol = VP_COM_UDP;
      cfg->socket.is_multicast = 0; // disable multicast for video
      cfg->socket.multicast_base_addr = MULTICAST_BASE_ADDR;

      res = vp_com_open(cfg->com, &cfg->socket, &cfg->read, &cfg->write);

      if( VP_SUCCEEDED(res) )
    	{
          int numi= 1;
          socklen_t numi1= sizeof(int);
#ifdef _WIN32
          setsockopt((int32_t)cfg->socket.priv, SOL_SOCKET, SO_RCVTIMEO, SSOPTCAST_RO(&timeout_for_windows), sizeof(timeout_for_windows));
#else
          setsockopt((int32_t)cfg->socket.priv, SOL_SOCKET, SO_RCVTIMEO, SSOPTCAST_RO(&tv), sizeof(tv));
#endif		
          // Increase buffer for receiving datas.
          setsockopt( (int32_t)cfg->socket.priv, SOL_SOCKET, SO_DEBUG, SSOPTCAST_RO(&numi), sizeof(numi));
          numi = SOCKET_BUFFER_SIZE;
          setsockopt( (int32_t)cfg->socket.priv, SOL_SOCKET, SO_RCVBUF, SSOPTCAST_RO(&numi),numi1);
          getsockopt( (int32_t)cfg->socket.priv, SOL_SOCKET, SO_RCVBUF, SSOPTCAST_RW(&numi),&numi1);
          PDBG ("New buffer size : %d", numi);
          numi1 = 0;
          setsockopt( (int32_t)cfg->socket.priv, SOL_SOCKET, SO_DEBUG, SSOPTCAST_RO(&numi1), sizeof(numi1));
          cfg->connected = TRUE;
    	}
    }
  else if( cfg->protocol == VP_COM_TCP )
    {
      PDBG ("Will open TCP");
      res = vp_com_open(cfg->com, &cfg->socket, &cfg->read, &cfg->write);

      if( VP_SUCCEEDED(res) )
    	{
          int numi= 1;
          socklen_t numi1= sizeof(int);
          PDBG ("Success open");
          vp_com_sockopt(cfg->com, &cfg->socket, cfg->sockopt);
#ifdef _WIN32
          setsockopt((int32_t)cfg->socket.priv, SOL_SOCKET, SO_RCVTIMEO, SSOPTCAST_RO(&timeout_for_windows), sizeof(timeout_for_windows));
#else
          setsockopt((int32_t)cfg->socket.priv, SOL_SOCKET, SO_RCVTIMEO, SSOPTCAST_RO(&tv), sizeof(tv));
#endif		
          // Increase buffer for receiving datas.
          setsockopt( (int32_t)cfg->socket.priv, SOL_SOCKET, SO_DEBUG, SSOPTCAST_RO(&numi), sizeof(numi));
          numi = SOCKET_BUFFER_SIZE;
          setsockopt( (int32_t)cfg->socket.priv, SOL_SOCKET, SO_RCVBUF, SSOPTCAST_RO(&numi),numi1);
          getsockopt( (int32_t)cfg->socket.priv, SOL_SOCKET, SO_RCVBUF, SSOPTCAST_RW(&numi),&numi1);
          PDBG ("NEW buffer size : %d", numi);
          numi1 = 0;
          setsockopt( (int32_t)cfg->socket.priv, SOL_SOCKET, SO_DEBUG, SSOPTCAST_RO(&numi1), sizeof(numi1));
          cfg->connected = TRUE;
    	}
    }


#ifdef _WIN32
  sizeinit = strlen("Init");
  vp_com_write_socket(&cfg->socket,"Init",&sizeinit);
#endif
  return res;
}
C_RESULT video_com_stage_close(video_com_config_t *cfg)
{
  vp_com_close(cfg->com, &cfg->socket);
  return C_OK;
}
Exemplo n.º 10
0
int test_drone_connection()
{
    const char * passivdeModeHeader = "\r\n227 PASV ok (";
    vp_com_socket_t ftp_client,ftp_client2;
    char buffer[1024];
    static Write ftp_write = NULL;
    static Read  ftp_read = NULL;
    int bytes_to_send,received_bytes;
    int i,L,x[6],port;
    int timeout_windows = 1000; /*milliseconds*/

    vp_os_memset(buffer,0,sizeof(buffer));

    wifi_config_socket(&ftp_client,VP_COM_CLIENT,FTP_PORT,WIFI_ARDRONE_IP);
    ftp_client.protocol = VP_COM_TCP;
    if(VP_FAILED(vp_com_init(wifi_com()))) return -1;
    if(VP_FAILED(vp_com_open(wifi_com(), &ftp_client, &ftp_read, &ftp_write))) return -2;
    setsockopt((int32_t)ftp_client.priv,
               SOL_SOCKET,
               SO_RCVTIMEO,
               (const char*)&timeout_windows, sizeof(timeout_windows)
              );

    bytes_to_send = _snprintf(buffer,sizeof(buffer),"%s",
                              "USER anonymous\r\nCWD /\r\nPWD\r\nTYPE A\r\nPASV\r\nRETR version.txt\r\n");
    ftp_write(&ftp_client,buffer,&bytes_to_send);
    Sleep(1000);


    received_bytes = sizeof(buffer);
    ftp_read(&ftp_client,buffer,&received_bytes);
    if (received_bytes<1) {
        vp_com_close(wifi_com(), &ftp_client);
        return -3;
    }
    L=received_bytes-strlen(passivdeModeHeader);

    for (i=0; i<L; i++) {
        if (strncmp((buffer+i),passivdeModeHeader,strlen(passivdeModeHeader))==0)  break;
    }
    if (i==L) {
        vp_com_close(wifi_com(), &ftp_client);
        return -4;
    }
    i+=strlen(passivdeModeHeader);
    if (sscanf(buffer+i,"%i,%i,%i,%i,%i,%i)",&x[0],&x[1],&x[2],&x[3],&x[4],&x[5])!=6)
    {
        vp_com_close(wifi_com(), &ftp_client);
        return -5;
    }
    port=(x[4]<<8)+x[5];

    wifi_config_socket(&ftp_client2,VP_COM_CLIENT,port,"192.168.1.1");
    ftp_client2.protocol = VP_COM_TCP;
    if(VP_FAILED(vp_com_init(wifi_com())))
    {
        vp_com_close(wifi_com(), &ftp_client2);
        return -6;
    }
    if(VP_FAILED(vp_com_open(wifi_com(), &ftp_client2, &ftp_read, &ftp_write)))
    {
        vp_com_close(wifi_com(), &ftp_client2);
        return -7;
    }

    received_bytes = sizeof(buffer);
    ftp_read(&ftp_client2,buffer,&received_bytes);
    if (received_bytes>0) {
        buffer[min(received_bytes,sizeof(buffer)-1)]=0;
        printf("无人机版本 %s 被检测到 ... 按下 <Enter> 开始应用.\n",buffer);
        getchar();
    }


    vp_com_close(wifi_com(), &ftp_client);
    vp_com_close(wifi_com(), &ftp_client2);

    return 0;
}
DEFINE_THREAD_ROUTINE( navdata_update, nomParams )
{
    C_RESULT res;
    int32_t  i, size;
    uint32_t cks, navdata_cks, sequence = NAVDATA_SEQUENCE_DEFAULT-1;
    struct timeval tv;
#ifdef _WIN32
    int timeout_for_windows=1000/*milliseconds*/;
#endif


    navdata_t* navdata = (navdata_t*) &navdata_buffer[0];

    tv.tv_sec   = 1/*second*/;
    tv.tv_usec  = 0;

    res = C_OK;

    if( VP_FAILED(vp_com_open(COM_NAVDATA(), &navdata_socket, &navdata_read, &navdata_write)) )
    {
        printf("VP_Com : Failed to open socket for navdata\n");
        res = C_FAIL;
    }

    if( VP_SUCCEEDED(res) )
    {
        PRINT("Thread navdata_update in progress...\n");

#ifdef _WIN32
        setsockopt((int32_t)navdata_socket.priv, SOL_SOCKET, SO_RCVTIMEO, (const char*)&timeout_for_windows, sizeof(timeout_for_windows));
        /* Added by Stephane to force the drone start sending data. */
        if(navdata_write)
        {
            int sizeinit = 5;
            navdata_write( (void*)&navdata_socket, (int8_t*)"Init", &sizeinit );
        }
#else
        setsockopt((int32_t)navdata_socket.priv, SOL_SOCKET, SO_RCVTIMEO, (const char*)&tv, sizeof(tv));
#endif


        i = 0;
        while( ardrone_navdata_handler_table[i].init != NULL )
        {
            // if init failed for an handler we set its process function to null
            // We keep its release function for cleanup
            if( VP_FAILED( ardrone_navdata_handler_table[i].init(ardrone_navdata_handler_table[i].data) ) )
                ardrone_navdata_handler_table[i].process = NULL;

            i ++;
        }

        navdata_thread_in_pause = FALSE;
        while( VP_SUCCEEDED(res)
                && !ardrone_tool_exit()
                && bContinue )
        {
            if(navdata_thread_in_pause)
            {
                vp_os_mutex_lock(&navdata_client_mutex);
                num_retries = NAVDATA_MAX_RETRIES + 1;
                vp_os_cond_wait(&navdata_client_condition);
                vp_os_mutex_unlock(&navdata_client_mutex);
            }

            if( navdata_read == NULL )
            {
                res = C_FAIL;
                continue;
            }

            size = NAVDATA_MAX_SIZE;
            navdata->header = 0; // Soft reset
            res = navdata_read( (void*)&navdata_socket, (int8_t*)&navdata_buffer[0], &size );

#ifdef _WIN32
            if( size <= 0 )
#else
            if( size == 0 )
#endif
            {
                // timeout
                PRINT("Timeout\n");
                ardrone_navdata_open_server();
                sequence = NAVDATA_SEQUENCE_DEFAULT-1;
                num_retries++;
            }
            else
                num_retries = 0;

            if( VP_SUCCEEDED( res ) )
            {
                if( navdata->header == NAVDATA_HEADER )
                {
                    if( ardrone_get_mask_from_state(navdata->ardrone_state, ARDRONE_COM_WATCHDOG_MASK) )
                    {
                        // reset sequence number because of com watchdog
                        // This code is mandatory because we can have a com watchdog without detecting it on mobile side :
                        //        Reconnection is fast enough (less than one second)
                        sequence = NAVDATA_SEQUENCE_DEFAULT-1;

                        if( ardrone_get_mask_from_state(navdata->ardrone_state, ARDRONE_NAVDATA_BOOTSTRAP) == FALSE )
                            ardrone_tool_send_com_watchdog(); // acknowledge
                    }

                    if( navdata->sequence > sequence )
                    {
                        i = 0;

                        ardrone_navdata_unpack_all(&navdata_unpacked, navdata, &navdata_cks);
                        cks = ardrone_navdata_compute_cks( &navdata_buffer[0], size - sizeof(navdata_cks_t) );

                        if( cks == navdata_cks )
                        {
                            while( ardrone_navdata_handler_table[i].init != NULL )
                            {
                                if( ardrone_navdata_handler_table[i].process != NULL )
                                    ardrone_navdata_handler_table[i].process( &navdata_unpacked );

                                i++;
                            }
                        }
                        else
                        {
                            PRINT("[Navdata] Checksum failed : %d (distant) / %d (local)\n", navdata_cks, cks);
                        }
                    }
                    else
                    {
                        PRINT("[Navdata] Sequence pb : %d (distant) / %d (local)\n", navdata->sequence, sequence);
                    }

                    // remaining = sizeof(navdata);

                    sequence = navdata->sequence;
                }
            }
        }

        // Release resources alllocated by handlers
        i = 0;
        while( ardrone_navdata_handler_table[i].init != NULL )
        {
            ardrone_navdata_handler_table[i].release();

            i ++;
        }
    }

    vp_com_close(COM_NAVDATA(), &navdata_socket);

    DEBUG_PRINT_SDK("Thread navdata_update ended\n");

    return (THREAD_RET)res;
}
Exemplo n.º 12
0
void closeClient(TClient *client) {
	if (client->socket.priv != NULL) {
		shutdown((int)client->socket.priv, SHUT_RDWR);
		vp_com_close(&globalCom, &client->socket);
	}
}
Exemplo n.º 13
0
C_RESULT wifi_server_auth(struct in_addr *addr)
{
	Read read = NULL;
	Write write = NULL;
	C_RESULT result = C_FAIL;
    int numretries = 1;
    uint8_t recvString[BUFFER_SIZE]; /* Buffer for received string */
    int recvStringLen;            /* Length of received string */
    struct timeval tv = {0, 500000};
    int on=1;
    const uint8_t msg[] = AUTH_MSG;
    struct in_addr to;
    struct in_addr from;

    vp_com_socket_t socket;

    // Initialize sending socket
    to.s_addr = htonl(WIFI_BROADCAST_ADDR);
	COM_CONFIG_SOCKET_AUTH(&socket, VP_COM_CLIENT, AUTH_PORT, inet_ntoa(to));
	socket.protocol = VP_COM_UDP;

	if(VP_FAILED(vp_com_init(COM_AUTH())))
	{
		printf("Failed to init Authentification\n");
		vp_com_shutdown( COM_AUTH() );
		return C_FAIL;
	}

	if(VP_FAILED(vp_com_open(COM_AUTH(), &socket, &read, &write)))
	{
		printf("Failed to open Authentification\n");
		vp_com_shutdown( COM_AUTH() );
		return C_FAIL;
	}

    if (setsockopt((int)socket.priv, SOL_SOCKET, SO_BROADCAST,(char *)&on,sizeof(on)) < 0)
    {
		printf("Failed to set socket option Authentification\n");
    	vp_com_close(COM_AUTH(), &socket);
		vp_com_shutdown( COM_AUTH() );
        return C_FAIL;
    }

    if (setsockopt((int)socket.priv, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)) < 0)
    {
		printf("Failed to set socket option Authentification\n");
    	vp_com_close(COM_AUTH(), &socket);
		vp_com_shutdown( COM_AUTH() );
        return C_FAIL;
    }

    do
    {
    	int len = strlen((char*)msg);
    	if(write != NULL)
    	{
    		if (VP_FAILED(write(&socket, msg, &len)))
			{
				vp_com_close(COM_AUTH(), &socket);
				vp_com_shutdown( COM_AUTH() );
				return C_FAIL;
			}
    	}

    	printf("Wait authentification\n");
    	do
        {
        	if(read != NULL)
        	{
             	recvStringLen = BUFFER_SIZE;
				if(VP_FAILED(read(&socket, recvString, &recvStringLen)))
				{
					vp_com_close(COM_AUTH(), &socket);
					vp_com_shutdown( COM_AUTH());
					return C_FAIL;
				}
        	}

        	recvString[recvStringLen] = '\0';
        }
        while((recvStringLen != 0) && (strcmp((char *)recvString, AUTH_MSG) == 0));
    }
    while((strcmp((char *)recvString, AUTH_MSG_OK) != 0) && (numretries++ < AUTH_MAX_NUMRETRIES));

    if(strcmp((char*)recvString, AUTH_MSG_OK) == 0)
    {
    	from.s_addr = socket.scn;
    	printf("Authentification ok from %s:%d\n", inet_ntoa(from), socket.port);
    	memcpy(addr, &from, sizeof(struct in_addr));
    	result = C_OK;
    }

    vp_com_close(COM_AUTH(), &socket);
	vp_com_shutdown( COM_AUTH() );

    return result;
}
DEFINE_THREAD_ROUTINE( ardrone_control, nomParams )
{
	C_RESULT res_wait_navdata = C_OK;
	C_RESULT res = C_OK;
	uint32_t retry, current_ardrone_state;
	int32_t next_index_in_queue;
	ardrone_control_event_ptr_t  current_event;
	
	retry = 0;
	current_event = NULL;
	
	DEBUG_PRINT_SDK("Thread control in progress...\n");
	control_socket.is_disable = TRUE;
	
	ardrone_control_connect_to_drone();

	while( bContinue 
          && !ardrone_tool_exit() )
	{
		vp_os_mutex_lock(&control_mutex);
		control_waited = TRUE;

		/* Wait for new navdata to be received. */
		res_wait_navdata = vp_os_cond_timed_wait(&control_cond, 1000);
		vp_os_mutex_unlock(&control_mutex);

		/*
		 * In case of timeout on the navdata, we assume that there was a problem
		 * with the Wifi connection.
		 * It is then safer to close and reopen the control socket (TCP 5559) since
		 * some OS might stop giving data but not signal any disconnection.
		 */
		if(VP_FAILED(res_wait_navdata))
		{
			DEBUG_PRINT_SDK("Timeout while waiting for new navdata.\n");
			if(!control_socket.is_disable)
				control_socket.is_disable = TRUE;
		}
			
		if(control_socket.is_disable)
		{
			ardrone_control_connect_to_drone();
		}
		
		if(VP_SUCCEEDED(res_wait_navdata) && (!control_socket.is_disable))
		{
			vp_os_mutex_lock(&control_mutex);
			current_ardrone_state = ardrone_state;
			control_waited = FALSE;
			vp_os_mutex_unlock(&control_mutex);
			
			if( ardrone_tool_exit() ) // Test if we received a signal because we are quitting the application
				THREAD_RETURN( res );
			
 			if( current_event == NULL )
			{
				vp_os_mutex_lock(&event_queue_mutex);
				next_index_in_queue = (end_index_in_queue + 1) & (ARDRONE_CONTROL_MAX_NUM_EVENTS_IN_QUEUE - 1);
				
				if( next_index_in_queue != start_index_in_queue )
				{ // There's an event to process
					current_event = ardrone_control_event_queue[next_index_in_queue];
					if( current_event != NULL )
					{
						if( current_event->ardrone_control_event_start != NULL )
						{
							current_event->ardrone_control_event_start( current_event );
						}
					}
					end_index_in_queue = next_index_in_queue;
					
					retry = 0;
				}
				
				vp_os_mutex_unlock(&event_queue_mutex);
			}
			
			if( current_event != NULL )
			{
				switch( current_event->event )
				{
					case ARDRONE_UPDATE_CONTROL_MODE:
						res = ardrone_control_soft_update_run( current_ardrone_state, (ardrone_control_soft_update_event_t*) current_event );
						break;
						
					case PIC_UPDATE_CONTROL_MODE:
						res = ardrone_control_soft_update_run( current_ardrone_state, (ardrone_control_soft_update_event_t*) current_event );
						break;
						
					case LOGS_GET_CONTROL_MODE:
						break;
						
					case CFG_GET_CONTROL_MODE:
					case CUSTOM_CFG_GET_CONTROL_MODE: /* multiconfiguration support */
						res = ardrone_control_configuration_run( current_ardrone_state, (ardrone_control_configuration_event_t*) current_event );
						break;
						
					case ACK_CONTROL_MODE:
						res = ardrone_control_ack_run( current_ardrone_state, (ardrone_control_ack_event_t *) current_event);
						break;
						
					default:
						break;
				}
				
				if( VP_FAILED(res) )
				{
					retry ++;
					if( retry > current_event->num_retries)
						current_event->status = ARDRONE_CONTROL_EVENT_FINISH_FAILURE;
				}
				else
				{
					retry = 0;
				}
				
				if( current_event->status & ARDRONE_CONTROL_EVENT_FINISH )
				{
 					if( current_event->ardrone_control_event_end != NULL )
						current_event->ardrone_control_event_end( current_event );
					
 					/* Make the thread read a new event on the next loop iteration */
					current_event = NULL;
				}
				else
				{
					/* Not changing 'current_event' makes the loop process the same
					 * event when the next navdata packet arrives. */
				}
			}
		}
  }// while

  /* Stephane : Bug fix - mutexes were previously detroyed by another thread,
  which made ardrone_control crash.*/
	  vp_os_mutex_destroy(&event_queue_mutex);
	  vp_os_cond_destroy(&control_cond);
	  vp_os_mutex_destroy(&control_mutex);

  vp_com_close(COM_CONTROL(), &control_socket);

  THREAD_RETURN( res );
}