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;
}
C_RESULT ardrone_tool_input_add( input_device_t* device )//×¢²á¼üÅÌ
{
C_RESULT res;
int32_t i;
VP_OS_ASSERT( device != NULL );
res = C_FAIL;
i = 0;
while( i < MAX_NUM_DEVICES && devices[i] != NULL ) i++;
if( i < MAX_NUM_DEVICES )
{
if( VP_SUCCEEDED(device->init()) )
{
devices[i] = device;
PRINT("Input device %s added\n", device->name);
res = C_OK;
}
else
{
PRINT("Input device %s init failed\n", device->name);
res = C_FAIL;
}
}
else
{
PRINT("Not enough memory to add input device %s\n", device->name);
}
return res;
}
/*void test(void * null){
while(1){
printf("将来在这里处理无人机控制事件");
//sleep(2);
}}*/
int main(int argc, char **argv)
{
C_RESULT res;
const char* appname = argv[0];
WSADATA wsaData = {0};
int iResult = 0;
iResult = WSAStartup(MAKEWORD(2, 2), &wsaData);
if (iResult != 0) {
wprintf(L"WSAStartup failed: %d\n", iResult);
return 1;
}
#include <VP_Os/vp_os_signal.h>
#if defined USE_PTHREAD_FOR_WIN32
#pragma comment (lib,"pthreadVC2.lib")
#endif
res = test_drone_connection();//检测WiFi连接
if(res!=0) {
printf("%s","Could not detect the drone version ... press <Enter> to try connecting anyway.\n");
getchar();
WSACleanup();
exit(-1);
}
init_client();
res = ardrone_tool_setup_com( NULL );//配置AT命令，视频传输
if( FAILED(res) ) {
PRINT("Wifi initialization failed.\n");
return -1;
}
res = ardrone_tool_init(argc, argv);//调用ardrone_tool_init_custom()函数
while( VP_SUCCEEDED(res) && ardrone_tool_exit() == FALSE ) {
res = ardrone_tool_update();
}//循环调用ardrone_too_update()函数
res = ardrone_tool_shutdown();
closesocket(ConnectSocket);
WSACleanup();
system("cls");
printf("End of SDK Demo for Windows\n");
getchar();
return VP_SUCCEEDED(res) ? 0 : -1;
}
BOOL _stdcall UpdateDrone()
{
/* Keeps sending AT commands to control the drone as long as
everything is OK */
C_RESULT res = ardrone_tool_update();
printf("Sent update\n");
return (VP_SUCCEEDED(res) && ardrone_tool_exit() == FALSE);
}
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;
}
C_RESULT video_decode_picture( video_controller_t* controller, vp_api_picture_t* picture, video_stream_t* ex_stream, bool_t* got_image )
{
vp_api_picture_t blockline = { 0 };
controller->mode = VIDEO_DECODE; // mandatory because of video_cache_stream
blockline = *picture;
blockline.height = MB_HEIGHT_Y;
blockline.complete = 1;
blockline.vision_complete = 0;
while( VP_SUCCEEDED(video_cache_stream( controller, ex_stream )) )
{
video_codec_type_select(controller,ex_stream); // to be verified
video_decode_blockline( controller, &blockline, got_image );
}
return C_OK;
}
C_RESULT ardrone_control_connect_to_drone()
{
int res_open_socket;
#ifdef _WIN32
int timeout_windows=1000;/*milliseconds*/
#endif
struct timeval tv;
vp_com_close(COM_CONTROL(), &control_socket);
res_open_socket = vp_com_open(COM_CONTROL(), &control_socket, &control_read, &control_write);
if( VP_SUCCEEDED(res_open_socket) )
{
tv.tv_sec = 1;
tv.tv_usec = 0;
setsockopt((int32_t)control_socket.priv,
SOL_SOCKET,
SO_RCVTIMEO,
#ifdef _WIN32
(const char*)&timeout_windows, sizeof(timeout_windows)
#else
(const char*)&tv, sizeof(tv)
#endif
);
control_socket.is_disable = FALSE;
return C_OK;
}
else
{
DEBUG_PRINT_SDK("VP_Com : Failed to open socket for control\n");
perror("FTOSFC");
return C_FAIL;
}
}
BOOL _stdcall ShutdownDrone()
{
C_RESULT res = ardrone_tool_shutdown();
WSACleanup();
//start^=1;
//ardrone_tool_set_ui_pad_start(start);
//printf("Sending start %i.%s\n",start,linefiller);
// * @param enable 1,with pitch,roll and 0,without pitch,roll.
// * @param pitch Using floating value between -1 to +1.
// * @param roll Using floating value between -1 to +1.
// * @param gaz Using floating value between -1 to +1.
// * @param yaw Using floating value between -1 to +1.
//ardrone_at_set_progress_cmd((hovering)? 0:1, roll, pitch, gaz, yaw);
return VP_SUCCEEDED(res) ? 0 : -1;
}
static C_RESULT ardrone_tool_input_remove_i( int32_t i )
{
C_RESULT res;
res = C_OK;
if( devices[i] != NULL )
{
if( VP_SUCCEEDED(devices[i]->shutdown()) )
{
PRINT("Input device %s removed\n", devices[i]->name);
res = C_OK;
}
else
{
PRINT("Input device %s removed but an error occured during its shutdown\n", devices[i]->name);
res = C_FAIL;
}
devices[i] = NULL;
}
return res;
}
C_RESULT ardrone_tool_input_update(void)
{
C_RESULT res;
int32_t i;
res = C_OK;
i = 0;
while( VP_SUCCEEDED(res) && i < MAX_NUM_DEVICES )
{
if( devices[i] != NULL && VP_FAILED(devices[i]->update()) )
{
PRINT("Input device %s update failed... it'll be removed\n", devices[i]->name);
ardrone_tool_input_remove_i(i);
res = C_FAIL;
}
i++;
}
ui_pad_update_user_input(&input_state);
return res;
}
C_RESULT video_com_multisocket_stage_open(video_com_multisocket_config_t *cfg)
{
int i;
C_RESULT res = C_OK;
int nb_failed_connections = 0;
for (i = 0; i < cfg->nb_sockets; i++)
{
video_com_stage_register (cfg->configs[i]);
cfg->configs[i]->mustReconnect = 0;
}
PRINT("Video multisocket : init %i sockets\n",cfg->nb_sockets);
for(i=0;i<cfg->nb_sockets;i++)
{
PRINT("Video multisocket : connecting socket %d on port %d %s\n",
i,
cfg->configs[i]->socket.port,
(cfg->configs[i]->protocol==VP_COM_TCP)?"TCP":"UDP");
cfg->configs[i]->connected = FALSE;
res = video_com_stage_connect(cfg->configs[i]);
if (!VP_SUCCEEDED(res)) {
PRINT(" - Connection failed\n");
nb_failed_connections++;
}
}
cfg->last_active_socket = -1;
if (nb_failed_connections==cfg->nb_sockets) { return C_FAIL; }
return C_OK;
}
/*
The video processing thread.
This function can be kept as it is by most users.
It automatically receives the video stream in a loop, decode it, and then
call the 'output_rendering_device_stage_transform' function for each decoded frame.
*/
DEFINE_THREAD_ROUTINE(video_stage, data)
{
C_RESULT res;
vp_api_io_pipeline_t pipeline;
vp_api_io_data_t out;
vp_api_io_stage_t stages[NB_STAGES];
vp_api_picture_t picture;
video_com_config_t icc;
vlib_stage_decoding_config_t vec;
vp_stages_yuv2rgb_config_t yuv2rgbconf;
/* Picture configuration */
picture.format = PIX_FMT_YUV420P;
picture.width = DRONE_VIDEO_MAX_WIDTH;
picture.height = DRONE_VIDEO_MAX_HEIGHT;
picture.framerate = 15;
//picture.framerate = 3;
picture.y_buf = vp_os_malloc( DRONE_VIDEO_MAX_WIDTH * DRONE_VIDEO_MAX_HEIGHT );
picture.cr_buf = vp_os_malloc( DRONE_VIDEO_MAX_WIDTH * DRONE_VIDEO_MAX_HEIGHT / 4 );
picture.cb_buf = vp_os_malloc( DRONE_VIDEO_MAX_WIDTH * DRONE_VIDEO_MAX_HEIGHT / 4 );
picture.y_line_size = DRONE_VIDEO_MAX_WIDTH;
picture.cb_line_size = DRONE_VIDEO_MAX_WIDTH / 2;
picture.cr_line_size = DRONE_VIDEO_MAX_WIDTH / 2;
vp_os_memset(&icc, 0, sizeof( icc ));
vp_os_memset(&vec, 0, sizeof( vec ));
vp_os_memset(&yuv2rgbconf, 0, sizeof( yuv2rgbconf ));
/* Video socket configuration */
icc.com = COM_VIDEO();
icc.buffer_size = 100000;
icc.protocol = VP_COM_UDP;
COM_CONFIG_SOCKET_VIDEO(&icc.socket, VP_COM_CLIENT, VIDEO_PORT, wifi_ardrone_ip);
/* Video decoder configuration */
/* Size of the buffers used for decoding
This must be set to the maximum possible video resolution used by the drone
The actual video resolution will be stored by the decoder in vec.controller
(see vlib_stage_decode.h) */
vec.width = DRONE_VIDEO_MAX_WIDTH;
vec.height = DRONE_VIDEO_MAX_HEIGHT;
vec.picture = &picture;
vec.block_mode_enable = TRUE;
vec.luma_only = FALSE;
yuv2rgbconf.rgb_format = VP_STAGES_RGB_FORMAT_RGB24;
/* Video pipeline building */
pipeline.nb_stages = 0;
/* Video stream reception */
stages[pipeline.nb_stages].type = VP_API_INPUT_SOCKET;
stages[pipeline.nb_stages].cfg = (void *)&icc;
stages[pipeline.nb_stages].funcs = video_com_funcs;
pipeline.nb_stages++;
/* Video stream decoding */
stages[pipeline.nb_stages].type = VP_API_FILTER_DECODER;
stages[pipeline.nb_stages].cfg = (void*)&vec;
stages[pipeline.nb_stages].funcs = vlib_decoding_funcs;
pipeline.nb_stages++;
/* YUV to RGB conversion
YUV format is used by the video stream protocol
Remove this stage if your rendering device can handle
YUV data directly
*/
stages[pipeline.nb_stages].type = VP_API_FILTER_YUV2RGB;
stages[pipeline.nb_stages].cfg = (void*)&yuv2rgbconf;
stages[pipeline.nb_stages].funcs = vp_stages_yuv2rgb_funcs;
pipeline.nb_stages++;
/* User code */
stages[pipeline.nb_stages].type = VP_API_OUTPUT_SDL; /* Set to VP_API_OUTPUT_SDL even if SDL is not used */
stages[pipeline.nb_stages].cfg = (void*)&vec; /* give the decoder information to the renderer */
stages[pipeline.nb_stages].funcs = vp_stages_output_rendering_device_funcs;
pipeline.nb_stages++;
pipeline.stages = &stages[0];
/* Processing of a pipeline */
if( !ardrone_tool_exit() )
{
PRINT("\n Video stage thread initialisation\n\n");
// switch to vertical camera
//ardrone_at_zap(ZAP_CHANNEL_VERT);
res = vp_api_open(&pipeline, &pipeline_handle);
if( VP_SUCCEEDED(res) )
{
int loop = VP_SUCCESS;
out.status = VP_API_STATUS_PROCESSING;
while( !ardrone_tool_exit() && (loop == VP_SUCCESS) )
{
if( VP_SUCCEEDED(vp_api_run(&pipeline, &out)) ) {
if( (out.status == VP_API_STATUS_PROCESSING || out.status == VP_API_STATUS_STILL_RUNNING) ) {
loop = VP_SUCCESS;
}
}
else loop = -1; // Finish this thread
}
vp_api_close(&pipeline, &pipeline_handle);
}
}
PRINT(" Video stage thread ended\n\n");
return (THREAD_RET)0;
}
/**
* \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;
}
DEFINE_THREAD_ROUTINE(video_stage, data)
{
C_RESULT res;
vp_api_io_pipeline_t pipeline;
vp_api_io_data_t out;
vp_api_io_stage_t stages[NB_STAGES];
vp_api_picture_t picture;
vlib_stage_decoding_config_t vec;
vp_os_memset(&icc, 0, sizeof( icc ));
vp_os_memset(&vec, 0, sizeof( vec ));
vp_os_memset(&picture, 0, sizeof( picture ));
//#ifdef RECORD_VIDEO
// video_stage_recorder_config_t vrc;
//#endif
/// Picture configuration
picture.format = /*PIX_FMT_YUV420P */ PIX_FMT_RGB565;
picture.width = 512;
picture.height = 512;
picture.framerate = 15;
picture.y_buf = vp_os_malloc( picture.width * picture.height * 2);
picture.cr_buf = NULL;
picture.cb_buf = NULL;
picture.y_line_size = picture.width * 2;
picture.cb_line_size = 0;
picture.cr_line_size = 0;
icc.com = COM_VIDEO();
icc.buffer_size = 100000;
icc.protocol = VP_COM_UDP;
COM_CONFIG_SOCKET_VIDEO(&icc.socket, VP_COM_CLIENT, VIDEO_PORT, wifi_ardrone_ip);
vec.width = 512;
vec.height = 512;
vec.picture = &picture;
vec.luma_only = FALSE;
vec.block_mode_enable = TRUE;
pipeline.nb_stages = 0;
stages[pipeline.nb_stages].type = VP_API_INPUT_SOCKET;
stages[pipeline.nb_stages].cfg = (void *)&icc;
stages[pipeline.nb_stages].funcs = video_com_funcs;
pipeline.nb_stages++;
stages[pipeline.nb_stages].type = VP_API_FILTER_DECODER;
stages[pipeline.nb_stages].cfg = (void*)&vec;
stages[pipeline.nb_stages].funcs = vlib_decoding_funcs;
pipeline.nb_stages++;
/*
#ifdef RECORD_VIDEO
stages[pipeline.nb_stages].type = VP_API_FILTER_DECODER;
stages[pipeline.nb_stages].cfg = (void*)&vrc;
stages[pipeline.nb_stages].funcs = video_recorder_funcs;
pipeline.nb_stages++;
#endif
*/
stages[pipeline.nb_stages].type = VP_API_OUTPUT_LCD;
stages[pipeline.nb_stages].cfg = (void*)&vec;
stages[pipeline.nb_stages].funcs = video_stage_funcs;
pipeline.nb_stages++;
pipeline.stages = &stages[0];
if( !ardrone_tool_exit() )
{
PRINT("\nvideo stage thread initialisation\n\n");
// NICK
video_stage_init();
res = vp_api_open(&pipeline, &pipeline_handle);
if( VP_SUCCEEDED(res) )
{
int loop = VP_SUCCESS;
out.status = VP_API_STATUS_PROCESSING;
#ifdef RECORD_VIDEO
{
DEST_HANDLE dest;
dest.stage = 2;
dest.pipeline = pipeline_handle;
vp_api_post_message( dest, PIPELINE_MSG_START, NULL, (void*)NULL);
}
#endif
video_stage_in_pause = FALSE;
while( !ardrone_tool_exit() && (loop == VP_SUCCESS) )
{
if(video_stage_in_pause)
{
vp_os_mutex_lock(&video_stage_mutex);
icc.num_retries = VIDEO_MAX_RETRIES;
vp_os_cond_wait(&video_stage_condition);
vp_os_mutex_unlock(&video_stage_mutex);
}
if( VP_SUCCEEDED(vp_api_run(&pipeline, &out)) ) {
if( (out.status == VP_API_STATUS_PROCESSING || out.status == VP_API_STATUS_STILL_RUNNING) ) {
loop = VP_SUCCESS;
}
}
else loop = -1; // Finish this thread
}
vp_api_close(&pipeline, &pipeline_handle);
}
}
PRINT(" video stage thread ended\n\n");
return (THREAD_RET)0;
}
C_RESULT video_com_stage_open(video_com_config_t *cfg)
{
#ifdef _WIN32
int sizeinit;
#endif
C_RESULT res = C_FAIL;
if( cfg->protocol == VP_COM_UDP )
{
struct timeval tv;
#ifdef _WIN32
int timeout_for_windows=1000; /* timeout in milliseconds */
#endif
// 1 second timeout
tv.tv_sec = 1;
tv.tv_usec = 0;
cfg->socket.protocol = VP_COM_UDP;
cfg->socket.is_multicast = 0; // enable multicast for video
cfg->socket.multicast_base_addr = MULTICAST_BASE_ADDR;
res = vp_com_open(cfg->com, &cfg->socket, &cfg->read, &cfg->write);
if( VP_SUCCEEDED(res) )
{
int numi= 1;
socklen_t numi1= sizeof(int);
#ifdef _WIN32
setsockopt((int32_t)cfg->socket.priv, SOL_SOCKET, SO_RCVTIMEO, SSOPTCAST_RO(&timeout_for_windows), sizeof(timeout_for_windows));
#else
setsockopt((int32_t)cfg->socket.priv, SOL_SOCKET, SO_RCVTIMEO, SSOPTCAST_RO(&tv), sizeof(tv));
#endif
// Increase buffer for receiving datas.
setsockopt( (int32_t)cfg->socket.priv, SOL_SOCKET, SO_DEBUG, SSOPTCAST_RO(&numi), sizeof(numi));
numi= 256*256;
setsockopt( (int32_t)cfg->socket.priv, SOL_SOCKET, SO_RCVBUF, SSOPTCAST_RO(&numi),numi1);
getsockopt( (int32_t)cfg->socket.priv, SOL_SOCKET, SO_RCVBUF, SSOPTCAST_RW(&numi),&numi1);
numi1=0;
setsockopt( (int32_t)cfg->socket.priv, SOL_SOCKET, SO_DEBUG, SSOPTCAST_RO(&numi1), sizeof(numi1));
}
}
else if( cfg->protocol == VP_COM_TCP )
{
res = vp_com_open(cfg->com, &cfg->socket, &cfg->read, &cfg->write);
if( VP_SUCCEEDED(res) )
{
vp_com_sockopt(cfg->com, &cfg->socket, cfg->sockopt);
}
}
/* Stephane */
#ifdef _WIN32
sizeinit = strlen("Init");
vp_com_write_socket(&cfg->socket,"Init",&sizeinit);
#endif
return res;
}
DEFINE_THREAD_ROUTINE( navdata_update, nomParams )
{
C_RESULT res;
int32_t i, size;
uint32_t cks, navdata_cks, sequence = NAVDATA_SEQUENCE_DEFAULT-1;
struct timeval tv;
#ifdef _WIN32
int timeout_for_windows=1000/*milliseconds*/;
#endif
navdata_t* navdata = (navdata_t*) &navdata_buffer[0];
tv.tv_sec = 1/*second*/;
tv.tv_usec = 0;
res = C_OK;
if( VP_FAILED(vp_com_open(COM_NAVDATA(), &navdata_socket, &navdata_read, &navdata_write)) )
{
printf("VP_Com : Failed to open socket for navdata\n");
res = C_FAIL;
}
if( VP_SUCCEEDED(res) )
{
PRINT("Thread navdata_update in progress...\n");
#ifdef _WIN32
setsockopt((int32_t)navdata_socket.priv, SOL_SOCKET, SO_RCVTIMEO, (const char*)&timeout_for_windows, sizeof(timeout_for_windows));
/* Added by Stephane to force the drone start sending data. */
if(navdata_write)
{
int sizeinit = 5;
navdata_write( (void*)&navdata_socket, (int8_t*)"Init", &sizeinit );
}
#else
setsockopt((int32_t)navdata_socket.priv, SOL_SOCKET, SO_RCVTIMEO, (const char*)&tv, sizeof(tv));
#endif
i = 0;
while( ardrone_navdata_handler_table[i].init != NULL )
{
// if init failed for an handler we set its process function to null
// We keep its release function for cleanup
if( VP_FAILED( ardrone_navdata_handler_table[i].init(ardrone_navdata_handler_table[i].data) ) )
ardrone_navdata_handler_table[i].process = NULL;
i ++;
}
navdata_thread_in_pause = FALSE;
while( VP_SUCCEEDED(res)
&& !ardrone_tool_exit()
&& bContinue )
{
if(navdata_thread_in_pause)
{
vp_os_mutex_lock(&navdata_client_mutex);
num_retries = NAVDATA_MAX_RETRIES + 1;
vp_os_cond_wait(&navdata_client_condition);
vp_os_mutex_unlock(&navdata_client_mutex);
}
if( navdata_read == NULL )
{
res = C_FAIL;
continue;
}
size = NAVDATA_MAX_SIZE;
navdata->header = 0; // Soft reset
res = navdata_read( (void*)&navdata_socket, (int8_t*)&navdata_buffer[0], &size );
#ifdef _WIN32
if( size <= 0 )
#else
if( size == 0 )
#endif
{
// timeout
PRINT("Timeout\n");
ardrone_navdata_open_server();
sequence = NAVDATA_SEQUENCE_DEFAULT-1;
num_retries++;
}
else
num_retries = 0;
if( VP_SUCCEEDED( res ) )
{
if( navdata->header == NAVDATA_HEADER )
{
if( ardrone_get_mask_from_state(navdata->ardrone_state, ARDRONE_COM_WATCHDOG_MASK) )
{
// reset sequence number because of com watchdog
// This code is mandatory because we can have a com watchdog without detecting it on mobile side :
// Reconnection is fast enough (less than one second)
sequence = NAVDATA_SEQUENCE_DEFAULT-1;
if( ardrone_get_mask_from_state(navdata->ardrone_state, ARDRONE_NAVDATA_BOOTSTRAP) == FALSE )
ardrone_tool_send_com_watchdog(); // acknowledge
}
if( navdata->sequence > sequence )
{
i = 0;
ardrone_navdata_unpack_all(&navdata_unpacked, navdata, &navdata_cks);
cks = ardrone_navdata_compute_cks( &navdata_buffer[0], size - sizeof(navdata_cks_t) );
if( cks == navdata_cks )
{
while( ardrone_navdata_handler_table[i].init != NULL )
{
if( ardrone_navdata_handler_table[i].process != NULL )
ardrone_navdata_handler_table[i].process( &navdata_unpacked );
i++;
}
}
else
{
PRINT("[Navdata] Checksum failed : %d (distant) / %d (local)\n", navdata_cks, cks);
}
}
else
{
PRINT("[Navdata] Sequence pb : %d (distant) / %d (local)\n", navdata->sequence, sequence);
}
// remaining = sizeof(navdata);
sequence = navdata->sequence;
}
}
}
// Release resources alllocated by handlers
i = 0;
while( ardrone_navdata_handler_table[i].init != NULL )
{
ardrone_navdata_handler_table[i].release();
i ++;
}
}
vp_com_close(COM_NAVDATA(), &navdata_socket);
DEBUG_PRINT_SDK("Thread navdata_update ended\n");
return (THREAD_RET)res;
}
DEFINE_THREAD_ROUTINE_STACK( vp_com_server, thread_params, VP_COM_THREAD_SERVER_STACK_SIZE )
{
vp_com_socket_t client_sockets[VP_COM_THREAD_NUM_MAX_CLIENTS];
struct timeval tv, *ptv;
// This thread setup connection then loop & wait for a socket event
vp_com_server_thread_param_t* params = (vp_com_server_thread_param_t*) thread_params;
int32_t i, rc, ncs, s, max = 0, num_server_sockets = params->num_servers, num_client_sockets = 0;
vp_com_socket_t* server_sockets = params->servers;
fd_set read_fs;
vp_os_memset( client_sockets, 0, sizeof( client_sockets ));
if(VP_FAILED(vp_com_init(params->com)))
{
DEBUG_PRINT_SDK("[VP_COM_SERVER] Failed to init com\n");
vp_com_shutdown(params->com);
}
else if(VP_FAILED(vp_com_local_config(params->com, params->config)))
{
DEBUG_PRINT_SDK("[VP_COM_SERVER] Failed to configure com\n");
vp_com_shutdown(params->com);
}
else if(VP_FAILED(vp_com_connect(params->com, params->connection, 1)))
{
DEBUG_PRINT_SDK("[VP_COM_SERVER] Failed to connect\n");
vp_com_shutdown(params->com);
}
else
{
vp_os_mutex_lock(&server_initialisation_mutex);
vp_os_cond_signal(&server_initialisation_wait);
vp_os_mutex_unlock(&server_initialisation_mutex);
server_init_not_finished = FALSE;
for( i = 0; i < num_server_sockets; i++ )
{
if(VP_FAILED( vp_com_open_socket(&server_sockets[i], NULL, NULL) ))
{
DEBUG_PRINT_SDK("[VP_COM_SERVER] Unable to open server socket\n");
server_sockets[i].is_disable = TRUE;
}
else
{
listen((int32_t)server_sockets[i].priv, server_sockets[i].queue_length);
}
}
params->run = TRUE;
while( params->run == TRUE )
{
if( params->timer_enable == FALSE || ( params->wait_sec == 0 && params->wait_usec == 0 ) )
{
ptv = NULL;
}
else
{
tv.tv_sec = params->wait_sec;
tv.tv_usec = params->wait_usec;
ptv = &tv;
}
FD_ZERO(&read_fs);
max = vp_com_fill_read_fs( &server_sockets[0], num_server_sockets, 0, &read_fs );
max = vp_com_fill_read_fs( &client_sockets[0], num_client_sockets, max, &read_fs );
rc = select( max + 1, &read_fs, NULL, NULL, ptv );
if( rc == -1 && ( errno == EINTR || errno == EAGAIN ) )
continue;
if( rc == 0 )
{
DEBUG_PRINT_SDK("[VP_COM_SERVER] select timeout\n");
vp_com_close_client_sockets(&client_sockets[0], num_client_sockets);
num_client_sockets = 0;
params->timer_enable = FALSE;
vp_os_memset( client_sockets, 0, sizeof( client_sockets ));
}
for( i = 0; i < num_server_sockets && rc != 0; i++ )
{
s = (int32_t) server_sockets[i].priv;
if( ( !server_sockets[i].is_disable ) && FD_ISSET( s, &read_fs) )
{
rc --;
// Recycle previously released sockets
for( ncs = 0; ncs < num_client_sockets && client_sockets[ncs].priv != NULL; ncs++ );
if( ncs < VP_COM_THREAD_NUM_MAX_CLIENTS)
{
if( VP_SUCCEEDED(vp_com_client_open_socket(&server_sockets[i], &client_sockets[ncs])) && ( ncs == num_client_sockets ) )
num_client_sockets ++;
}
}
}
for( i = 0; i < num_client_sockets && rc != 0; i++ )
{
s = (int32_t) client_sockets[i].priv;
if( ( !client_sockets[i].is_disable ) && FD_ISSET( s, &read_fs) )
{
rc--;
vp_com_client_receive( &client_sockets[i] );
}
}
}
for( i = 0; i < num_server_sockets; i++ )
{
vp_com_close_socket(&server_sockets[i]);
}
}
vp_com_disconnect(params->com);
vp_com_shutdown(params->com);
THREAD_RETURN( 0 );
}
C_RESULT vp_com_open_socket(vp_com_socket_t* sck, Read* read, Write* write)
{
C_RESULT res = VP_COM_OK;
BOOL reuseaddroption = TRUE;
BOOL exclusiveaddroption = FALSE;
SOCKET s = -1;
struct sockaddr_in name = { 0 };
struct sockaddr_in local_address = { 0 };
struct sockaddr_in remote_address = { 0 };
int err;
int res_setsockopt=0,res_connect=0,res_bind=0;
switch( sck->protocol )
{
case VP_COM_TCP:
s = socket( AF_INET, SOCK_STREAM, IPPROTO_TCP );
res = ( s == INVALID_SOCKET ) ? VP_COM_ERROR : VP_COM_OK;
break;
case VP_COM_UDP:
s = socket( AF_INET, SOCK_DGRAM, IPPROTO_UDP );
sck->scn = inet_addr(sck->serverHost); // Cache destination in int format
res = ( s == INVALID_SOCKET ) ? VP_COM_ERROR : VP_COM_OK;
break;
default:
sck->type = VP_COM_CLIENT;
res = VP_COM_PARAMERROR;
break;
}
if( VP_FAILED(res) )
{
PRINT("\nSocket opening failed\n");
}
VP_COM_CHECK( res );
// res_setsockopt = setsockopt(s,SOL_SOCKET,SO_REUSEADDR,(char*)&reuseaddroption,sizeof(reuseaddroption));
res_setsockopt = setsockopt(s,SOL_SOCKET,SO_EXCLUSIVEADDRUSE,(char*)&exclusiveaddroption,sizeof(exclusiveaddroption));
name.sin_family = AF_INET;
name.sin_port = htons( sck->port );
switch( sck->type )
{
case VP_COM_CLIENT:
remote_address.sin_family = AF_INET;
remote_address.sin_port = htons( sck->port );
remote_address.sin_addr.s_addr = inet_addr(sck->serverHost);
if ( sck->protocol ==VP_COM_UDP)
{
local_address.sin_addr.s_addr= INADDR_ANY;
local_address.sin_family = AF_INET;
local_address.sin_port = htons( sck->port ); /* Bind to any available port */
res_bind = bind(s,(const struct sockaddr*)&local_address,sizeof(local_address));
err = WSAGetLastError();
res = (res_bind==0)? VP_COM_OK : VP_COM_ERROR; /* Convert from Win32 error code to VP SDK error code */
}
if (VP_SUCCEEDED(res))// && (sck->protocol !=VP_COM_UDP))
{
res_connect = connect( s, (struct sockaddr*)&remote_address, sizeof( remote_address ) );
if( res_connect == -1 ){ res = VP_COM_ERROR; err = WSAGetLastError(); }
}
break;
case VP_COM_SERVER:
/* Local TCP/UDP address on which we wait for connections */
local_address.sin_family = AF_INET;
local_address.sin_port = htons( sck->port );
local_address.sin_addr.s_addr = INADDR_ANY; /* Accept connections on any network interface */
res_bind = bind( s, (const struct sockaddr*)&local_address, sizeof(local_address) );
res = (res_bind==0)? VP_COM_OK : VP_COM_ERROR ; /* Convert from Win32 error code to VP SDK error code */
break;
default:
res = VP_COM_PARAMERROR;
break;
}
if(res == VP_COM_OK)
{
sck->priv = (void*) s;
switch( sck->protocol )
{
case VP_COM_TCP:
if(read) *read = (Read) vp_com_read_socket;
if(write) *write = (Write) vp_com_write_socket;
break;
case VP_COM_UDP:
if(read) *read = (Read) vp_com_read_udp_socket;
if(write) *write = (Write) vp_com_write_udp_socket;
break;
default:
if(read) *read = NULL;
if(write) *write = NULL;
break;
}
}
else
{
closesocket( s );
}
if (sck->block != VP_COM_DEFAULT &&
sck->block != VP_COM_WAITALL &&
sck->block != VP_COM_DONTWAIT)
{
sck->block = VP_COM_DEFAULT;
}
return res;
}
int _stdcall InitDrone() // TODO : Should SSID be passed in here?
{
C_RESULT res; // functions return value
const char* appname = "droneapp";
WSADATA wsaData = {0};
int iResult = 0;
/* Initializes Windows socket subsystem */
iResult = WSAStartup(MAKEWORD(2, 2), &wsaData);
if (iResult != 0)
{
wprintf(L"WSAStartup failed: %d\n", iResult);
return -3;
}
/* Includes the Pthread for Win32 Library if necessary */
#include <VP_Os/vp_os_signal.h>
#if defined USE_PTHREAD_FOR_WIN32
#pragma comment (lib,"pthreadVC2.lib")
#endif
res = test_drone_connection();
if( res !=0 )
{
// Failed;
WSACleanup();
return -1;
}
res = ardrone_tool_setup_com(NULL); // Can pass in the SSID here
if(FAILED(res))
{
// Wifi init has failed
return -2;
}
/* Initialises ARDroneTool */
res = ardrone_tool_init(0, NULL);
if(FAILED(res))
{
return -4;
}
// Success
return 0;
//-----------------------------------
//while (VP_SUCCEEDED(res) && ardrone_tool_exit() == FALSE)
// res = ardrone_tool_update();
//res = ardrone_tool_shutdown();
//WSACleanup();
/* Bye bye */
return VP_SUCCEEDED(res) ? 0 : -1;
}
int main(int argc , char** argv)
{
int cnt=0;
C_RESULT res; // functions return value
WSADATA wsaData = {0};
int iResult = 0;
/* Initializes Windows socket subsystem */
iResult = WSAStartup(MAKEWORD(2, 2), &wsaData);
if (iResult != 0) { wprintf(L"WSAStartup failed: %d\n", iResult); return 1; }
/* Includes the Pthread for Win32 Library if necessary */
#include <VP_Os/vp_os_signal.h>
#if defined USE_PTHREAD_FOR_WIN32
#pragma comment (lib,"pthreadVC2.lib")
#endif
#ifdef VIDEO_RECORD
cv_writer = cvCreateVideoWriter("D:\\out.avi",-1,15,cvSize(320,240),1);
#endif
cv_image = cvCreateImage(cvSize(320,240),IPL_DEPTH_8U,3);
//Kinect_Init();
/* Initializes communication sockets */
hSocketThread = CreateThread(NULL,0,Socket_ProcessThread,NULL,0,0);
// while (1);
res = test_drone_connection();
if(res!=0){
printf("%s","Could not detect the drone version ... press <Enter> to try connecting anyway.\n");
getchar();
//WSACleanup(); exit(-1);
}
res = ardrone_tool_setup_com( NULL );
if( FAILED(res) ){ PRINT("Wifi initialization failed.\n"); return -1; }
/* Initialises ARDroneTool */
res = ardrone_tool_init(argc, argv);
/*record video*/
/* Keeps sending AT commands to control the drone as long as
everything is OK */
while( VP_SUCCEEDED(res) && ardrone_tool_exit() == FALSE) {
#ifdef VIDEO_RECORD
if ( cnt++ > 3000 )
cvReleaseVideoWriter(&cv_writer);
#endif
res = ardrone_tool_update();
}
printf("out\n");
/**/
//close Socket Thread
if ( hSocketThread!=NULL ) {
WaitForSingleObject(hSocketThread,INFINITE);
CloseHandle(hSocketThread);
}
res = ardrone_tool_shutdown();
//Kinect_UnInit();
WSACleanup();
/* Bye bye */
cvReleaseVideoWriter(&cv_writer);
system("cls");
printf("End of SDK Demo for Windows\n");
getchar();
return VP_SUCCEEDED(res) ? 0 : -1;
}
DEFINE_THREAD_ROUTINE( ardrone_control, nomParams )
{
C_RESULT res_wait_navdata = C_OK;
C_RESULT res = C_OK;
uint32_t retry, current_ardrone_state;
int32_t next_index_in_queue;
ardrone_control_event_ptr_t current_event;
retry = 0;
current_event = NULL;
DEBUG_PRINT_SDK("Thread control in progress...\n");
control_socket.is_disable = TRUE;
ardrone_control_connect_to_drone();
while( bContinue
&& !ardrone_tool_exit() )
{
vp_os_mutex_lock(&control_mutex);
control_waited = TRUE;
/* Wait for new navdata to be received. */
res_wait_navdata = vp_os_cond_timed_wait(&control_cond, 1000);
vp_os_mutex_unlock(&control_mutex);
/*
* In case of timeout on the navdata, we assume that there was a problem
* with the Wifi connection.
* It is then safer to close and reopen the control socket (TCP 5559) since
* some OS might stop giving data but not signal any disconnection.
*/
if(VP_FAILED(res_wait_navdata))
{
DEBUG_PRINT_SDK("Timeout while waiting for new navdata.\n");
if(!control_socket.is_disable)
control_socket.is_disable = TRUE;
}
if(control_socket.is_disable)
{
ardrone_control_connect_to_drone();
}
if(VP_SUCCEEDED(res_wait_navdata) && (!control_socket.is_disable))
{
vp_os_mutex_lock(&control_mutex);
current_ardrone_state = ardrone_state;
control_waited = FALSE;
vp_os_mutex_unlock(&control_mutex);
if( ardrone_tool_exit() ) // Test if we received a signal because we are quitting the application
THREAD_RETURN( res );
if( current_event == NULL )
{
vp_os_mutex_lock(&event_queue_mutex);
next_index_in_queue = (end_index_in_queue + 1) & (ARDRONE_CONTROL_MAX_NUM_EVENTS_IN_QUEUE - 1);
if( next_index_in_queue != start_index_in_queue )
{ // There's an event to process
current_event = ardrone_control_event_queue[next_index_in_queue];
if( current_event != NULL )
{
if( current_event->ardrone_control_event_start != NULL )
{
current_event->ardrone_control_event_start( current_event );
}
}
end_index_in_queue = next_index_in_queue;
retry = 0;
}
vp_os_mutex_unlock(&event_queue_mutex);
}
if( current_event != NULL )
{
switch( current_event->event )
{
case ARDRONE_UPDATE_CONTROL_MODE:
res = ardrone_control_soft_update_run( current_ardrone_state, (ardrone_control_soft_update_event_t*) current_event );
break;
case PIC_UPDATE_CONTROL_MODE:
res = ardrone_control_soft_update_run( current_ardrone_state, (ardrone_control_soft_update_event_t*) current_event );
break;
case LOGS_GET_CONTROL_MODE:
break;
case CFG_GET_CONTROL_MODE:
case CUSTOM_CFG_GET_CONTROL_MODE: /* multiconfiguration support */
res = ardrone_control_configuration_run( current_ardrone_state, (ardrone_control_configuration_event_t*) current_event );
break;
case ACK_CONTROL_MODE:
res = ardrone_control_ack_run( current_ardrone_state, (ardrone_control_ack_event_t *) current_event);
break;
default:
break;
}
if( VP_FAILED(res) )
{
retry ++;
if( retry > current_event->num_retries)
current_event->status = ARDRONE_CONTROL_EVENT_FINISH_FAILURE;
}
else
{
retry = 0;
}
if( current_event->status & ARDRONE_CONTROL_EVENT_FINISH )
{
if( current_event->ardrone_control_event_end != NULL )
current_event->ardrone_control_event_end( current_event );
/* Make the thread read a new event on the next loop iteration */
current_event = NULL;
}
else
{
/* Not changing 'current_event' makes the loop process the same
* event when the next navdata packet arrives. */
}
}
}
}// while
/* Stephane : Bug fix - mutexes were previously detroyed by another thread,
which made ardrone_control crash.*/
vp_os_mutex_destroy(&event_queue_mutex);
vp_os_cond_destroy(&control_cond);
vp_os_mutex_destroy(&control_mutex);
vp_com_close(COM_CONTROL(), &control_socket);
THREAD_RETURN( res );
}
C_RESULT video_com_stage_connect (video_com_config_t *cfg)
{
#ifdef _WIN32
int timeout_for_windows=1000; /* timeout in milliseconds */
int sizeinit;
#else
struct timeval tv;
// 1 second timeout
tv.tv_sec = 1;
tv.tv_usec = 0;
#endif
C_RESULT res = C_FAIL;
if (TRUE == cfg->connected)
{
PDBG ("Will close");
res = vp_com_close (cfg->com, &cfg->socket);
cfg->connected = FALSE;
if (VP_FAILED (res))
{
PDBG ("Close failed");
return res;
}
}
if( cfg->protocol == VP_COM_PROBE)
{
PRINT("\n\nPROBING\n");
cfg->socket.protocol = VP_COM_TCP;
res = vp_com_open(cfg->com, &cfg->socket, &cfg->read, &cfg->write);
if( VP_SUCCEEDED(res) )
{
PRINT("\n\nTCP\n");
vp_com_close (cfg->com, &cfg->socket);
cfg->protocol = VP_COM_TCP;
}
else
{
PRINT("\n\nUDP\n");
cfg->protocol = VP_COM_UDP;
}
}
if( cfg->protocol == VP_COM_UDP )
{
cfg->socket.protocol = VP_COM_UDP;
cfg->socket.is_multicast = 0; // disable multicast for video
cfg->socket.multicast_base_addr = MULTICAST_BASE_ADDR;
res = vp_com_open(cfg->com, &cfg->socket, &cfg->read, &cfg->write);
if( VP_SUCCEEDED(res) )
{
int numi= 1;
socklen_t numi1= sizeof(int);
#ifdef _WIN32
setsockopt((int32_t)cfg->socket.priv, SOL_SOCKET, SO_RCVTIMEO, SSOPTCAST_RO(&timeout_for_windows), sizeof(timeout_for_windows));
#else
setsockopt((int32_t)cfg->socket.priv, SOL_SOCKET, SO_RCVTIMEO, SSOPTCAST_RO(&tv), sizeof(tv));
#endif
// Increase buffer for receiving datas.
setsockopt( (int32_t)cfg->socket.priv, SOL_SOCKET, SO_DEBUG, SSOPTCAST_RO(&numi), sizeof(numi));
numi = SOCKET_BUFFER_SIZE;
setsockopt( (int32_t)cfg->socket.priv, SOL_SOCKET, SO_RCVBUF, SSOPTCAST_RO(&numi),numi1);
getsockopt( (int32_t)cfg->socket.priv, SOL_SOCKET, SO_RCVBUF, SSOPTCAST_RW(&numi),&numi1);
PDBG ("New buffer size : %d", numi);
numi1 = 0;
setsockopt( (int32_t)cfg->socket.priv, SOL_SOCKET, SO_DEBUG, SSOPTCAST_RO(&numi1), sizeof(numi1));
cfg->connected = TRUE;
}
}
else if( cfg->protocol == VP_COM_TCP )
{
PDBG ("Will open TCP");
res = vp_com_open(cfg->com, &cfg->socket, &cfg->read, &cfg->write);
if( VP_SUCCEEDED(res) )
{
int numi= 1;
socklen_t numi1= sizeof(int);
PDBG ("Success open");
vp_com_sockopt(cfg->com, &cfg->socket, cfg->sockopt);
#ifdef _WIN32
setsockopt((int32_t)cfg->socket.priv, SOL_SOCKET, SO_RCVTIMEO, SSOPTCAST_RO(&timeout_for_windows), sizeof(timeout_for_windows));
#else
setsockopt((int32_t)cfg->socket.priv, SOL_SOCKET, SO_RCVTIMEO, SSOPTCAST_RO(&tv), sizeof(tv));
#endif
// Increase buffer for receiving datas.
setsockopt( (int32_t)cfg->socket.priv, SOL_SOCKET, SO_DEBUG, SSOPTCAST_RO(&numi), sizeof(numi));
numi = SOCKET_BUFFER_SIZE;
setsockopt( (int32_t)cfg->socket.priv, SOL_SOCKET, SO_RCVBUF, SSOPTCAST_RO(&numi),numi1);
getsockopt( (int32_t)cfg->socket.priv, SOL_SOCKET, SO_RCVBUF, SSOPTCAST_RW(&numi),&numi1);
PDBG ("NEW buffer size : %d", numi);
numi1 = 0;
setsockopt( (int32_t)cfg->socket.priv, SOL_SOCKET, SO_DEBUG, SSOPTCAST_RO(&numi1), sizeof(numi1));
cfg->connected = TRUE;
}
}
#ifdef _WIN32
sizeinit = strlen("Init");
vp_com_write_socket(&cfg->socket,"Init",&sizeinit);
#endif
return res;
}
DEFINE_THREAD_ROUTINE(ihm, data)
{
C_RESULT res;
WSADATA wsaData = {0};
int iResult = 0;
/* Initializes Windows socket subsystem */
iResult = WSAStartup(MAKEWORD(2, 2), &wsaData);
if (iResult != 0)
{
wprintf(L"WSAStartup failed: %d\n", iResult);
return 1;
}
/* Initializes communication sockets */
res = test_drone_connection(); // Nick disabled the press enter (wait)
if(res!=0)
{
printf("%s","Could not detect the drone version ... press <Enter> to try connecting anyway.\n");
getchar();
//WSACleanup();
exit(-1);
}
res = ardrone_tool_setup_com( NULL );
if( FAILED(res) )
{
PRINT("Wifi initialization failed.\n");
return -1;
}
START_THREAD(video_stage, 0);
res = ardrone_tool_init(WIFI_ARDRONE_IP, strlen(WIFI_ARDRONE_IP), NULL, ARDRONE_CLIENT_APPNAME, ARDRONE_CLIENT_USRNAME);
ardrone_tool_set_refresh_time(20); // 20 ms
ardrone_at_reset_com_watchdog();
// config
ardrone_control_config.euler_angle_max = 0.20943951f; // 12 degrees
ardrone_control_config.video_channel = ZAP_CHANNEL_VERT;
ardrone_control_config.video_codec = UVLC_CODEC; //P264_CODEC;
ardrone_control_config.navdata_demo = FALSE;
ardrone_control_config.altitude_max = 10000;
ardrone_control_config.control_vz_max = 1000.0f;
ardrone_control_config.outdoor = FALSE;
//ardrone_control_config.flight_without_shell = TRUE;
ARDRONE_TOOL_CONFIGURATION_ADDEVENT(video_channel, &ardrone_control_config.video_channel, (ardrone_tool_configuration_callback) ardrone_demo_config_callback);
//ARDRONE_TOOL_CONFIGURATION_ADDEVENT(video_channel, &ardrone_control_config.video_channel, NULL);
//ARDRONE_TOOL_CONFIGURATION_ADDEVENT(video_codec, &ardrone_control_config.video_codec, NULL);
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (navdata_demo, &ardrone_control_config.navdata_demo, NULL);
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (altitude_max, &ardrone_control_config.altitude_max, NULL);
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (control_vz_max, &ardrone_control_config.control_vz_max, NULL);
//ARDRONE_TOOL_CONFIGURATION_ADDEVENT (outdoor, &ardrone_control_config.outdoor, NULL);
//ARDRONE_TOOL_CONFIGURATION_ADDEVENT (flight_without_shell, &ardrone_control_config.flight_without_shell, NULL);
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (euler_angle_max, &ardrone_control_config.euler_angle_max, NULL);
// flat trim
ardrone_at_set_flat_trim();
//SetEvent(ardrone_ready);
//ardrone_demo_redirect_to_interface = 1;
while( VP_SUCCEEDED(res) && ardrone_tool_exit() == FALSE )
{
res = ardrone_tool_update();
}
JOIN_THREAD(video_stage);
res = ardrone_tool_shutdown();
WSACleanup();
return (THREAD_RET)res;
}
