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