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