C_RESULT vp_com_client_open_socket(vp_com_socket_t* server_socket, vp_com_socket_t* client_socket) { C_RESULT res; struct sockaddr_in raddr = { 0 }; // remote address socklen_t l = sizeof(raddr); int32_t s = (int32_t) server_socket->priv; Write write = (Write) (server_socket->protocol == VP_COM_TCP ? vp_com_write_socket : vp_com_write_udp_socket); vp_os_memcpy( client_socket, server_socket, sizeof(vp_com_socket_t) ); res = server_socket->select( server_socket, client_socket, VP_COM_SOCKET_SELECT_ENABLE, write ); if( VP_SUCCEEDED(res) ) { if( server_socket->protocol == VP_COM_TCP ) { client_socket->priv = (void*)accept( s, (struct sockaddr*)&raddr, &l ); } DEBUG_PRINT_SDK("[VP_COM_SERVER] Opening socket for server %d\n", (int)s); client_socket->server = server_socket; } else { DEBUG_PRINT_SDK("[VP_COM_SERVER] Failed to open socket for server %d\n", (int)s); vp_os_memset( client_socket, 0, sizeof(vp_com_socket_t) ); } return res; }
C_RESULT ardrone_tool_setup_com( const char* ssid ) { LOGII("not in lib"); C_RESULT res = C_OK; #ifdef CHECK_WIFI_CONFIG if( FAILED(vp_com_init(COM_NAVDATA())) ) { DEBUG_PRINT_SDK("VP_Com : Failed to init com for navdata\n"); vp_com_shutdown(COM_NAVDATA()); res = C_FAIL; } vp_com_network_adapter_lookup(COM_NAVDATA(), ardrone_toy_network_adapter_cb); if( SUCCEED(res) && FAILED(vp_com_local_config(COM_NAVDATA(), COM_CONFIG_NAVDATA())) ) { DEBUG_PRINT_SDK("VP_Com : Failed to configure com for navdata\n"); vp_com_shutdown(COM_NAVDATA()); res = C_FAIL; } if( ssid != NULL ) { strncpy( ((vp_com_wifi_connection_t*)wifi_connection())->networkName, ssid, VP_COM_NAME_MAXSIZE-1 ); ((vp_com_wifi_connection_t*)wifi_connection())->networkName[VP_COM_NAME_MAXSIZE-1]='\0'; } if( SUCCEED(res) && FAILED(vp_com_connect(COM_NAVDATA(), COM_CONNECTION_NAVDATA(), NUM_ATTEMPTS))) { DEBUG_PRINT_SDK("VP_Com: Failed to connect for navdata\n"); vp_com_shutdown(COM_NAVDATA()); res = C_FAIL; } #else vp_com_init(COM_NAVDATA()); vp_com_network_adapter_lookup(COM_NAVDATA(), ardrone_toy_network_adapter_cb); vp_com_local_config(COM_NAVDATA(), COM_CONFIG_NAVDATA()); if( ssid != NULL ) { strncpy( ((vp_com_wifi_connection_t*)wifi_connection())->networkName, ssid, VP_COM_NAME_MAXSIZE-1 ); ((vp_com_wifi_connection_t*)wifi_connection())->networkName[VP_COM_NAME_MAXSIZE-1]='\0'; } vp_com_connect(COM_NAVDATA(), COM_CONNECTION_NAVDATA(), NUM_ATTEMPTS); ((vp_com_wifi_connection_t*)wifi_connection())->is_up=1; #endif return res; }
void vp_com_close_client_sockets(vp_com_socket_t* client_sockets, int32_t num_client_sockets) { int32_t s; // Select timed out - We close all sockets because it should mean we lost connection with client while( num_client_sockets > 0 ) { if( !client_sockets->is_disable ) { s = (int32_t) client_sockets->priv; DEBUG_PRINT_SDK("[VP_COM_SERVER] Closing socket %d\n", (int)s); client_sockets->select( client_sockets->server, client_sockets, VP_COM_SOCKET_SELECT_DISABLE, (Write) vp_com_write_socket ); if( client_sockets->protocol == VP_COM_TCP ) { closesocket( s ); } vp_os_memset( client_sockets, 0, sizeof(vp_com_socket_t) ); client_sockets->is_disable = TRUE; } client_sockets++; num_client_sockets--; } }
static void ihm_RAWCapture(GtkWidget *widget, gpointer data) { static int is_recording = 0; DEBUG_PRINT_SDK(" RAW video capture\n"); DEST_HANDLE dest; is_recording ^= 1; // Sending AT command to drone. ardrone_at_start_raw_capture(); /* * Tells the Raw capture stage to start dumping YUV frames from * the pipeline to a disk file. */ dest.stage = 2; dest.pipeline = pipeline_handle; vp_api_post_message(dest, PIPELINE_MSG_START, ihm_video_recording_callback, NULL); /* Tells the FFMPEG recorder stage to start dumping the video in a * MPEG4 video file. */ dest.stage = 3; vp_api_post_message(dest, PIPELINE_MSG_START, NULL, NULL); if (is_recording) { gtk_button_set_label((GtkButton*) ihm_ImageButton[RAW_CAPTURE_BUTTON], (const gchar*) "Recording...\n(click again to stop)"); gtk_button_set_label((GtkButton*) ihm_fullScreenButton[0], (const gchar*) "Recording...\n(click again to stop)"); } else { gtk_button_set_label((GtkButton*) ihm_ImageButton[RAW_CAPTURE_BUTTON], (const gchar*) "Recording stopped.\nClick again to start a new video"); gtk_button_set_label((GtkButton*) ihm_fullScreenButton[0], (const gchar*) "Recording stopped.\nClick again to start a new video"); } }
static void ihm_sendVisionConfigParams(GtkWidget *widget, gpointer data) { api_vision_tracker_params_t params; params.coarse_scale = tab_vision_config_params[0]; // scale of current picture with respect to original picture params.nb_pair = tab_vision_config_params[1]; // number of searched pairs in each direction params.loss_per = tab_vision_config_params[2]; // authorized lost pairs percentage for tracking params.nb_tracker_width = tab_vision_config_params[3]; // number of trackers in width of current picture params.nb_tracker_height = tab_vision_config_params[4]; // number of trackers in height of current picture params.scale = tab_vision_config_params[5]; // distance between two pixels in a pair params.trans_max = tab_vision_config_params[6]; // largest value of trackers translation between two adjacent pictures params.max_pair_dist = tab_vision_config_params[7]; // largest distance of pairs research from tracker location params.noise = tab_vision_config_params[8]; // threshold of significative contrast ardrone_at_set_vision_track_params( ¶ms ); DEBUG_PRINT_SDK("CS %04d NB_P %04d Lossp %04d NB_Tlg %04d NB_TH %04d Scale %04d Dist_Max %04d Max_Dist %04d Noise %04d\n", tab_vision_config_params[0], tab_vision_config_params[1], tab_vision_config_params[2], tab_vision_config_params[3], tab_vision_config_params[4], tab_vision_config_params[5], tab_vision_config_params[6], tab_vision_config_params[7], tab_vision_config_params[8] ); }
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 ardrone_tool_input_remove( input_device_t* device ) { C_RESULT res; int32_t i; VP_OS_ASSERT( device != NULL ); res = C_FAIL; i = 0; while( i < MAX_NUM_DEVICES && devices[i] != device ) i++; if( i < MAX_NUM_DEVICES ) { res = ardrone_tool_input_remove_i(i); } else { DEBUG_PRINT_SDK("Input %s not found while removing\n", device->name); } return res; }
C_RESULT video_com_multisocket_stage_transform(video_com_multisocket_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out) { C_RESULT res; fd_set rfds; int retval; int fs,maxfs; struct timeval tv; int i; bool_t selectTimeout; vp_os_mutex_lock(&out->lock); out->size = 0; for (i=0;i<cfg->nb_sockets;i++) { if (1 == cfg->configs[i]->mustReconnect) { PDBG ("Will call connect"); PRINT ("Reconnecting ... "); res = C_FAIL; res = video_com_stage_connect (cfg->configs[i]); PRINT ("%s\n", (VP_FAILED (res) ? "FAIL" : "OK")); cfg->configs[i]->mustReconnect = 0; } } if(out->status == VP_API_STATUS_INIT) { out->numBuffers = 1; out->size = 0; out->buffers = (uint8_t **) vp_os_malloc (sizeof(uint8_t *)+cfg->buffer_size*sizeof(uint8_t)); out->buffers[0] = (uint8_t *)(out->buffers+1); out->indexBuffer = 0; // out->lineSize not used out->status = VP_API_STATUS_PROCESSING; } if(out->status == VP_API_STATUS_PROCESSING) { /* Check which socket has data to read */ tv.tv_sec = 1; tv.tv_usec = 0; if(cfg->forceNonBlocking && *(cfg->forceNonBlocking)==TRUE) { tv.tv_sec = 0; } FD_ZERO(&rfds); maxfs=0; for (i=0;i<cfg->nb_sockets;i++) { if(cfg->configs[i]->connected) { fs = (int)cfg->configs[i]->socket.priv; if (fs>maxfs) maxfs=fs; FD_SET(fs,&rfds); } } retval = select(maxfs+1, &rfds, NULL, NULL, &tv); /* Read the socket which has available data */ selectTimeout = FALSE; i = -1; if (retval>0) { if (cfg->last_active_socket!=-1) { i=cfg->last_active_socket; fs = (int)cfg->configs[i]->socket.priv; if (cfg->configs[i]->read && FD_ISSET(fs, &rfds)) { out->size = cfg->configs[i]->buffer_size; } else { i = -1; } } if (-1 == i) { for (i=0;i<cfg->nb_sockets;i++) { fs = (int)cfg->configs[i]->socket.priv; if (cfg->configs[i]->read && FD_ISSET(fs, &rfds)) { DEBUG_PRINT_SDK("Video multisocket : found data on port %s %d\n", (cfg->configs[i]->socket.protocol==VP_COM_TCP)?"TCP":"UDP",cfg->configs[i]->socket.port); cfg->last_active_socket = i; if (VP_COM_TCP == cfg->configs[i]->protocol) { DEBUG_isTcp = 1; } else { DEBUG_isTcp = 0; } out->size = cfg->configs[i]->buffer_size; break; } } if(i == cfg->nb_sockets) { PRINT("%s:%d BUG !!!!!", __FUNCTION__, __LINE__); selectTimeout = TRUE; } } } else { DEBUG_PRINT_SDK("%s\n",(retval==0)?"timeout":"error"); selectTimeout = TRUE; } if (FALSE == selectTimeout) { // DEBUG_PRINT_SDK ("Will read on socket %d\n", i); // Actual first time read res = cfg->configs[i]->read(&cfg->configs[i]->socket, out->buffers[0], &out->size); if (VP_FAILED (res)) { PDBG ("%s [%d] : status set to error !\n", __FUNCTION__, __LINE__); perror ("Video_com_stage"); cfg->configs[i]->mustReconnect = 1; out->size = 0; vp_os_mutex_unlock (&out->lock); return C_OK; } // Loop read to empty the socket buffers if needed cfg->configs[i]->socket.block = VP_COM_DONTWAIT; bool_t bContinue = TRUE; int32_t readSize = cfg->configs[i]->buffer_size - out->size; while (TRUE == bContinue) { // DEBUG_PRINT_SDK ("Will read %d octets from socket %d\n", readSize, i); res = cfg->configs[i]->read(&cfg->configs[i]->socket, &(out->buffers[0][out->size]), &readSize); if (VP_FAILED (res)) { PDBG ("%s [%d] : status set to error !\n", __FUNCTION__, __LINE__); perror ("Video_com_stage"); cfg->configs[i]->mustReconnect = 1; out->size = 0; vp_os_mutex_unlock (&out->lock); return C_OK; } if (0 == readSize) { bContinue = FALSE; } out->size += readSize; readSize = cfg->configs[i]->buffer_size - out->size; } cfg->configs[i]->socket.block = VP_COM_DEFAULT; } /* Resend a connection packet on UDP sockets */ if(!(cfg->forceNonBlocking && *(cfg->forceNonBlocking)==TRUE)) { if( /* select timed out */ retval==0 ) { for (i=0;i<cfg->nb_sockets;i++) { if( cfg->configs[i]->protocol == VP_COM_UDP ) { // Send "1" for Unicast // Send "2" to enable Multicast char flag = 1; int len = sizeof(flag); if ( cfg->configs[i]->socket.is_multicast == 1 ) flag = 2; DEBUG_PRINT_SDK("Video multisocket : sending connection byte on port %s %d\n", (cfg->configs[i]->socket.protocol==VP_COM_TCP)?"TCP":"UDP",cfg->configs[i]->socket.port); cfg->configs[i]->write(&cfg->configs[i]->socket, (uint8_t*) &flag, &len); } } } } if( (TRUE == selectTimeout || out->size == 0) && (!cfg->forceNonBlocking || (*(cfg->forceNonBlocking) == FALSE)) ) { cfg->num_retries++; } else cfg->num_retries = 0; if((selectTimeout == TRUE) && cfg->forceNonBlocking && *(cfg->forceNonBlocking)==TRUE) { //PRINT("Debug %s:%d\n",__FUNCTION__,__LINE__); /* No data are available here, but some are available in the next stage */ /* out->size=0 would restart the pipeline */ out->size=-1; } } vp_os_mutex_unlock(&out->lock); if (out->size > 0) { DEBUG_totalBytes += out->size; static struct timeval DEBUG_now = {0, 0}, DEBUG_prev = {0, 0}; gettimeofday (&DEBUG_now, NULL); float DEBUG_timeDiff = ((DEBUG_now.tv_sec - DEBUG_prev.tv_sec) * 1000.0) + ((DEBUG_now.tv_usec - DEBUG_prev.tv_usec) / 1000.0); if (DEBUG_TIME_BITRATE_CALCULATION_MSEC <= DEBUG_timeDiff) { DEBUG_prev.tv_sec = DEBUG_now.tv_sec; DEBUG_prev.tv_usec = DEBUG_now.tv_usec; float DEBUG_instantBitrate = 8.0 * (float)(DEBUG_totalBytes) / DEBUG_TIME_BITRATE_CALCULATION_MSEC; DEBUG_totalBytes = 0; DEBUG_bitrate = 0.8 * DEBUG_bitrate + 0.2 * DEBUG_instantBitrate; } } return C_OK; }
static void set_state(ardrone_control_configuration_event_t* event, ardrone_config_state_t s) { DEBUG_PRINT_SDK("[CONTROL CONFIGURATION] Switching to state %d\n", s); event->config_state = s; }
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; }
DEFINE_THREAD_ROUTINE_STACK( vp_com_server, thread_params, VP_COM_THREAD_SERVER_STACK_SIZE ) { vp_com_socket_t client_sockets[VP_COM_THREAD_NUM_MAX_CLIENTS]; struct timeval tv, *ptv; // This thread setup connection then loop & wait for a socket event vp_com_server_thread_param_t* params = (vp_com_server_thread_param_t*) thread_params; int32_t i, rc, ncs, s, max = 0, num_server_sockets = params->num_servers, num_client_sockets = 0; vp_com_socket_t* server_sockets = params->servers; fd_set read_fs; vp_os_memset( client_sockets, 0, sizeof( client_sockets )); if(VP_FAILED(vp_com_init(params->com))) { DEBUG_PRINT_SDK("[VP_COM_SERVER] Failed to init com\n"); vp_com_shutdown(params->com); } else if(VP_FAILED(vp_com_local_config(params->com, params->config))) { DEBUG_PRINT_SDK("[VP_COM_SERVER] Failed to configure com\n"); vp_com_shutdown(params->com); } else if(VP_FAILED(vp_com_connect(params->com, params->connection, 1))) { DEBUG_PRINT_SDK("[VP_COM_SERVER] Failed to connect\n"); vp_com_shutdown(params->com); } else { vp_os_mutex_lock(&server_initialisation_mutex); vp_os_cond_signal(&server_initialisation_wait); vp_os_mutex_unlock(&server_initialisation_mutex); server_init_not_finished = FALSE; for( i = 0; i < num_server_sockets; i++ ) { if(VP_FAILED( vp_com_open_socket(&server_sockets[i], NULL, NULL) )) { DEBUG_PRINT_SDK("[VP_COM_SERVER] Unable to open server socket\n"); server_sockets[i].is_disable = TRUE; } else { listen((int32_t)server_sockets[i].priv, server_sockets[i].queue_length); } } params->run = TRUE; while( params->run == TRUE ) { if( params->timer_enable == FALSE || ( params->wait_sec == 0 && params->wait_usec == 0 ) ) { ptv = NULL; } else { tv.tv_sec = params->wait_sec; tv.tv_usec = params->wait_usec; ptv = &tv; } FD_ZERO(&read_fs); max = vp_com_fill_read_fs( &server_sockets[0], num_server_sockets, 0, &read_fs ); max = vp_com_fill_read_fs( &client_sockets[0], num_client_sockets, max, &read_fs ); rc = select( max + 1, &read_fs, NULL, NULL, ptv ); if( rc == -1 && ( errno == EINTR || errno == EAGAIN ) ) continue; if( rc == 0 ) { DEBUG_PRINT_SDK("[VP_COM_SERVER] select timeout\n"); vp_com_close_client_sockets(&client_sockets[0], num_client_sockets); num_client_sockets = 0; params->timer_enable = FALSE; vp_os_memset( client_sockets, 0, sizeof( client_sockets )); } for( i = 0; i < num_server_sockets && rc != 0; i++ ) { s = (int32_t) server_sockets[i].priv; if( ( !server_sockets[i].is_disable ) && FD_ISSET( s, &read_fs) ) { rc --; // Recycle previously released sockets for( ncs = 0; ncs < num_client_sockets && client_sockets[ncs].priv != NULL; ncs++ ); if( ncs < VP_COM_THREAD_NUM_MAX_CLIENTS) { if( VP_SUCCEEDED(vp_com_client_open_socket(&server_sockets[i], &client_sockets[ncs])) && ( ncs == num_client_sockets ) ) num_client_sockets ++; } } } for( i = 0; i < num_client_sockets && rc != 0; i++ ) { s = (int32_t) client_sockets[i].priv; if( ( !client_sockets[i].is_disable ) && FD_ISSET( s, &read_fs) ) { rc--; vp_com_client_receive( &client_sockets[i] ); } } } for( i = 0; i < num_server_sockets; i++ ) { vp_com_close_socket(&server_sockets[i]); } } vp_com_disconnect(params->com); vp_com_shutdown(params->com); THREAD_RETURN( 0 ); }
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 ); }