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