自学教程：C++ vp_os_mutex_unlock函数代码示例

inline C_RESULT navdata_process( const navdata_unpacked_t* const navdata ){	if (bIsInitialized == FALSE) {		LOGW(TAG, "Navdata is not initialized yet");		return C_OK;	}	vp_os_mutex_lock( &instance_navdata_mutex);	vp_os_memcpy(&inst_nav, navdata, sizeof(navdata_unpacked_t));	vp_os_mutex_unlock( &instance_navdata_mutex );	return C_OK;}

bool SetFlightAnimationCallback(ardrone_autonomy::FlightAnim::Request &request,                                ardrone_autonomy::FlightAnim::Response &response){  char param[20];  const int anim_type = request.type % ARDRONE_NB_ANIM_MAYDAY;  const int anim_duration = (request.duration > 0) ? request.duration : MAYDAY_TIMEOUT[anim_type];  snprintf(param, sizeof(param), "%d,%d", anim_type, anim_duration);  vp_os_mutex_lock(&twist_lock);  ARDRONE_TOOL_CONFIGURATION_ADDEVENT(flight_anim, param, NULL);  vp_os_mutex_unlock(&twist_lock);  response.result = true;  return true;}

void teleopcmdVelCallback(const geometry_msgs::TwistStampedConstPtr &msg){        vp_os_mutex_lock(&twist_lock);    //printf("cmdVel/n");	const float maxHorizontalSpeed = 1; // use 0.1f for testing and 1 for the real thing	teleopcmd_vel.twist.linear.x  = max(min(-msg->twist.linear.x, maxHorizontalSpeed), -maxHorizontalSpeed);	teleopcmd_vel.twist.linear.y  = max(min(-msg->twist.linear.y, maxHorizontalSpeed), -maxHorizontalSpeed);	teleopcmd_vel.twist.linear.z  = max(min(msg->twist.linear.z, 1), -1);	teleopcmd_vel.twist.angular.x = 0;	teleopcmd_vel.twist.angular.y = 0;	teleopcmd_vel.twist.angular.z = max(min(-msg->twist.angular.z, 1), -1);        vp_os_mutex_unlock(&twist_lock);}

C_RESULT vp_com_shutdown(vp_com_t* vp_com){  VP_OS_ASSERT( vp_com != NULL );  vp_os_mutex_lock( &vp_com->mutex );  if(vp_com->ref_count > 0)  {    vp_com->ref_count--;    if(vp_com->ref_count == 0)    {      vp_os_mutex_unlock( &vp_com->mutex );      vp_os_mutex_destroy( &vp_com->mutex );      return VP_COM_SHUTDOWN();    }  }  vp_os_mutex_unlock(&vp_com->mutex);  return VP_COM_OK;}

C_RESULT vp_com_timed_wait_for_server_up(uint32_t ms){  C_RESULT res = C_OK;  if( server_init_not_finished )  {    vp_os_mutex_lock(&server_initialisation_mutex);    res = vp_os_cond_timed_wait(&server_initialisation_wait, ms);    vp_os_mutex_unlock(&server_initialisation_mutex);  }  return res;}

static ATCODEC_RETvalist_ATcodec_Queue_Message_valist_Tree(ATcodec_Tree_t *tree, AT_CODEC_MSG_ID id, va_list *va){    int32_t len;    ATCODEC_RET res;    ATcodec_Tree_Node_t *node = ATcodec_Tree_Node_get(tree, (int)id);    ATcodec_Message_Data_t *data = (ATcodec_Message_Data_t *)ATcodec_Buffer_getElement(&tree->leaves, node->data);    char *total_str = (char *)ATcodec_Buffer_getElement(&tree->strs, data->total_str);    ATcodec_Memory_t msg, fmt;    char buffer[INTERNAL_BUFFER_SIZE];    if(!atcodec_lib_init_ok)        return ATCODEC_FALSE;    vp_os_mutex_lock(&ATcodec_cond_mutex);    ATcodec_Memory_Init(&msg, (char*)&buffer[0], INTERNAL_BUFFER_SIZE, 1, NULL, NULL);    ATcodec_Memory_Init(&fmt, total_str, 0, 1, NULL, NULL);    if((res = vp_atcodec_sprintf_valist(&msg, &len, &fmt, va)) !=  ATCODEC_TRUE)    {        vp_os_mutex_unlock(&ATcodec_cond_mutex);        va_end(*va);        return res;    }    if(ATcodec_Message_len + len < INTERNAL_BUFFER_SIZE)    {        memcpy(&ATcodec_Message_Buffer[ATcodec_Message_len], &buffer[0], len);        ATcodec_Message_len += len;    }    //vp_os_cond_signal(&ATcodec_wait_cond);    vp_os_mutex_unlock(&ATcodec_cond_mutex);    va_end(*va);    return ATCODEC_TRUE;}

PROTO_THREAD_ROUTINE(dct, params){  uint32_t i;  PRINT("DCT thread start/n");  while(1)  {    if( current_io_buffer == NULL )    {      vp_os_mutex_lock(&dct_start_mutex);        vp_os_cond_wait(&dct_start_cond);      vp_os_mutex_unlock(&dct_start_mutex);    }    if( current_io_buffer->dct_mode == DCT_MODE_FDCT )    {      for( i = 0; i < current_io_buffer->num_total_blocks; i++ )      {        fdct(current_io_buffer->input[i], current_io_buffer->output[i]);      }    }    else if( current_io_buffer->dct_mode == DCT_MODE_IDCT )    {      for( i = 0; i < current_io_buffer->num_total_blocks; i++ )      {        idct(current_io_buffer->input[i], current_io_buffer->output[i]);      }    }    vp_os_mutex_lock(&critical_section);      result_io_buffer = current_io_buffer;      current_io_buffer = NULL;    vp_os_mutex_unlock(&critical_section);  }  return 0;}

void ardrone_at_set_anim( anim_mayday_t type, int32_t duration ){	int32_t animtype = type;	if (!at_init)		return;	vp_os_mutex_lock(&at_mutex);	ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_ANIM_EXE,			++nb_sequence,			animtype,			duration );	vp_os_mutex_unlock(&at_mutex);}

void ardrone_at_set_ui_misc(int32_t m1, int32_t m2, int32_t m3, int32_t m4){  if (!at_init)     return;  vp_os_mutex_lock(&at_mutex);  ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_MISC_EXE,                                 ++nb_sequence,                                m1,                                 m2,                                 m3,                                 m4 );  vp_os_mutex_unlock(&at_mutex);}

C_RESULT navdata_get(navdata_unpacked_t *data){	C_RESULT result = C_FAIL;		if(data)	{		vp_os_mutex_lock( &inst_nav_mutex );		vp_os_memcpy(data, &inst_nav, sizeof(navdata_unpacked_t));		vp_os_mutex_unlock( &inst_nav_mutex );		result = C_OK;	}		return result;}

void cmdVelCallback(const geometry_msgs::TwistConstPtr &msg){    vp_os_mutex_lock(&twist_lock);    // Main 4DOF    cmd_vel.linear.x  = max(min(-msg->linear.x, 1.0), -1.0);    cmd_vel.linear.y  = max(min(-msg->linear.y, 1.0), -1.0);	cmd_vel.linear.z  = max(min(msg->linear.z, 1.0), -1.0);	cmd_vel.angular.z = max(min(-msg->angular.z, 1.0), -1.0);    // These 2DOF just change the auto hover behaviour    // No bound() required    cmd_vel.angular.x = msg->angular.x;    cmd_vel.angular.y = msg->angular.y;    vp_os_mutex_unlock(&twist_lock);}

void ardrone_at_set_control_gains( api_control_gains_t* gains ){  if (!at_init)     return;  vp_os_mutex_lock(&at_mutex);  ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_GAIN_EXE,                                ++nb_sequence,                                gains->pq_kp, gains->r_kp, gains->r_ki, gains->ea_kp, gains->ea_ki,                                gains->alt_kp, gains->alt_ki, gains->vz_kp, gains->vz_ki,                                gains->hovering_kp, gains->hovering_ki,                                gains->hovering_b_kp, gains->hovering_b_ki);  vp_os_mutex_unlock(&at_mutex);}

/** * @param p1 * * @param p2 * * @param p3 * * @param p4 * * @brief . * * @DESCRIPTION * *******************************************************************/void ardrone_at_set_pwm(int32_t p1, int32_t p2, int32_t p3, int32_t p4){  if (!at_init)     return;  vp_os_mutex_lock(&at_mutex);  ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_PWM_EXE,                                 ++nb_sequence,                                p1,                                  p2,                                 p3,                                 p4 );  vp_os_mutex_unlock(&at_mutex);}

extern "C" C_RESULT export_stage_transform(void *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out){//    PRINT("In Transform before copy/n");//        printf("The size of buffer is %d/n", in->size);  vp_os_mutex_lock(&video_lock);  memcpy(buffer, in->buffers[0], in->size);  current_frame_id++;  if (realtime_video)  {    ros_driver->PublishVideo();  }  vp_os_mutex_unlock(&video_lock);//    vp_os_mutex_unlock(&video_update_lock);  return (SUCCESS);}

void ardrone_at_cad( CAD_TYPE type, float32_t tag_size ){  float_or_int_t size;  size.f = tag_size;  if (!at_init)     return;  vp_os_mutex_lock(&at_mutex);  ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_CAD_EXE,                                 ++nb_sequence,                                type,                                 size.i);  vp_os_mutex_unlock(&at_mutex);}

C_RESULTvp_stages_output_sdl_stage_close(vp_stages_output_sdl_config_t *cfg){  vp_os_mutex_lock(&xlib_mutex);  SDL_ShowCursor(SDL_ENABLE);  SDL_FreeYUVOverlay(cfg->overlay);  SDL_FreeSurface(cfg->surface);  SDL_Quit();  vp_os_mutex_unlock(&xlib_mutex);  return (VP_SUCCESS);}

DEFINE_THREAD_ROUTINE_STACK(ATcodec_Commands_Client,data,ATCODEC_STACK_SIZE){    AT_CODEC_ERROR_CODE res;    int32_t v_loop;    v_continue = 1;    PRINT("Thread AT Commands Client Start/n");    while(!atcodec_lib_init_ok)    {        vp_os_thread_yield();    }    while(v_continue)    {        // open and init        if((res = func_ptrs.open()) != AT_CODEC_OPEN_OK)            v_continue = 0;        for ( v_loop = 1 ; v_loop ; )        {            // vp_os_delay(ATCODEC_SERVER_YIELD_DELAY);            vp_os_thread_yield();            // wait a successful ATcodec_Queue_... has been called            vp_os_mutex_lock(&ATcodec_cond_mutex);            //vp_os_cond_wait(&ATcodec_wait_cond);            //ATCODEC_PRINT("Must send /"%s/"/n", &ATcodec_Message_Buffer[0]);            if(ATcodec_Message_len && func_ptrs.write((int8_t*)&ATcodec_Message_Buffer[0], (int32_t*)&ATcodec_Message_len) != AT_CODEC_WRITE_OK)                v_loop = 0;            ATcodec_Message_len = 0;            vp_os_mutex_unlock(&ATcodec_cond_mutex);        }        // close and un-init : user-defined        if((res = func_ptrs.close()) != AT_CODEC_CLOSE_OK)            v_continue = 0;    }    if((res = func_ptrs.shutdown()) != AT_CODEC_SHUTDOWN_OK)    {        ATCODEC_PRINT("ATcodec Shutdown error/n");    }    return((THREAD_RET)0);}

void ardrone_at_set_led_animation ( LED_ANIMATION_IDS anim_id, float32_t freq, uint32_t duration_sec ){	float_or_int_t _freq;	if (!at_init)     return;		_freq.f = freq;	  vp_os_mutex_lock(&at_mutex);	ATcodec_Queue_Message_valist(ids.AT_MSG_ATCMD_LED_EXE,                                ++nb_sequence,                                anim_id,                                 _freq.i,                                 duration_sec);  vp_os_mutex_unlock(&at_mutex);}

/** * @param param A key as read from an ini file is given as "section:key". * * @param value * * @brief identified ardrone_at_set_toy_configuration * * @DESCRIPTION * *******************************************************************/void ardrone_at_set_toy_configuration_ids(const char* param,char* ses_id, char* usr_id, char* app_id,  const char* value){  if (!at_init)     return;  vp_os_mutex_lock(&at_mutex);  ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_CONFIG_IDS,                                ++nb_sequence,                                ses_id,                                usr_id,                                app_id );  ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_CONFIG_EXE,                                ++nb_sequence,                                param,                                value );  vp_os_mutex_unlock(&at_mutex);}

C_RESULTvp_stages_output_file_stage_transform(vp_stages_output_file_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out){  static int total_size = 0;  vp_os_mutex_lock(&out->lock);  if(in->status == VP_API_STATUS_PROCESSING && in->size > 0)    fwrite(in->buffers[in->indexBuffer], sizeof(int8_t), in->size*sizeof(int8_t), cfg->f);  fflush(cfg->f);  total_size += in->size;  out->status = in->status;  vp_os_mutex_unlock(&out->lock);  return (VP_SUCCESS);}

C_RESULT output_gtk_stage_transform( void *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out){     vp_os_mutex_lock(&video_update_lock);      /* Get a reference to the last decoded picture */     pixbuf_data      = (uint8_t*)in->buffers[0]; // we casten het naar een pointer van type uint8_t!!               // returns: A newly-created GdkPixbuf structure with a reference count of 1.     pixbuf = gdk_pixbuf_new_from_data(pixbuf_data,  // image data in 8-bit/sample packed format                                     GDK_COLORSPACE_RGB,                                      FALSE, // has alpha                                     8, // bits per sample                                     imageWidth,                                      imageHeight,                                     imageWidth * 3,  // distance in bites between rowstride                                     NULL, // GdkPixbufDestroyNotify detroy_fn                                     NULL); // gpoint destroy_fn_data);      sprintf(imageName, "images/frame%05d.jpg", ++imageCounter);     gdk_pixbuf_save(pixbuf, imageName, "jpeg", NULL, "quality", "100", NULL);          vp_os_mutex_unlock(&video_update_lock);     // Check if we need to switch the video feed to bottom or horizontal camera     if(videoSwitch != oldVideoSwitch)     {	     oldVideoSwitch = videoSwitch;	     if(videoSwitch > 0)         {	       	ardrone_at_zap(ZAP_CHANNEL_HORI);            imageWidth = 320;            imageHeight = 240;         }	     else         {            ardrone_at_zap(ZAP_CHANNEL_VERT);            imageWidth = 320;            imageHeight = 144;         }     }     return (SUCCESS);}

voidvp_os_thread_resume(THREAD_HANDLE handle){  vp_os_mutex_lock(&thread_mutex);  pthread_data_t* freeSlot = findThread( handle );  vp_os_mutex_unlock(&thread_mutex);  if( freeSlot != NULL )  {    if(freeSlot->isSleeping)    {      pthread_kill(handle,SIGRESUME);      freeSlot->isSleeping = 0;    }  }}

int8_t *videoServer_waitForNewFrame() {	/* Wait for a new available frame */	vp_os_mutex_lock(&frameBufferMutex);	//C_RESULT res = vp_os_cond_timed_wait(&frameBufferCond, MAX_FRAME_WAIT_SECONDS * 1000);	//if (SUCCEED(res) && availVideoBuffer != NULL)	vp_os_cond_wait(&frameBufferCond);	if (availVideoBuffer != NULL) {		((VideoFrameHeader *)frameBuffer)->videoData.timeCodeH = (unsigned int)(videoServer_lastFrameTimeCode >> 32);		((VideoFrameHeader *)frameBuffer)->videoData.timeCodeL = (unsigned int)(videoServer_lastFrameTimeCode & 0xFFFFFFFF);		((VideoFrameHeader *)frameBuffer)->videoData.encoding = videoServer_frameSourceEncoding;		((VideoFrameHeader *)frameBuffer)->videoData.width = videoServer_frameWidth;		((VideoFrameHeader *)frameBuffer)->videoData.height = videoServer_frameHeight;		((VideoFrameHeader *)frameBuffer)->videoData.dataLength = videoServer_framePacketLength - sizeof(VideoFrameHeader);		vp_os_memcpy(&frameBuffer[sizeof(VideoFrameHeader)], availVideoBuffer, videoServer_framePacketLength - sizeof(VideoFrameHeader));		vp_os_mutex_unlock(&frameBufferMutex);		availVideoBuffer = NULL;		return frameBuffer;	}

/* Receving navdata during the event loop */inline C_RESULT demo_navdata_client_process( const navdata_unpacked_t* const navdata ){	//printf("/033[%dA", show_state(NULL, navdata));    const navdata_demo_t *nd = &navdata->navdata_demo;	if (nd->altitude == 0) 			tag_on_the_ground = true;    else 			tag_on_the_ground = false;	fill_navdata_slot(navdata, &navdata_slot);	vp_os_mutex_lock(&NOW_mutex);	NOW.hori_angle = nd->psi;	//printf("now%lf/n psi%lf", NOW.hori_angle, nd->psi);	vp_os_mutex_unlock(&NOW_mutex);	return C_OK;}

voidvp_os_thread_suspend(THREAD_HANDLE handle){  vp_os_mutex_lock(&thread_mutex);  pthread_data_t* freeSlot = findThread( handle );  vp_os_mutex_unlock(&thread_mutex);  if( freeSlot != NULL )  {    if(!freeSlot->isSleeping)    {      freeSlot->isSleeping = 1;      pthread_kill(handle,SIGSUSPEND);    }  }}

void ardrone_at_set_manual_trims(float32_t trim_pitch, float32_t trim_roll, float32_t trim_yaw){  float_or_int_t _trim_pitch, _trim_roll, _trim_yaw;  if (!at_init)     return;  _trim_pitch.f = trim_pitch;  _trim_roll.f = trim_roll;  _trim_yaw.f = trim_yaw;  vp_os_mutex_lock(&at_mutex);  ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_MTRIM_EXE,                                 ++nb_sequence,                                _trim_pitch.i,                                 _trim_roll.i,                                 _trim_yaw.i);  vp_os_mutex_unlock(&at_mutex);}

voidvp_os_thread_create(THREAD_ROUTINE f, THREAD_PARAMS parameters, THREAD_HANDLE *handle, ...){  int32_t priority;  char* name;  void* stack_base;  unsigned int stack_size;  va_list va;  pthread_data_t* freeSlot = NULL;  pthread_once(&once,&init_thread);  vp_os_mutex_lock(&thread_mutex);  freeSlot = findThread( NULL_THREAD_HANDLE );  while(freeSlot == NULL)  {    int old_size = threadTabSize;    threadTabSize += 128;    threadTab = ( pthread_data_t* )vp_os_realloc( threadTab, threadTabSize * sizeof( pthread_data_t ) );    vp_os_memset( threadTab + old_size, 0, threadTabSize * sizeof( pthread_data_t ) );    freeSlot = findThread( NULL_THREAD_HANDLE );  }  vp_os_mutex_unlock(&thread_mutex);  pthread_attr_init( &freeSlot->attr );  va_start(va, handle);  priority    = va_arg(va, int32_t);  name        = va_arg(va, char *);  stack_base  = va_arg(va, void *);  stack_size  = va_arg(va, unsigned int);/*  thread = va_arg(va, cyg_thread *);*/  va_end(va);  pthread_create( &freeSlot->handle, &freeSlot->attr, f, parameters);  *handle = freeSlot->handle;  vp_os_thread_priority(freeSlot->handle, priority);}

voidvp_os_thread_join(THREAD_HANDLE handle){  PRINT("vp_os_thread_join/n %d/n", (int)handle);  vp_os_mutex_lock(&thread_mutex);  pthread_data_t* freeSlot = findThread( handle );  vp_os_mutex_unlock(&thread_mutex);  if( freeSlot != NULL )  {    void *res;    vp_os_memset(freeSlot, 0, sizeof(pthread_data_t));    pthread_join( handle, &res);  }}

void ardrone_at_set_vision_track_params( api_vision_tracker_params_t* params ){  if (!at_init)     return;  vp_os_mutex_lock(&at_mutex);  ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_VISP_EXE,                                ++nb_sequence,                                params->coarse_scale,                                params->nb_pair,                                params->loss_per,                                params->nb_tracker_width,                                params->nb_tracker_height,                                params->scale,                                params->trans_max,                                params->max_pair_dist,                                params->noise );  vp_os_mutex_unlock(&at_mutex);}