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_frame_pipe_receiver_close(vp_stages_frame_pipe_config_t *cfg)
{
vp_os_free (cfg->outPicture.raw);
cfg->outPicture.raw = NULL;
vp_os_cond_destroy (&(cfg->buffer_sent));
vp_os_mutex_destroy (&(cfg->pipe_mut));
return C_OK;
}
C_RESULT ardrone_control_shutdown(void)
{
ardrone_control_resume_on_navdata_received(0);
/*BUG FIX : Dont destroy the mutexes here,
they are still being used by the ardrone_control thread,
while this function is called by another thread.*/
#ifdef THIS_IS_A_BUG
vp_os_mutex_destroy(&event_queue_mutex);
vp_os_cond_destroy(&control_cond);
vp_os_mutex_destroy(&control_mutex);*/
#endif
bContinue = FALSE;
return C_OK;
}
C_RESULT videoServer_init() {
videoServer_clientList = clientList_create(VIDEOSERVER_MAXCLIENTS);
/* Build frame packet buffer */
videoServer_framePacketLength = sizeof(VideoFrameHeader) + videoServer_frameWidth * videoServer_frameHeight * (videoServer_frameBpp / 8);
frameBuffer = vp_os_malloc(videoServer_framePacketLength);
vp_os_mutex_init(&frameBufferMutex);
vp_os_cond_init(&frameBufferCond, &frameBufferMutex);
/* Create server socket */
videoServerSocket.type = VP_COM_SERVER;
videoServerSocket.protocol = VP_COM_TCP;
videoServerSocket.block = VP_COM_DONTWAIT;
videoServerSocket.is_multicast = 0;
videoServerSocket.port = VIDEO_SERVER_PORT;
/* if (FAILED(vp_com_open(&globalCom, &videoServerSocket, NULL, NULL))) {
vp_os_cond_destroy(&frameBufferCond);
vp_os_mutex_destroy(&frameBufferMutex);
vp_os_free(frameBuffer);
vp_os_mutex_destroy(&clientListMutex);
PRINT("[VideoServer] Unable to open server socket\n");
return C_FAIL;
}*/
/* We create the server socket manually, as we need to set the reuse flag before binding it and Parrot's SDK doesn't allow that */
bool_t error = TRUE;
videoServerSocket.priv = (void *)socket(AF_INET, SOCK_STREAM, 0);
if ((int)videoServerSocket.priv >= 0) {
struct sockaddr_in serverAddress;
/* Try reusing the address */
int s = 1;
setsockopt((int)videoServerSocket.priv, SOL_SOCKET, SO_REUSEADDR, &s, sizeof(int));
/* Bind to address and port */
bzero((char *)&serverAddress, sizeof(serverAddress));
serverAddress.sin_family = AF_INET;
serverAddress.sin_addr.s_addr = INADDR_ANY;
serverAddress.sin_port = htons(videoServerSocket.port);
if (bind((int)videoServerSocket.priv, (struct sockaddr *) &serverAddress, sizeof(struct sockaddr_in)) >= 0)
{
error = FALSE;
} else close((int)videoServerSocket.priv);
}
if (error) {
vp_os_cond_destroy(&frameBufferCond);
vp_os_mutex_destroy(&frameBufferMutex);
vp_os_free(frameBuffer);
if (videoServer_clientList != NULL) {
clientList_destroy(videoServer_clientList);
videoServer_clientList = NULL;
}
PRINT("[VideoServer] Unable to open server socket\n");
return C_FAIL;
}
/* Set server socket timeout */
struct timeval tm;
tm.tv_sec = 0;
tm.tv_usec = ACCEPT_TIMEOUT * 1e6;
setsockopt((int32_t)videoServerSocket.priv, SOL_SOCKET, SO_RCVTIMEO, &tm, sizeof(tm));
videoTranscoder_init();
videoServerStarted = TRUE;
vp_os_mutex_init(&settingsMutex);
return C_OK;
}
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 );
}
