/*--------------------------------------------------------------------------------------------------------------------- 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; }
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); }
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; }
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; }
void closeClient(TClient *client) { if (client->socket.priv != NULL) { shutdown((int)client->socket.priv, SHUT_RDWR); vp_com_close(&globalCom, &client->socket); } }
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 ); }