void p264_realloc_ref (video_controller_t* controller)
{
// realloc internal p264 buffers and make last decoded picture as the reference
p264_codec_t* video_codec;
video_codec = (p264_codec_t*)controller->video_codec;
if (controller->width != video_codec->ref_picture.width && controller->height != video_codec->ref_picture.height)
{
// resolution has changed, realloc buffers
video_codec->ref_picture.width = controller->width;
video_codec->ref_picture.height = controller->height;
video_codec->decoded_picture.width = controller->width;
video_codec->decoded_picture.height = controller->height;
// allocate a YUV 4:2:0 ref frame
video_codec->ref_picture.y_buf = (uint8_t*)vp_os_realloc(video_codec->ref_picture.y_buf,controller->width*controller->height*3/2);
video_codec->decoded_picture.y_buf = (uint8_t*)vp_os_realloc(video_codec->decoded_picture.y_buf,controller->width*controller->height*3/2);
if (video_codec->ref_picture.y_buf == NULL || video_codec->decoded_picture.y_buf == NULL)
{
PRINT("p264 ref realloc failed\n");
}
// fill cb/cr fields
video_codec->ref_picture.cb_buf = video_codec->ref_picture.y_buf + video_codec->ref_picture.width*video_codec->ref_picture.height;
video_codec->ref_picture.cr_buf = video_codec->ref_picture.cb_buf + (video_codec->ref_picture.width*video_codec->ref_picture.height)/4;
video_codec->ref_picture.y_line_size = video_codec->ref_picture.width;
video_codec->ref_picture.cb_line_size = video_codec->ref_picture.width>>1;
video_codec->ref_picture.cr_line_size = video_codec->ref_picture.width>>1;
video_codec->decoded_picture.cb_buf = video_codec->decoded_picture.y_buf + video_codec->decoded_picture.width*video_codec->decoded_picture.height;
video_codec->decoded_picture.cr_buf = video_codec->decoded_picture.cb_buf + (video_codec->decoded_picture.width*video_codec->decoded_picture.height)/4;
video_codec->decoded_picture.y_line_size = video_codec->decoded_picture.width;
video_codec->decoded_picture.cb_line_size = video_codec->decoded_picture.width>>1;
video_codec->decoded_picture.cr_line_size = video_codec->decoded_picture.width>>1;
}
void
ATcodec_Buffer_pushElements (ATcodec_Buffer_t *s, const void *elements, int nb)
{
void *oldPtr;
while ((s->nbElements+nb-1)*s->elementSize >= s->totalSize)
{
oldPtr = s->data;
s->totalSize <<= 1;
s->data = vp_os_realloc(s->data, s->totalSize);
if (s->data != oldPtr)
s->topElement = (void *)(((int)s->topElement-(int)oldPtr)+(int)s->data);
}
if (!s->nbElements)
{
s->topElement = (char *)s->data+(nb-1)*s->elementSize;
memcpy(s->data, elements, nb*s->elementSize);
}
else
{
memcpy((char *)s->topElement+s->elementSize, elements, nb*s->elementSize);
s->topElement = (void *)((char *)s->topElement+nb*s->elementSize);
}
s->nbElements += nb;
}
void
ATcodec_Buffer_pushElement (ATcodec_Buffer_t *s, const void *element)
{
void *oldPtr;
if (s->nbElements*s->elementSize == s->totalSize)
{
oldPtr = s->data;
s->totalSize <<= 1;
s->data = vp_os_realloc(s->data, s->totalSize);
if (s->data != oldPtr)
s->topElement = (void *)(((int)s->topElement-(int)oldPtr)+(int)s->data);
}
if (!s->nbElements++)
{
s->topElement = s->data;
}
else
{
s->topElement = (void *)((char *)s->topElement+s->elementSize);
}
memcpy(s->topElement, element, s->elementSize);
}
ardrone_users_t *ardrone_get_user_list(void)
{
if (0 == droneSupportsMulticonfig)
{
return NULL;
}
ardrone_users_t *retVal = vp_os_malloc (sizeof (ardrone_users_t));
if (NULL == retVal)
return NULL;
// Assume that userlist is up to date
int validUserCount = 0; // User whose descriptions start with a dot ('.') are hidden users that may not be shown to the application user (e.g. default user for each iPhone, or user specific to a control mode
int configIndex;
for (configIndex = 0; configIndex < available_configurations[CAT_USER].nb_configurations; configIndex++) // Check all existing user_ids
{
if ('.' != available_configurations[CAT_USER].list[configIndex].description[0]) // Not an hidden user
{
validUserCount++;
retVal->userList = vp_os_realloc (retVal->userList, validUserCount * sizeof (ardrone_user_t));
if (NULL == retVal->userList)
{
vp_os_free (retVal);
return NULL;
}
strncpy (retVal->userList[validUserCount-1].ident, available_configurations[CAT_USER].list[configIndex].id, MULTICONFIG_ID_SIZE);
strncpy (retVal->userList[validUserCount-1].description, available_configurations[CAT_USER].list[configIndex].description, USER_NAME_SIZE);
}
}
retVal->userCount = validUserCount;
return retVal;
}
void
vp_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);
}
C_RESULT video_com_stage_register (video_com_config_t *cfg)
{
static int runOnce = 1;
if (1 == runOnce)
{
vp_os_mutex_init (®isterMutex);
runOnce = 0;
}
vp_os_mutex_lock (®isterMutex);
nbRegisteredStages++;
registeredStages = vp_os_realloc (registeredStages, nbRegisteredStages * sizeof (video_com_config_t *));
C_RESULT retVal = C_FAIL;
if (NULL != registeredStages)
{
registeredStages[nbRegisteredStages - 1] = cfg;
retVal = C_OK;
}
vp_os_mutex_unlock (®isterMutex);
return retVal;
}
void ardrone_control_read_custom_configurations_list(/*in*/char * buffer,
/*in*/int buffer_size,
/*out*/custom_configuration_list_t *available_configurations)
{
custom_configuration_list_t * current_scope = NULL;
char id[CUSTOM_CONFIGURATION_ID_LENGTH+1];
char description[1024];
int index = 0;
char * pindex; int j;
char * end_of_buffer;
index = 0;
pindex = buffer;
end_of_buffer = buffer + buffer_size;
DEBUG_CONFIG_RECEIVE("Decoding %i bytes",buffer_size);
DEBUG_CONFIG_RECEIVE("\n");
while(1)
{
//DEBUG_CONFIG_RECEIVE("Analysing <"); for (i=index;i<buffer_size;i++) DEBUG_CONFIG_RECEIVE("[%i]",buffer[i]); DEBUG_CONFIG_RECEIVE(">\n");
/* Go to the beginning of a section */
while((*pindex)!='[') { index++; pindex++; if (pindex==end_of_buffer) return; }
/* Search the end of the section name */
for (;index<buffer_size;index++) { if (buffer[index]==13 ) { buffer[index]=0; break; } } if(index==buffer_size) return;
/* Search the corresponding category */
for (j=0;j<NB_CONFIG_CATEGORIES;j++){
if ( strcmp(custom_configuration_headers[j],pindex)==0 ){
/* Found the category */
current_scope = &available_configurations[j];
DEBUG_CONFIG_RECEIVE(" Found Scope <%s>\n",custom_configuration_headers[j]);
break;
}
}
if (j==NB_CONFIG_CATEGORIES) { DEBUG_CONFIG_RECEIVE("Unknown category."); return ;}
/* Reset the list */
if (current_scope!=NULL)
{
current_scope->nb_configurations = 0;
if (current_scope->list!=NULL) { vp_os_free(current_scope->list); current_scope->list = NULL; }
}
/* Points on the first ID */
index++;
pindex=buffer+index;
/* Read the IDs */
while(pindex<end_of_buffer && (*pindex)!='[' && (*pindex)!=0)
{
vp_os_memset(id,0,sizeof(id));
vp_os_memset(description,0,sizeof(description));
//DEBUG_CONFIG_RECEIVE("Now scanning <%c> %i\n",*pindex,index);
for (;index<buffer_size;index++) { if (buffer[index]==',' || buffer[index]=='\r') { buffer[index]=0; break; } } if(index==buffer_size) return;
strncpy(id,pindex,sizeof(id));
index++;
pindex=buffer+index;
for (;index<buffer_size;index++) { if (buffer[index]==0 || buffer[index]=='\r') { buffer[index]=0; break; } } if(index==buffer_size) return;
strncpy(description,pindex,sizeof(description));
DEBUG_CONFIG_RECEIVE(" Found ID <%s> description <%s>\n",id,description);
index++;
pindex=buffer+index;
/* Store the found ID */
/* Increase the size of the list by one element */
current_scope->list = vp_os_realloc(current_scope->list,sizeof(*current_scope->list)*(current_scope->nb_configurations+1));
/* Store the new element */
strncpy(current_scope->list[current_scope->nb_configurations].id,
id,
sizeof(current_scope->list[current_scope->nb_configurations].id) );
strncpy(current_scope->list[current_scope->nb_configurations].description,
description,
sizeof(current_scope->list[current_scope->nb_configurations].description) );
current_scope->nb_configurations++;
}
}
return;
}
/**
* \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;
}
C_RESULT video_stage_merge_slices_transform(video_stage_merge_slices_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
bool_t switchBuffers;
bool_t dataReady = 0;
video_stage_merge_slices_buffer_t *buf;
int dataSize;
parrot_video_encapsulation_t *PaVE;
parrot_video_encapsulation_t *mergingPaVE;
out->size = 0;
if(out->status == VP_API_STATUS_INIT)
{
/* By default, feed the next stage with our local output */
out->numBuffers = 1;
out->buffers = cfg->bufs[0].bufferPointer;
out->buffers[0] = cfg->bufs[0].buffer;
out->indexBuffer = 0;
out->lineSize = 0;
out->status = VP_API_STATUS_PROCESSING;
}
// check input
PaVE = (parrot_video_encapsulation_t *) in->buffers[in->indexBuffer];
mergingPaVE = (parrot_video_encapsulation_t *) cfg->bufs[cfg->mergingBuffer].buffer;
VIDEO_SLICE_DEBUG("PAVE frm %d sl %d/%d %s\n ",
PaVE->frame_number,
PaVE->slice_index+1,
PaVE->total_slices,
(PaVE->frame_type == FRAME_TYPE_I_FRAME || PaVE->frame_type == FRAME_TYPE_IDR_FRAME) ? "I-frm":"p-frm" );
if ( (PaVE==NULL) || (!PAVE_CHECK(PaVE)) || (PaVE->total_slices == 1) )
{
// No slices, just send the data to the next stage
/* Feed the next stage with the previous stage's output */
out->buffers = in->buffers;
out->indexBuffer = in->indexBuffer;
out->lineSize = in->lineSize;
out->numBuffers = in->numBuffers;
out->status = in->status;
out->size = in->size;
out->status = VP_API_STATUS_PROCESSING;
/* Get rid of previously accumulated data */
video_stage_merge_slices_reset(cfg);
/* IPHONE DEBUG : set slices miss to 0/1 (to avoid keeping old/fake slice datas) */
DEBUG_totalSlices = 1;
DEBUG_nbSlices = 0;
return C_OK;
}
/*
* Check if the incoming PaVE belongs to the same frame.
*/
switchBuffers = (mergingPaVE) /* At program startup this buffer is empty and switching makes no sense */
&& ( !(pave_is_same_frame(PaVE,mergingPaVE)) );
/* If not the same frame, start building a new one */
if (switchBuffers)
{
if (0 != DEBUG_prevNumber)
{
int DEBUG_tempMiss = (PaVE->frame_number - (DEBUG_prevNumber + 1));
if (0 < DEBUG_tempMiss)
{
DEBUG_missed += DEBUG_tempMiss;
}
}
DEBUG_prevNumber = PaVE->frame_number;
cfg->readyBuffer = cfg->mergingBuffer;
cfg->mergingBuffer = (cfg->mergingBuffer+1)%2;
cfg->bufs[cfg->mergingBuffer].accumulated_size = 0;
cfg->bufs[cfg->mergingBuffer].nb_slices = 0;
}
/* Get a pointer to the buffer to store data to */
buf = &cfg->bufs[cfg->mergingBuffer];
// Check destination buffer size
if (buf->buffer_size < (buf->accumulated_size + in->size))
{
buf->buffer_size = buf->accumulated_size + in->size;
buf->buffer = vp_os_realloc(buf->buffer, buf->buffer_size );
if(!buf->buffer) { return C_FAIL; }
}
/* Copy data */
if (PaVE->slice_index == 0 || buf->accumulated_size==0)
{
/* Copy the entire packet */
if (buf->buffer) { vp_os_memcpy(buf->buffer, PaVE, in->size); }
buf->accumulated_size = in->size;
buf->nb_slices++;
}
else
{
#define mymin(x,y) ( ((x)<(y))?(x):(y) )
/* Copy only the payload */
dataSize = mymin(PaVE->payload_size,in->size); /* safety if the PaVE is corrupted */
if (buf->buffer) { vp_os_memcpy(buf->buffer + buf->accumulated_size, in->buffers[in->indexBuffer] + PaVE->header_size, dataSize); }
buf->accumulated_size += dataSize;
buf->nb_slices++;
}
/* Select the buffer to send to the next stage */
if (PaVE->slice_index == PaVE->total_slices - 1)
{
/* The buffer we just used for merging is ready to be sent */
cfg->readyBuffer = cfg->mergingBuffer;
cfg->mergingBuffer = (cfg->mergingBuffer+1)%2;
cfg->bufs[cfg->mergingBuffer].accumulated_size = 0;
cfg->bufs[cfg->mergingBuffer].nb_slices = 0;
dataReady = 1;
}
else if (switchBuffers)
{
/* Send the previous buffer if it contains something */
dataReady = 1;
}
// If slice was the last one, continue to next stage
if (dataReady)
{
buf = &cfg->bufs[cfg->readyBuffer];
if (buf->buffer && buf->accumulated_size)
{
int totalSlices;
parrot_video_encapsulation_t *newPaVE = (parrot_video_encapsulation_t *) buf->buffer;
/* Save the old value of total slices */
totalSlices = newPaVE->total_slices;
newPaVE->payload_size = buf->accumulated_size - PaVE->header_size;
newPaVE->slice_index = 0;
newPaVE->total_slices = 1;
/* Feed the next stage with our local output */
out->size = buf->accumulated_size;
out->buffers = buf->bufferPointer;
out->buffers[0] = buf->buffer;
out->indexBuffer = 0;
out->numBuffers = 1;
out->lineSize = 0;
out->status = VP_API_STATUS_PROCESSING;
if(!PAVE_CHECK(buf->buffer)) { printf("%s:%d - No PaVE !\n",__FUNCTION__,__LINE__); assert(0==1); }
if (buf->nb_slices != totalSlices) { printf("Missing slices (%d)\n",totalSlices-buf->nb_slices); }
DEBUG_nbSlices = 0.9*DEBUG_nbSlices + 0.1*(totalSlices-buf->nb_slices);
DEBUG_totalSlices = 0.9*DEBUG_totalSlices + 0.1*totalSlices;
}
}
else
{
out->size = 0;
}
if (out->size)
{
PaVE = (parrot_video_encapsulation_t*) out->buffers[0];
VIDEO_SLICE_DEBUG("Switch %d - Sending - PAVE frm %d sl %d/%d\n ",
switchBuffers,
PaVE->frame_number,
PaVE->slice_index+1,
PaVE->total_slices );}
return C_OK;
}
vp_com_config_t* wifi_config(void)
{
static vp_com_wifi_config_t config =
{
{ 0 },
WIFI_MOBILE_IP,
WIFI_NETMASK,
WIFI_BROADCAST,
WIFI_GATEWAY,
WIFI_SERVER,
WIFI_INFRASTRUCTURE,
WIFI_SECURE,
WIFI_PASSKEY,
{ 0 },
};
unsigned char *u;
int sockfd, size = 1;
struct ifreq *ifr;
struct ifconf ifc;
struct sockaddr_in sa;
if (0 > (sockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_IP))) {
fprintf(stderr, "Cannot open socket.\n");
exit(EXIT_FAILURE);
}
ifc.ifc_len = IFRSIZE;
ifc.ifc_req = NULL;
do {
++size;
/* realloc buffer size until no overflow occurs */
if (NULL == (ifc.ifc_req = vp_os_realloc(ifc.ifc_req, IFRSIZE))) {
fprintf(stderr, "Out of memory.\n");
exit(EXIT_FAILURE);
}
ifc.ifc_len = IFRSIZE;
if (ioctl(sockfd, SIOCGIFCONF, &ifc)) {
perror("ioctl SIOCFIFCONF");
exit(EXIT_FAILURE);
}
} while (IFRSIZE <= ifc.ifc_len);
ifr = ifc.ifc_req;
for (; (char *) ifr < (char *) ifc.ifc_req + ifc.ifc_len; ++ifr) {
if (ifr->ifr_addr.sa_data == (ifr + 1)->ifr_addr.sa_data) {
continue; /* duplicate, skip it */
}
if (ioctl(sockfd, SIOCGIFFLAGS, ifr)) {
continue; /* failed to get flags, skip it */
}
struct in_addr addr;
addr = inaddrr(ifr_addr.sa_data);
if ( (ntohl(addr.s_addr) & 0xFFFFFF00) == WIFI_BASE_ADDR )
{
inet_ntop(AF_INET, &addr, config.localHost, VP_COM_NAME_MAXSIZE);
//INTERFACE
memcpy(config.itfName, ifr->ifr_name, strlen(ifr->ifr_name));
//IP ADDRESS
addr.s_addr = htonl ( ntohl(addr.s_addr) - ( ( ( ntohl(addr.s_addr) & 0xFF ) - 1 ) % 5 ) );
memcpy(config.server, inet_ntoa(addr), strlen(inet_ntoa(addr)));
//NET MASK
if (0 == ioctl(sockfd, SIOCGIFNETMASK, ifr) && strcmp(
"255.255.255.255", inet_ntoa(inaddrr(ifr_addr.sa_data)))) {
addr = inaddrr(ifr_addr.sa_data);
inet_ntop(AF_INET, &addr, config.netmask, VP_COM_NAME_MAXSIZE);
}
//BROADCAST ADDRESS
if (ifr->ifr_flags & IFF_BROADCAST) {
if (0 == ioctl(sockfd, SIOCGIFBRDADDR, ifr) && strcmp("0.0.0.0",
inet_ntoa(inaddrr(ifr_addr.sa_data)))) {
addr = inaddrr(ifr_addr.sa_data);
inet_ntop(AF_INET, &addr, config.broadcast, VP_COM_NAME_MAXSIZE);
}
}
break;
}
}
close(sockfd);
return (vp_com_config_t*)&config;
}
C_RESULT video_transform (video_cfg_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
// Realloc frameBuffer if needed
if (in->size != cfg->fbSize)
{
cfg->frameBuffer = vp_os_realloc (cfg->frameBuffer, in->size);
cfg->fbSize = in->size;
}
// Copy last frame to frameBuffer
vp_os_memcpy (cfg->frameBuffer, in->buffers[in->indexBuffer], cfg->fbSize);
// Convert BRG to RGB
int k;
for (k=0; k<cfg->fbSize; k+=3) {
uint8_t tmp = cfg->frameBuffer[k];
cfg->frameBuffer[k] = cfg->frameBuffer[k+2];
cfg->frameBuffer[k+2] = tmp;
}
// Grab image width from data structure
int img_width = cfg->width;
int img_height = cfg->height;
// If belly camera indicated on AR.Drone 1.0, move camera data to beginning
// of buffer
if (!IS_ARDRONE2 && g_is_bellycam) {
int j;
for (j=0; j<QCIF_HEIGHT; ++j) {
memcpy(&cfg->frameBuffer[j*QCIF_WIDTH*3],
&cfg->frameBuffer[j*QVGA_WIDTH*3],
QCIF_WIDTH*3);
}
img_width = QCIF_WIDTH;
img_height = QCIF_HEIGHT;
}
// normalize navdata angles to (-1,+1)
g_navdata.phi /= 100000;
g_navdata.theta /= 100000;
g_navdata.psi /= 180000;
// Init new commands
commands_t commands;
// Call the Python (or Matlab or C) agent's action routine
agent_act(cfg->frameBuffer, img_width, img_height, g_is_bellycam, &g_navdata, &commands);
if (g_autopilot) {
set(commands.phi, commands.theta, commands.gaz, commands.yaw);
if (commands.zap) {
zap();
}
}
// Tell the pipeline that we don't have any output
out->size = 0;
return C_OK;
}
