C_RESULT video_stage_merge_slices_close(video_stage_merge_slices_config_t *cfg) { int idx; video_stage_merge_slices_buffer_t *buf; for (idx=0;idx<2;idx++) { buf = &cfg->bufs[idx]; if (buf->buffer) { vp_os_sfree((void**)&buf->buffer); } if (buf->bufferPointer) { vp_os_sfree((void**)&buf->bufferPointer); } } return C_OK; }
C_RESULT video_stage_merge_slices_reset(video_stage_merge_slices_config_t *cfg) { int idx; for (idx=0;idx<2;idx++) { if (cfg->bufs[idx].buffer) { vp_os_sfree((void**)&(cfg->bufs[idx].buffer)); } cfg->bufs[idx].accumulated_size =0; cfg->bufs[idx].buffer_size = 0; cfg->bufs[idx].nb_slices = 0; } cfg->mergingBuffer = 0; cfg->readyBuffer = 0; return C_OK; }
/** * \brief Runs a control event managing the configuration file. * This functions handles reading the configuration information * sent by the drone on the TCP 'control' socket on port 5559. * Called by the 'ardrone_control' thread loop in 'ardrone_control.c'. */ C_RESULT ardrone_control_configuration_run( uint32_t ardrone_state, ardrone_control_configuration_event_t* event ) { int32_t buffer_size; char *start_buffer, *buffer; char *value, *param, *c; bool_t ini_buffer_end, ini_buffer_more; C_RESULT res = C_OK; int bytes_to_read; /* Multiconfiguration support */ static char *custom_configuration_list_buffer = NULL; static int custom_configuration_list_buffer_size = 0; static int custom_configuration_list_data_size = 0; #define CUSTOM_CFG_BLOCK_SIZE 1024 int i; switch( event->config_state ) { case CONFIG_REQUEST_INI: if( ardrone_state & ARDRONE_COMMAND_MASK ) { /* If the ACK bit is set, we must ask the drone to clear it before asking the configuration. */ DEBUG_CONFIG_RECEIVE("%s %s %i - Requesting ack. bit reset.\n",__FILE__,__FUNCTION__,__LINE__); ardrone_at_update_control_mode(ACK_CONTROL_MODE, 0); } else { DEBUG_CONFIG_RECEIVE("%s %s %i - Requesting the configuration.\n",__FILE__,__FUNCTION__,__LINE__); ini_buffer_index = 0; ardrone_at_configuration_get_ctrl_mode(); set_state(event, CONFIG_RECEIVE_INI); } break; /* multi-configuration support */ case CUSTOM_CONFIG_REQUEST: DEBUG_CONFIG_RECEIVE("%s %s %i\n",__FILE__,__FUNCTION__,__LINE__); if( ardrone_state & ARDRONE_COMMAND_MASK ) { DEBUG_CONFIG_RECEIVE("%s %s %i - Requesting ack. bit reset.\n",__FILE__,__FUNCTION__,__LINE__); ardrone_at_update_control_mode(ACK_CONTROL_MODE, 0); } else { DEBUG_CONFIG_RECEIVE("%s %s %i - Requesting the custom config.\n",__FILE__,__FUNCTION__,__LINE__); custom_configuration_list_buffer = NULL; custom_configuration_list_buffer_size = 0; custom_configuration_list_data_size = 0; ardrone_at_custom_configuration_get_ctrl_mode(); set_state(event, CUSTOM_CONFIG_RECEIVE); } break; case CONFIG_RECEIVE_INI: DEBUG_CONFIG_RECEIVE("%s %s %i - Trying to read the control socket.\n",__FILE__,__FUNCTION__,__LINE__); // Read data coming from ARDrone buffer_size = ARDRONE_CONTROL_CONFIGURATION_INI_BUFFER_SIZE - ini_buffer_index; res = ardrone_control_read( &ini_buffer[ini_buffer_index], &buffer_size ); DEBUG_CONFIG_RECEIVE("Received <<%s>>\n",&ini_buffer[ini_buffer_index]); if(VP_SUCCEEDED(res)) { buffer_size += ini_buffer_index; ini_buffer[buffer_size]=0; // Makes sure the buffer ends with a zero // Parse received data if( buffer_size > 0 ) { //DEBUG_CONFIG_RECEIVE(" Searching value\n"); ini_buffer_end = ini_buffer[buffer_size-1] == '\0'; // End of configuration data ini_buffer_more = ini_buffer[buffer_size-1] != '\n'; // Need more configuration data to end parsing current line //if (ini_buffer_end) DEBUG_CONFIG_RECEIVE(" ini_buffer_end\n"); //if (ini_buffer_more) DEBUG_CONFIG_RECEIVE(" ini_buffer_more\n"); start_buffer = (char*)&ini_buffer[0]; buffer = strchr(start_buffer, '\n'); //DEBUG_CONFIG_RECEIVE("Le start buffer : <<%s>>\n",start_buffer); while( buffer != NULL ) { //DEBUG_CONFIG_RECEIVE(" Found an '\\n' \n"); value = start_buffer; param = strchr(value, '='); *buffer = '\0'; *param = '\0'; // Remove spaces at end of strings c = param - 1; while( *c == ' ' ) { *c = '\0'; c = c-1; } c = buffer-1; while( *c == ' ' ) { *c = '\0'; c = c-1; } // Remove spaces at beginning of strings while( *value == ' ' ) { value = value + 1; } param = param + 1; while( *param == ' ' ) { param = param + 1; } DEBUG_CONFIG_RECEIVE(" Decoding from control stream : <%s>=<%s>\n",value,param); iniparser_setstring( event->ini_dict, value, param ); start_buffer = buffer + 1; buffer = strchr(start_buffer, '\n'); } if( ini_buffer_end ) { /* Reading the condfiguration is finished, we ask the drone * to clear the ACK bit */ ardrone_at_update_control_mode(ACK_CONTROL_MODE, 0); set_state(event, CONFIG_RECEIVED); } else if( ini_buffer_more ) { // Compute number of bytes to copy int32_t size = (int32_t)&ini_buffer[buffer_size] - (int32_t)start_buffer; vp_os_memcpy( &ini_buffer[0], start_buffer, size ); ini_buffer_index = size; } else { /* The previous line was completely consumed - next data * from the network will be stored at the beginning of the * buffer. */ ini_buffer_index = 0; } } } else { res = C_FAIL; DEBUG_CONFIG_RECEIVE("%s %s %i - no data to read from the control socket.\n",__FILE__,__FUNCTION__,__LINE__); if(!(ardrone_state & ARDRONE_COMMAND_MASK)) set_state(event, CONFIG_REQUEST_INI); } break; case CUSTOM_CONFIG_RECEIVE: DEBUG_CONFIG_RECEIVE("%s %s %i - Trying to read the control socket.\n",__FILE__,__FUNCTION__,__LINE__); DEBUG_CONFIG_RECEIVE("%s %s %i\n",__FILE__,__FUNCTION__,__LINE__); /* Read data until a zero byte is received */ /* Clear old data from the buffer when receiving the first bytes */ if (custom_configuration_list_buffer!=NULL && custom_configuration_list_data_size==0){ vp_os_memset(custom_configuration_list_buffer,0,custom_configuration_list_buffer_size); } /* Enlarge the buffer if needed */ if (custom_configuration_list_data_size==custom_configuration_list_buffer_size) { custom_configuration_list_buffer_size += CUSTOM_CFG_BLOCK_SIZE; custom_configuration_list_buffer = vp_os_realloc(custom_configuration_list_buffer,custom_configuration_list_buffer_size); } /* Read data at the end of the buffer */ bytes_to_read = custom_configuration_list_buffer_size - custom_configuration_list_data_size; res = ardrone_control_read( &custom_configuration_list_buffer[custom_configuration_list_data_size], &bytes_to_read ); DEBUG_CONFIG_RECEIVE(" Reading %i bytes of the custom config. list\n",bytes_to_read); for (i=0;i<bytes_to_read;i++) { DEBUG_CONFIG_RECEIVE("<%i>",custom_configuration_list_buffer[i]); } /* Searches a zero */ if (VP_SUCCEEDED(res)) { custom_configuration_list_data_size += bytes_to_read; for (i=bytes_to_read;i>0;i--){ if (custom_configuration_list_buffer[custom_configuration_list_data_size - i] == 0) { /* Reading the condfiguration is finished, we ask the drone * to clear the ACK bit */ DEBUG_CONFIG_RECEIVE("Finished receiving\n"); ardrone_at_update_control_mode(ACK_CONTROL_MODE, 0); set_state(event, CUSTOM_CONFIG_RECEIVED); ardrone_control_reset_custom_configurations_list(available_configurations); ardrone_control_read_custom_configurations_list(custom_configuration_list_buffer, custom_configuration_list_data_size, available_configurations); /* Clean up */ vp_os_sfree((void *)&custom_configuration_list_buffer); custom_configuration_list_buffer_size = 0; custom_configuration_list_data_size = 0; res = C_OK; break; } } } else{ /* No data are available on the control socket. */ DEBUG_CONFIG_RECEIVE("%s %s %i - no data to read from the control socket.\n",__FILE__,__FUNCTION__,__LINE__); /* Reset the buffer */ custom_configuration_list_data_size = 0; /* The request for the configuration file should have been acknowledged by the drone. * If not, another request is sent. */ if(!(ardrone_state & ARDRONE_COMMAND_MASK)) set_state(event, CUSTOM_CONFIG_REQUEST); } break; case CONFIG_RECEIVED: case CUSTOM_CONFIG_RECEIVED: /* We finished reading the configuration, we wait for the drone to reset the ACK bit */ if( ardrone_state & ARDRONE_COMMAND_MASK ) { /* If the ACK bit is set, we must ask the drone to clear it before asking the configuration. */ PRINT("%s %s %i - Requesting ack. bit reset.\n",__FILE__,__FUNCTION__,__LINE__); ardrone_at_update_control_mode(ACK_CONTROL_MODE, 0); } else { PRINT("%s %s %i - Finished.\n",__FILE__,__FUNCTION__,__LINE__); event->status = ARDRONE_CONTROL_EVENT_FINISH_SUCCESS; } res = C_OK; break; default: res = C_FAIL; break; } return res; }