C_RESULT ardrone_tool_shutdown()
{
C_RESULT res = C_OK;
#ifndef NO_ARDRONE_MAINLOOP
res = ardrone_tool_shutdown_custom();
#endif
// Shutdown subsystems
ardrone_navdata_client_shutdown();
ardrone_control_shutdown();
ardrone_tool_input_shutdown();
JOIN_THREAD(ardrone_control);
JOIN_THREAD(navdata_update);
// Shutdown AT Commands
ATcodec_exit_thread();
ATcodec_Shutdown_Library();
vp_com_disconnect(COM_NAVDATA());
vp_com_shutdown(COM_NAVDATA());
PRINT("Custom ardrone tool ended\n");
return res;
}
C_RESULT ardrone_tool_shutdown()
{
C_RESULT res = C_OK;
res = ardrone_tool_shutdown_custom();
// Shutdown subsystems
ardrone_navdata_client_shutdown();
ardrone_control_shutdown();
ardrone_tool_input_shutdown();
JOIN_THREAD(ardrone_control);
JOIN_THREAD(navdata_update);
fflush (NULL);
// Shutdown AT Commands
ATcodec_exit_thread();
ATcodec_Shutdown_Library();
vp_com_disconnect(COM_NAVDATA());
vp_com_shutdown(COM_NAVDATA());
academy_download_shutdown();
academy_shutdown();
PRINT("Custom ardrone tool ended\n");
return res;
}
C_RESULT ardrone_tool_setup_com( const char* ssid )
{
C_RESULT res = C_OK;
vp_com_init(COM_NAVDATA());
vp_com_local_config(COM_NAVDATA(), COM_CONFIG_NAVDATA());
vp_com_connect(COM_NAVDATA(), COM_CONNECTION_NAVDATA(), NUM_ATTEMPTS);
((vp_com_wifi_connection_t*)wifi_connection())->is_up=1;
return res;
}
C_RESULT ardrone_tool_setup_com( const char* ssid )
{
C_RESULT res = C_OK;
#ifdef CHECK_WIFI_CONFIG
if( FAILED(vp_com_init(COM_NAVDATA())) )
{
DEBUG_PRINT_SDK("VP_Com : Failed to init com for navdata\n");
vp_com_shutdown(COM_NAVDATA());
res = C_FAIL;
}
vp_com_network_adapter_lookup(COM_NAVDATA(), ardrone_toy_network_adapter_cb);
if( SUCCEED(res) && FAILED(vp_com_local_config(COM_NAVDATA(), COM_CONFIG_NAVDATA())) )
{
DEBUG_PRINT_SDK("VP_Com : Failed to configure com for navdata\n");
vp_com_shutdown(COM_NAVDATA());
res = C_FAIL;
}
if( ssid != NULL )
{
strcpy( ((vp_com_wifi_connection_t*)wifi_connection())->networkName, ssid );
}
if( SUCCEED(res) && FAILED(vp_com_connect(COM_NAVDATA(), COM_CONNECTION_NAVDATA(), NUM_ATTEMPTS)))
{
DEBUG_PRINT_SDK("VP_Com: Failed to connect for navdata\n");
vp_com_shutdown(COM_NAVDATA());
res = C_FAIL;
}
#else
vp_com_init(COM_NAVDATA());
vp_com_network_adapter_lookup(COM_NAVDATA(), ardrone_toy_network_adapter_cb);
vp_com_local_config(COM_NAVDATA(), COM_CONFIG_NAVDATA());
if( ssid != NULL )
{
strcpy( ((vp_com_wifi_connection_t*)wifi_connection())->networkName, ssid );
}
vp_com_connect(COM_NAVDATA(), COM_CONNECTION_NAVDATA(), NUM_ATTEMPTS);
((vp_com_wifi_connection_t*)wifi_connection())->is_up=1;
#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;
}
