static C_RESULT vp_api_get_message(vp_api_io_pipeline_t *pipeline, DEST_HANDLE *dest, PIPELINE_MSG *msg_id, void **callback, void **param)
{
C_RESULT res = SUCCESS;
VP_OS_ASSERT(pipeline->fifo.nb_waiting > 0);
if((pipeline->fifo.pget + sizeof(PIPELINE_MSG) + sizeof(void *) + sizeof(void *)) >= (pipeline->fifo.pbase + VP_API_PIPELINE_FIFO_SIZE))
pipeline->fifo.pget = pipeline->fifo.pbase;
if(dest != NULL)
vp_os_memcpy(dest, pipeline->fifo.pget, sizeof(DEST_HANDLE));
pipeline->fifo.pget += sizeof(DEST_HANDLE);
if(msg_id != NULL)
vp_os_memcpy(msg_id, pipeline->fifo.pget, sizeof(PIPELINE_MSG));
pipeline->fifo.pget += sizeof(PIPELINE_MSG);
if(callback != NULL)
vp_os_memcpy(callback, pipeline->fifo.pget, sizeof(void *));
pipeline->fifo.pget += sizeof(void *);
if(param != NULL)
vp_os_memcpy(param, pipeline->fifo.pget, sizeof(void *));
pipeline->fifo.pget += sizeof(void *);
pipeline->fifo.nb_waiting --;
return res;
}
C_RESULT
picture_to_buffer_transform(buffer_to_picture_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
vp_os_mutex_lock(&out->lock);
if(out->status == VP_API_STATUS_INIT)
{
out->numBuffers = 1;
out->size = (ACQ_WIDTH*ACQ_HEIGHT*3)/2;
out->buffers = (int8_t **) vp_os_malloc(out->size*sizeof(int8_t) + sizeof(int8_t*));
out->indexBuffer = 0;
out->status = VP_API_STATUS_PROCESSING;
out->buffers[0] = (int8_t *)(out->buffers+1);
}
if(out->status == VP_API_STATUS_PROCESSING)
{
if( in->size == 1 )
{
// got a picture
vp_os_memcpy( out->buffers[0], cfg->picture->y_buf, ACQ_WIDTH*ACQ_HEIGHT );
vp_os_memcpy( out->buffers[0] + ACQ_WIDTH*ACQ_HEIGHT, cfg->picture->cb_buf, ACQ_WIDTH*ACQ_HEIGHT/4);
vp_os_memcpy( out->buffers[0] + ACQ_WIDTH*ACQ_HEIGHT + ACQ_WIDTH*ACQ_HEIGHT/4, cfg->picture->cr_buf, ACQ_WIDTH*ACQ_HEIGHT/4);
}
}
// out->status = in->status;
vp_os_mutex_unlock(&out->lock);
return (SUCCESS);
}
void
ATcodec_Buffer_pushElements (ATcodec_Buffer_t *s, const void *elements, int nb)
{
void *oldPtr;
VP_OS_ASSERT(nb>=0);
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);
s->topElement = (void *)((char*)s->topElement - (char*)oldPtr + (char*)s->data);
}
if (!s->nbElements)
{
s->topElement = (char *)s->data+(nb-1)*s->elementSize;
vp_os_memcpy(s->data, elements, nb*s->elementSize);
}
else
{
vp_os_memcpy((char *)s->topElement+s->elementSize, elements, nb*s->elementSize);
s->topElement = (void *)((char *)s->topElement+nb*s->elementSize);
}
s->nbElements += nb;
}
C_RESULT
buffer_to_picture_transform(buffer_to_picture_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
vp_os_mutex_lock(&out->lock);
if(out->status == VP_API_STATUS_INIT)
{
out->numBuffers = 1;
out->size = (ACQ_WIDTH*ACQ_HEIGHT*3)/2;
out->buffers = (int8_t **) cfg->picture;
out->indexBuffer = 0;
out->status = VP_API_STATUS_PROCESSING;
}
if(out->status == VP_API_STATUS_ENDED)
{
}
if(out->status == VP_API_STATUS_PROCESSING)
{
vp_os_memcpy( cfg->picture->y_buf, in->buffers[0], ACQ_WIDTH*ACQ_HEIGHT );
vp_os_memcpy( cfg->picture->cb_buf, in->buffers[0] + ACQ_WIDTH*ACQ_HEIGHT, ACQ_WIDTH*ACQ_HEIGHT/4 );
vp_os_memcpy( cfg->picture->cr_buf, in->buffers[0] + ACQ_WIDTH*ACQ_HEIGHT + ACQ_WIDTH*ACQ_HEIGHT/4, ACQ_WIDTH*ACQ_HEIGHT/4 );
}
out->status = in->status;
vp_os_mutex_unlock(&out->lock);
return (SUCCESS);
}
C_RESULT vp_api_post_message(DEST_HANDLE dest, PIPELINE_MSG msg_id, void *callback, void *param)
{
C_RESULT res = SUCCESS;
vp_api_io_pipeline_t *pipeline = (vp_api_io_pipeline_t *) pipelines[dest.pipeline];
VP_OS_ASSERT(pipeline->fifo.nb_waiting >= 0);
if((pipeline->fifo.ppost + sizeof(PIPELINE_MSG) + sizeof(void *) + sizeof(void *)) >= (pipeline->fifo.pbase + VP_API_PIPELINE_FIFO_SIZE))
pipeline->fifo.ppost = pipeline->fifo.pbase;
vp_os_memcpy(pipeline->fifo.ppost, &dest, sizeof(DEST_HANDLE));
pipeline->fifo.ppost += sizeof(DEST_HANDLE);
vp_os_memcpy(pipeline->fifo.ppost, &msg_id, sizeof(PIPELINE_MSG));
pipeline->fifo.ppost += sizeof(PIPELINE_MSG);
if(callback != NULL)
vp_os_memcpy(pipeline->fifo.ppost, &callback, sizeof(void *));
else
vp_os_memset(pipeline->fifo.ppost, 0, sizeof(void *));
pipeline->fifo.ppost += sizeof(void *);
if(param != NULL)
vp_os_memcpy(pipeline->fifo.ppost, ¶m, sizeof(void *));
else
vp_os_memset(pipeline->fifo.ppost, 0, sizeof(void *));
pipeline->fifo.ppost += sizeof(void *);
pipeline->fifo.nb_waiting ++;
return res;
}
static inline bool_t check_and_copy_PaVE(parrot_video_encapsulation_t *PaVE, vp_api_io_data_t *data, parrot_video_encapsulation_t *prevPaVE, bool_t *dimChanged) {
parrot_video_encapsulation_t *localPaVE = (parrot_video_encapsulation_t *) data->buffers[data->indexBuffer];
if (localPaVE->signature[0] == 'P' &&
localPaVE->signature[1] == 'a' &&
localPaVE->signature[2] == 'V' &&
localPaVE->signature[3] == 'E') {
//ITTIAM_DEBUG_PRINT("Found a PaVE");
vp_os_memcpy(prevPaVE, PaVE, sizeof (parrot_video_encapsulation_t)); // Make a backup of previous PaVE so we can check if things have changed
vp_os_memcpy(PaVE, localPaVE, sizeof (parrot_video_encapsulation_t)); // Copy PaVE to our local one
#if ITTIAM_DEBUG_ENABLED
//printf ("PREV : ");
//ITTIAM_dumpPave (prevPaVE);
printf("CURR : ");
ITTIAM_dumpPave(PaVE);
#endif
if (prevPaVE->encoded_stream_width != PaVE->encoded_stream_width ||
prevPaVE->encoded_stream_height != PaVE->encoded_stream_height ||
prevPaVE->display_width != PaVE->display_width ||
prevPaVE->display_width != PaVE->display_width) {
*dimChanged = TRUE;
} else {
*dimChanged = FALSE;
}
data->size = localPaVE->payload_size;
memmove(data->buffers[data->indexBuffer], &(data->buffers[data->indexBuffer])[localPaVE->header_size], data->size);
#if DISPLAY_DROPPED_FRAMES
missed_frames += PaVE->frame_number - prevPaVE->frame_number - 1;
#endif
return TRUE;
} else {
ITTIAM_DEBUG_PRINT("No PaVE, signature was [%c][%c][%c][%c]", localPaVE->signature[0]
, localPaVE->signature[1]
, localPaVE->signature[2]
, localPaVE->signature[3]);
#if FAKE_PaVE
PaVE->encoded_stream_width = 1280; //640;
PaVE->encoded_stream_height = 720; //368;
PaVE->display_width = 1280; //640;
PaVE->display_height = 720; //360;
PaVE->video_codec = FAKE_PaVE_CODEC;
PaVE->frame_type = FRAME_TYPE_I_FRAME;
vp_os_memcpy(prevPaVE, PaVE, sizeof (parrot_video_encapsulation_t));
*dimChanged = FALSE;
return TRUE;
#else
return FALSE;
#endif
}
}
C_RESULT ardrone_tool_init( const char* ardrone_ip, size_t n, AT_CODEC_FUNCTIONS_PTRS *ptrs, const char *appname, const char *usrname)
{
// Initalize mutex and condition
vp_os_mutex_init(&ardrone_tool_mutex);
ardrone_tool_in_pause = FALSE;
// Initialize ardrone_control_config structures;
ardrone_tool_reset_configuration();
// ardrone_control_config_default initialisation. Sould not be modified after that !
vp_os_memcpy ((void *)&ardrone_control_config_default, (const void *)&ardrone_control_config, sizeof (ardrone_control_config_default));
// initialization of application defined default values
vp_os_memcpy ((void *)&ardrone_application_default_config, (const void *)&ardrone_control_config, sizeof (ardrone_application_default_config));
//Fill structure AT codec and built the library AT commands.
if( ptrs != NULL )
ardrone_at_init_with_funcs( ardrone_ip, n, ptrs );
else
ardrone_at_init( ardrone_ip, n );
// Save appname/appid for reconnections
if (NULL != appname)
{
ardrone_gen_appid (appname, __SDK_VERSION__, app_id, app_name, sizeof (app_name));
}
// Save usrname/usrid for reconnections
if (NULL != usrname)
{
ardrone_gen_usrid (usrname, usr_id, usr_name, sizeof (usr_name));
}
// Create pseudorandom session id
ardrone_gen_sessionid (ses_id, ses_name, sizeof (ses_name));
// Init subsystems
ardrone_timer_reset(&ardrone_tool_timer);
ardrone_timer_update(&ardrone_tool_timer);
ardrone_tool_input_init();
ardrone_control_init();
ardrone_tool_configuration_init();
ardrone_navdata_client_init();
//Opens a connection to AT port.
ardrone_at_open();
START_THREAD(navdata_update, 0);
START_THREAD(ardrone_control, 0);
// Send start up configuration
ardrone_at_set_pmode( MiscVar[0] );
ardrone_at_set_ui_misc( MiscVar[0], MiscVar[1], MiscVar[2], MiscVar[3] );
return C_OK;
}
static inline C_RESULT ardrone_navdata_process( const navdata_unpacked_t* const navdata )
{
if( writeToFile )
{
if( navdata_file == NULL )
{
ardrone_navdata_file_init(root_dir);
PRINT("Saving in %s file\n", root_dir);
}
ardrone_navdata_file_process( navdata );
}
else
{
if(navdata_file != NULL)
ardrone_navdata_file_release();
}
vp_os_mutex_lock( &inst_nav_mutex);
/* inst_nav.ardrone_state = navdata->ardrone_state;
inst_nav.vision_defined = navdata->vision_defined;
vp_os_memcpy(&inst_nav.navdata_demo, &navdata->navdata_demo, sizeof(navdata_demo_t));
vp_os_memcpy(&inst_nav.navdata_vision_detect, &navdata->navdata_vision_detect, sizeof(navdata_vision_detect_t));
*/
vp_os_memcpy(&inst_nav, navdata, sizeof(navdata_unpacked_t));
vp_os_mutex_unlock( &inst_nav_mutex );
return C_OK;
}
C_RESULT configManager_setParameter(uint32_t id, uint8_t *value, uint32_t valueLength, bool_t fromNetwork, bool_t verbose) {
if (id < 0 || id >= CONFIGPARAM_NUM_PARAMS) {
PRINT("[ConfigManager] Bad parameter ID when calling setParam()\n");
return C_FAIL;
}
if (configManager_params[id].valueLength != valueLength) {
PRINT("[ConfigManager] Error setting parameter \"%s\". The value length is not correct\n", configManager_params[id].name);
return C_FAIL;
}
vp_os_memcpy(configManager_params[id].value, value, valueLength);
/* Reorder some types into host order */
if (verbose) PRINT("[ConfigManager] New value set for parameter \"%s\" : ", configManager_params[id].name);
switch(configManager_params[id].valueType) {
case VALUETYPE_INT32:
if (fromNetwork)
*((uint32_t *)configManager_params[id].value) = ntohl(*((uint32_t *)configManager_params[id].value));
if (verbose) PRINT("%d\n", *((uint32_t *)configManager_params[id].value));
break;
case VALUETYPE_FLOAT:
if (fromNetwork)
float_ntoh((float *)configManager_params[id].value);
if (verbose) PRINT("%f\n", *((float *)configManager_params[id].value));
break;
case VALUETYPE_BOOL:
if (verbose) PRINT("%s\n", configManager_params[id].value[0] != 0 ? "TRUE" : "FALSE");
break;
default:
if (verbose) PRINT("Array\n");
}
/* Send value to the drone */
configManager_applyValue(id);
return C_OK;
}
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 *)(((char*)s->topElement-(char*)oldPtr)+(char*)s->data);
}
if (!s->nbElements++)
{
s->topElement = s->data;
}
else
{
s->topElement = (void *)((char *)s->topElement+s->elementSize);
}
vp_os_memcpy(s->topElement, element, s->elementSize);
}
DEFINE_THREAD_ROUTINE(mobile_main, data)
{
C_RESULT res = C_FAIL;
vp_com_wifi_config_t *config = NULL;
mobile_main_param_t *param = (mobile_main_param_t *)data;
ardroneEngineCallback callback = param->callback;
vp_os_memset(drone_address, 0x0, sizeof(drone_address));
// TODO(johnb): Make this autodetect based on network interfaces
while(((config = (vp_com_wifi_config_t *)wifi_config()) != NULL) && (strcmp(config->itfName, WIFI_ITFNAME) != 0))
{
PRINT("Wait WIFI connection !\n");
vp_os_delay(250);
}
// Get drone_address
vp_os_memcpy(drone_address, config->server, strlen(config->server));
PRINT("Drone address %s\n", drone_address);
// Get iphone_mac_address
get_iphone_mac_address(config->itfName);
PRINT("Iphone MAC Address %s\n", iphone_mac_address);
res = ardrone_tool_setup_com( NULL );
if( FAILED(res) )
{
PRINT("Wifi initialization failed. It means either:\n");
PRINT("\t* you're not root (it's mandatory because you can set up wifi connection only as root)\n");
PRINT("\t* wifi device is not present (on your pc or on your card)\n");
PRINT("\t* you set the wrong name for wifi interface (for example rausb0 instead of wlan0) \n");
PRINT("\t* ap is not up (reboot card or remove wifi usb dongle)\n");
PRINT("\t* wifi device has no antenna\n");
}
else
{
START_THREAD(video_stage, NULL);
res = ardrone_tool_init(drone_address, strlen(drone_address), NULL, param->appName, param->usrName);
callback(ARDRONE_ENGINE_INIT_OK);
ardrone_tool_set_refresh_time(1000 / kAPS);
while( SUCCEED(res) && bContinue == TRUE )
{
ardrone_tool_update();
}
JOIN_THREAD(video_stage);
res = ardrone_tool_shutdown();
}
vp_os_free (data);
return (THREAD_RET)res;
}
C_RESULT vp_com_client_open_socket(vp_com_socket_t* server_socket, vp_com_socket_t* client_socket)
{
C_RESULT res;
struct sockaddr_in raddr = { 0 }; // remote address
socklen_t l = sizeof(raddr);
int32_t s = (int32_t) server_socket->priv;
Write write = (Write) (server_socket->protocol == VP_COM_TCP ? vp_com_write_socket : vp_com_write_udp_socket);
vp_os_memcpy( client_socket, server_socket, sizeof(vp_com_socket_t) );
res = server_socket->select( server_socket, client_socket, VP_COM_SOCKET_SELECT_ENABLE, write );
if( VP_SUCCEEDED(res) )
{
if( server_socket->protocol == VP_COM_TCP )
{
client_socket->priv = (void*)accept( s, (struct sockaddr*)&raddr, &l );
}
DEBUG_PRINT_SDK("[VP_COM_SERVER] Opening socket for server %d\n", (int)s);
client_socket->server = server_socket;
}
else
{
DEBUG_PRINT_SDK("[VP_COM_SERVER] Failed to open socket for server %d\n", (int)s);
vp_os_memset( client_socket, 0, sizeof(vp_com_socket_t) );
}
return res;
}
C_RESULT vp_com_wait_socket(vp_com_socket_t* server, vp_com_socket_t* client, int32_t queue_length)
{
int s;
int c=0;
int l = sizeof(struct sockaddr_in);
struct sockaddr_in raddr = { 0 }; // remote address
C_RESULT res = VP_COM_OK;
if(server == NULL)
return VP_COM_PARAMERROR;
s = (int) server->priv;
c = 0;
server->queue_length = queue_length;
listen(s, queue_length);
c = accept( s, (struct sockaddr*)&raddr, &l );
if( c < 0 )
res = VP_COM_ERROR;
if(VP_SUCCEEDED( res ))
{
vp_os_memcpy( client, server, sizeof(vp_com_socket_t) );
client->priv = (void*) c;
}
return res;
}
static inline C_RESULT navdata_process( const navdata_unpacked_t* const navdata )
{
if( writeToFile )
{
if( navdata_file == NULL )
{
ardrone_navdata_file_data data;
data.filename = NULL;
data.print_header = NULL;
data.print = NULL;
ardrone_navdata_file_init(NULL);
}
ardrone_navdata_file_process( navdata );
}
else
{
if(navdata_file != NULL)
ardrone_navdata_file_release();
}
vp_os_mutex_lock( &inst_nav_mutex);
vp_os_memcpy(&inst_nav, navdata, sizeof(navdata_unpacked_t));
vp_os_mutex_unlock( &inst_nav_mutex );
return C_OK;
}
DEFINE_THREAD_ROUTINE(mobile_main, data)
{
C_RESULT res = C_FAIL;
char drone_address[64];
unsigned long theAddr;
ardroneEngineCallback callback = (ardroneEngineCallback)data;
vp_os_memset(drone_address, 0x0, sizeof(drone_address));
while((theAddr = deviceIPAddress(WIFI_ITFNAME, iphone_mac_address)) == LOCALHOST)
{
PRINT("Wait WIFI connection !\n");
vp_os_delay(250);
}
struct in_addr drone_addr;
drone_addr.s_addr = htonl( ntohl((in_addr_t)theAddr) - 1 );
vp_os_memcpy(drone_address, inet_ntoa(drone_addr), strlen(inet_ntoa(drone_addr)));
PRINT("Drone address %s\n", drone_address);
PRINT("Iphone MAC Address %s\n", iphone_mac_address);
res = ardrone_tool_setup_com( NULL );
if( FAILED(res) )
{
PRINT("Wifi initialization failed. It means either:\n");
PRINT("\t* you're not root (it's mandatory because you can set up wifi connection only as root)\n");
PRINT("\t* wifi device is not present (on your pc or on your card)\n");
PRINT("\t* you set the wrong name for wifi interface (for example rausb0 instead of wlan0) \n");
PRINT("\t* ap is not up (reboot card or remove wifi usb dongle)\n");
PRINT("\t* wifi device has no antenna\n");
}
else
{
res = ardrone_tool_init(drone_address, strlen(drone_address), NULL);
callback(ARDRONE_ENGINE_INIT_OK);
if(SUCCEED(res))
{
START_THREAD(video_stage, NULL);
res = ardrone_tool_set_refresh_time(1000 / kAPS);
while( SUCCEED(res) && bContinue == TRUE )
{
res = ardrone_tool_update();
}
JOIN_THREAD(video_stage);
}
res = ardrone_tool_shutdown();
}
return (THREAD_RET)0;
}
void p264_codec_alloc( video_controller_t* controller )
{
video_codec_t* video_codec;
video_codec = (video_codec_t*) vp_os_malloc( sizeof(p264_codec) );
vp_os_memcpy(video_codec, &p264_codec, sizeof(p264_codec));
controller->video_codec = video_codec;
}
C_RESULT configManager_getParam(uint32_t id, uint8_t *value, uint32_t maxLength, uint32_t *actualLength) {
(*actualLength) = 0;
if (id < 0 || id >= CONFIGPARAM_NUM_PARAMS) {
PRINT("[ConfigManager] Bad parameter ID when calling getParam()\n");
return C_FAIL;
}
uint32_t length = configManager_params[id].valueLength;
(*actualLength) = length;
if (length > maxLength) length = maxLength;
vp_os_memcpy(value, configManager_params[id].value, length);
return C_OK;
}
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;
}
int32_t vp_wlc_set_wep128_key(const char* wep_key)
{
wl_wsec_key_t key;
vp_os_memset(&key, 0, sizeof(wl_wsec_key_t));
vp_os_memcpy(&key.data, wep_key, WEP128_KEY_SIZE);
key.len = WEP128_KEY_SIZE;
key.algo = CRYPTO_ALGO_WEP128;
key.flags |= WL_PRIMARY_KEY;
return vp_wlc_set(WLC_SET_KEY, &key, sizeof(key));
}
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
ATcodec_Buffer_popElement (ATcodec_Buffer_t *s, void *dest)
{
VP_OS_ASSERT(s->nbElements);
vp_os_memcpy(dest, s->topElement, s->elementSize);
if (!--s->nbElements)
{
s->topElement = NULL;
}
else
{
s->topElement = (void *)((char *)s->topElement-s->elementSize);
}
}
/**
* @param void
*
* @brief Fill structure AT codec
* and built the library AT commands.
*
* @DESCRIPTION
*
*******************************************************************/
void ardrone_at_init ( const char* ip, size_t ip_len)
{
if ( at_init )
return;
VP_OS_ASSERT( ip_len < ARDRONE_IPADDRESS_SIZE );
if(ip_len >= ARDRONE_IPADDRESS_SIZE)
ip_len = ARDRONE_IPADDRESS_SIZE - 1;
vp_os_memcpy( &wifi_ardrone_ip[0], ip, ip_len);
wifi_ardrone_ip[ip_len]='\0';
atcodec_init (NULL);
at_init = 1;
}
/**
* @param void
*
* @brief Fill structure AT codec
* and built the library AT commands.
*
* @DESCRIPTION
*
*******************************************************************/
void ardrone_at_init_with_funcs ( const char* ip, size_t ip_len, AT_CODEC_FUNCTIONS_PTRS *funcs)
{
if ( at_init )
return;
VP_OS_ASSERT( ip_len < ARDRONE_IPADDRESS_SIZE );
if(ip_len >= ARDRONE_IPADDRESS_SIZE)
ip_len = ARDRONE_IPADDRESS_SIZE - 1;
vp_os_memcpy( &wifi_ardrone_ip[0], ip, ip_len);
wifi_ardrone_ip[ip_len]='\0';
atcodec_init (funcs);
at_init = 1;
}
static int vp_com_set_ip( int skfd, const char* ifname, int cmd, int ip )
{
#ifndef _WIN32
struct ifreq ifr;
struct sockaddr_in* addr;
struct sockaddr_in local_sin;
vp_os_memset( &ifr, 0, sizeof(struct ifreq) );
strncpy( ifr.ifr_name, ifname, IFNAMSIZ );
// Check if an ip is already set
if( ioctl( skfd, SIOCGIFADDR, &ifr ) != -1 )
{
addr = (struct sockaddr_in*) &ifr.ifr_addr;
// Is this the same ip?
if( addr->sin_family == AF_INET && ip == addr->sin_addr.s_addr )
return 0;
/*
// No so we try to delete it
if( ioctl( skfd, SIOCDIFADDR, &ifr ) < 0 )
{
DEBUG_PRINT_SDK("Unable to delete old ip address - You should remove your interface %s\n", ifname);
return -1;
}
*/
}
vp_os_memset( &ifr, 0, sizeof(struct ifreq) );
vp_os_memset( &local_sin, 0, sizeof(local_sin) );
strncpy( ifr.ifr_name, ifname, IFNAMSIZ );
addr = (struct sockaddr_in*) &ifr.ifr_addr;
local_sin.sin_family = AF_INET;
local_sin.sin_addr.s_addr = ip;
vp_os_memcpy(addr,&local_sin,sizeof(local_sin));
strncpy( ifr.ifr_name, ifname, IFNAMSIZ );
if( ioctl( skfd, cmd, &ifr ) < 0 )
return -1;
#endif
return 0;
}
C_RESULT video_stage_transform(vlib_stage_decoding_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
vp_os_mutex_lock( &out->lock );
if(out->status == VP_API_STATUS_INIT)
{
out->status = VP_API_STATUS_PROCESSING;
}
if( in->status == VP_API_STATUS_ENDED )
{
out->status = in->status;
}
if(out->status == VP_API_STATUS_PROCESSING )
{
vp_os_mutex_lock( &video_stage_config.mutex );
if(cfg->num_picture_decoded > video_stage_config.num_picture_decoded)
{
video_stage_config.num_picture_decoded = cfg->num_picture_decoded;
video_stage_config.num_frame = cfg->controller.num_frames;
video_stage_config.bytesPerPixel = 2;
video_stage_config.widthImage = cfg->controller.width;
video_stage_config.heightImage = cfg->controller.height;
if (video_stage_config.data != NULL)
{
vp_os_memcpy(video_stage_config.data, cfg->picture->y_buf, cfg->picture->width * cfg->picture->height );
}
out->numBuffers = in->numBuffers;
out->indexBuffer = in->indexBuffer;
out->buffers = in->buffers;
demo_video_client_process();
}
vp_os_mutex_unlock( &video_stage_config.mutex );
}
vp_os_mutex_unlock( &out->lock );
return C_OK;
}
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;
}
void*
vp_os_aligned_realloc(void* ptr, size_t size, size_t align_size)
{
void* ptr_ret;
void* aligned_ptr;
if( size == 0 )
{
ptr_ret = NULL;
if( ptr != NULL )
vp_os_aligned_free(ptr);
}
else
{
if( ptr != NULL )
{
int* ptr2 = (int*)ptr - 1;
size_t old_size;
aligned_ptr = ptr;
old_size = *ptr2--;
ptr_ret = vp_os_aligned_malloc(size, align_size);
// Compute smallest size
if( size > old_size )
{
size = old_size;
}
// Copy old data
vp_os_memcpy( ptr_ret, aligned_ptr, size );
vp_os_free( ((char*)ptr - *ptr2) );
}
else
{
ptr_ret = vp_os_aligned_malloc(size, align_size);
}
}
return ptr_ret;
}
C_RESULT ardrone_navdata_get_data(navdata_unpacked_t *data)
{
C_RESULT result = C_FAIL;
if(data)
{
vp_os_mutex_lock( &inst_nav_mutex );
/* data->ardrone_state = inst_nav.ardrone_state;
data->vision_defined = inst_nav.vision_defined;
vp_os_memcpy(&data->navdata_demo, &inst_nav.navdata_demo, sizeof(navdata_demo_t));
vp_os_memcpy(&data->navdata_vision_detect, &inst_nav.navdata_vision_detect, sizeof(navdata_vision_detect_t));
*/
vp_os_memcpy(data, &inst_nav, sizeof(navdata_unpacked_t));
vp_os_mutex_unlock( &inst_nav_mutex );
result = C_OK;
}
return result;
}
AT_CODEC_MSG_ID
ATcodec_Add_Hashed_Message_Tree (ATcodec_Tree_t *tree, const char *str, AT_CODEC_MSG_ID from_cmd, AT_CODEC_Message_Received func_ptr, int priority)
{
ATcodec_Message_Data_t data, *p_data;
ATcodec_Tree_Node_t *node;
int len_s, len_d, node_i;
char buffer[1024];
data.static_str = -1;
data.dynamic_str = -1;
data.total_str = -1;
data.from_cmd = from_cmd;
data.func_ptr = func_ptr;
data.priority = priority;
/* Retrieves the length of the never-changing part of the AT command */
len_s = static_len(str);
VP_OS_ASSERT(len_s < 1024);
vp_os_memcpy(&buffer[0], str, len_s);
buffer[len_s] = 0;
node_i = ATcodec_Tree_insert(tree, &buffer[0], &data);
node = ATcodec_Tree_Node_get(tree, node_i);
p_data = ATcodec_Buffer_getElement(&tree->leaves, node->data);
p_data->static_str = node->strkey;
// backup dynamic_str
p_data->dynamic_str = tree->strs.nbElements;
len_d = strlen(str+len_s)+1;
ATcodec_Buffer_pushElements (&tree->strs, str+len_s, len_d);
// backup concatenation of static_str and dynamic_str
p_data->total_str = tree->strs.nbElements;
ATcodec_Buffer_pushElements (&tree->strs, str, len_s+len_d);
return (AT_CODEC_MSG_ID)node_i;
}
C_RESULT
vp_stages_output_com_stage_open(vp_stages_output_com_config_t *cfg)
{
C_RESULT res;
res = vp_com_init(cfg->com);
if( VP_SUCCEEDED(res) )
{
vp_com_local_config(cfg->com, cfg->config);
if(cfg->connection && !cfg->connection->is_up)
{
res = vp_com_connect(cfg->com, cfg->connection, 1);
}
}
if( VP_SUCCEEDED(res) && VP_FAILED(vp_com_open(cfg->com, &cfg->socket, 0, &cfg->write)))
res = C_FAIL;
if( VP_SUCCEEDED(res) )
{
if(cfg->socket.type == VP_COM_SERVER)
{
res = vp_com_wait_connections(cfg->com, &cfg->socket, &cfg->socket_client, 1);
}
else
{
vp_os_memcpy(&cfg->socket_client, &cfg->socket, sizeof(vp_com_socket_t));
}
vp_com_sockopt(cfg->com, &cfg->socket_client, cfg->sockopt);
}
// \todo test
return res;
}
