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 );
}
