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