--- /dev/null
+/********************************************************************
+ * COPYRIGHT PARROT 2010
+ ********************************************************************
+ * PARROT - A.R.Drone SDK Windows Client Example
+ *-----------------------------------------------------------------*/
+/**
+ * @file ardrone_tool_win32.c
+ * @brief Main program of all client programs using ARDroneTool.
+ *
+ * @author Stephane Piskorski <stephane.piskorski.ext@parrot.fr>
+ * @date Sept, 8. 2010
+ * @warning This file is a modified version of the 'ardrone_tool.c'
+ * of the A.R. Drone SDK
+ *
+ *******************************************************************/
+
+
+#pragma warning( disable : 4996 ) // disable deprecation warning
+
+#include <stdlib.h>
+
+#include <VP_Os/vp_os_malloc.h>
+#include <VP_Os/vp_os_print.h>
+#include <VP_Api/vp_api_thread_helper.h>
+
+#include <VP_Com/vp_com.h>
+
+#include <ardrone_tool/ardrone_tool.h>
+#include <ardrone_tool/ardrone_time.h>
+#include <ardrone_tool/Control/ardrone_control.h>
+#include <ardrone_tool/Control/ardrone_control_ack.h>
+#include <ardrone_tool/Navdata/ardrone_navdata_client.h>
+#include <ardrone_tool/UI/ardrone_input.h>
+#include <ardrone_tool/Com/config_com.h>
+
+
+int32_t MiscVar[NB_MISC_VARS] = {
+ DEFAULT_MISC1_VALUE,
+ DEFAULT_MISC2_VALUE,
+ DEFAULT_MISC3_VALUE,
+ DEFAULT_MISC4_VALUE
+ };
+
+static bool_t need_update = TRUE;
+static ardrone_timer_t ardrone_tool_timer;
+static int ArdroneToolRefreshTimeInMs = ARDRONE_REFRESH_MS;
+
+/* Those should be defined in ArDroneLib, but there are compilation issues to solve */
+unsigned long bswap(unsigned long x) { return _byteswap_ulong(x); }
+/*
+#include <cmnintrin.h>
+int clz(unsigned long x) { return _CountLeadingZeros(x); }
+*/
+
+int clz(unsigned long x)
+{
+ /* Barbarian counting method if no instrinsic is available */
+ int i; const int L=sizeof(x)*8-1;
+ const unsigned long mask = ( 1 << L );
+ if (x==0) { return L+1; }
+ for (i=0;i<L;i++) { if (x&mask) return i; x<<=1; }
+ return i;
+}
+
+
+char wifi_ardrone_ip[256] = { WIFI_ARDRONE_IP };
+
+
+/*---------------------------------------------------------------------------------------------------------------------
+Array containing parameters to be sent to the drone before starting flight.
+Those parameters are sent one by one with the use of an acknowledgment to make sure they
+were properly set on the drone.
+---------------------------------------------------------------------------------------------------------------------*/
+/// Remote data configuration ///
+ardrone_tool_configure_data_t configure_data[] = {
+ { "general:navdata_demo", "FALSE" },
+ { NULL, NULL }
+};
+
+
+
+static int32_t configure_index = 0;
+static ardrone_control_ack_event_t ack_config;
+static bool_t send_com_watchdog = FALSE;
+
+
+
+/*---------------------------------------------------------------------------------------------------------------------
+Makes ardrone tool send a keepalive command to the drone, when the drone detects inactivity for too long.
+---------------------------------------------------------------------------------------------------------------------*/
+void ardrone_tool_send_com_watchdog( void )
+{
+ send_com_watchdog = TRUE;
+}
+
+
+/*---------------------------------------------------------------------------------------------------------------------
+Callback function used by 'ardrone_tool_configure' and by itself.
+This function uses the drone control socket and its acknowlegment system to securely
+set some parameters on the drone.
+The parameters to set are stored in the 'configure_data' array.
+---------------------------------------------------------------------------------------------------------------------*/
+
+static void ardrone_tool_end_configure( struct _ardrone_control_event_t* event )
+{
+ if( event->status == ARDRONE_CONTROL_EVENT_FINISH_SUCCESS )
+ configure_index ++;
+
+ if( configure_data[configure_index].var != NULL && configure_data[configure_index].value != NULL )
+ {
+ ack_config.event = ACK_CONTROL_MODE;
+ ack_config.num_retries = 20;
+ ack_config.status = ARDRONE_CONTROL_EVENT_WAITING;
+ ack_config.ardrone_control_event_start = NULL;
+ ack_config.ardrone_control_event_end = ardrone_tool_end_configure;
+ ack_config.ack_state = ACK_COMMAND_MASK_TRUE;
+
+ ardrone_at_set_toy_configuration( configure_data[configure_index].var, configure_data[configure_index].value );
+ ardrone_at_send();
+
+ ardrone_control_send_event( (ardrone_control_event_t*)&ack_config );
+ }
+}
+
+
+/*---------------------------------------------------------------------------------------------------------------------
+See 'ardrone_tool_end_configure'
+---------------------------------------------------------------------------------------------------------------------*/
+
+static C_RESULT ardrone_tool_configure()
+{
+ if( configure_data[configure_index].var != NULL && configure_data[configure_index].value != NULL )
+ {
+ ack_config.event = ACK_CONTROL_MODE;
+ ack_config.num_retries = 20;
+ ack_config.status = ARDRONE_CONTROL_EVENT_WAITING;
+ ack_config.ardrone_control_event_start = NULL;
+ ack_config.ardrone_control_event_end = ardrone_tool_end_configure;
+ ack_config.ack_state = ACK_COMMAND_MASK_TRUE;
+
+ ardrone_at_set_toy_configuration( configure_data[configure_index].var, configure_data[configure_index].value );
+ ardrone_at_send();
+
+ ardrone_control_send_event( (ardrone_control_event_t*)&ack_config );
+ }
+
+ return C_OK;
+}
+
+
+/*---------------------------------------------------------------------------------------------------------------------
+---------------------------------------------------------------------------------------------------------------------*/
+static void ardrone_toy_network_adapter_cb( const char* name )
+{
+ strcpy( COM_CONFIG_NAVDATA()->itfName, name );
+}
+
+
+/*---------------------------------------------------------------------------------------------------------------------
+---------------------------------------------------------------------------------------------------------------------*/
+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_init(int argc, char **argv)
+{
+ C_RESULT res;
+
+ //Fill structure AT codec and built the library AT commands.
+ ardrone_at_init( wifi_ardrone_ip, strlen( wifi_ardrone_ip) );
+
+ // Init subsystems
+ ardrone_timer_reset(&ardrone_tool_timer);
+
+ ardrone_tool_input_init();
+ ardrone_control_init();
+ ardrone_navdata_client_init();
+
+ // Init custom tool
+ res = ardrone_tool_init_custom(argc, argv);
+
+ //Opens a connection to AT port.
+ ardrone_at_open();
+
+
+ START_THREAD(navdata_update, 0);
+ START_THREAD(ardrone_control, 0);
+
+ ardrone_tool_configure();
+
+ // Send start up configuration
+ ardrone_at_set_pmode( MiscVar[0] );
+ ardrone_at_set_ui_misc( MiscVar[0], MiscVar[1], MiscVar[2], MiscVar[3] );
+
+ return res;
+}
+
+
+C_RESULT ardrone_tool_set_refresh_time(int refresh_time_in_ms)
+{
+ ArdroneToolRefreshTimeInMs = refresh_time_in_ms;
+
+ return C_OK;
+}
+
+C_RESULT ardrone_tool_pause( void )
+{
+ ardrone_navdata_client_suspend();
+
+ return C_OK;
+}
+
+C_RESULT ardrone_tool_resume( void )
+{
+ ardrone_navdata_client_resume();
+
+ return C_OK;
+}
+
+
+
+/*---------------------------------------------------------------------------------------------------------------------
+Updates the AT command client by flushing pending commands.
+---------------------------------------------------------------------------------------------------------------------*/
+C_RESULT ardrone_tool_update()
+{
+ int delta;
+
+ C_RESULT res = C_OK;
+
+ // Update subsystems & custom tool
+ if( need_update )
+ {
+ ardrone_timer_update(&ardrone_tool_timer);
+
+ ardrone_tool_input_update();
+ res = ardrone_tool_update_custom();
+
+ if( send_com_watchdog == TRUE )
+ {
+ ardrone_at_reset_com_watchdog();
+ send_com_watchdog = FALSE;
+ }
+ // Send all pushed messages
+ ardrone_at_send();
+
+ need_update = FALSE;
+ }
+
+ delta = ardrone_timer_delta_ms(&ardrone_tool_timer);
+ if( delta >= ArdroneToolRefreshTimeInMs)
+ {
+ // Render frame
+ res = ardrone_tool_display_custom();
+ need_update = TRUE;
+ }
+ else
+ {
+ Sleep((ArdroneToolRefreshTimeInMs - delta));
+ }
+
+ return res;
+}
+
+
+
+
+/*---------------------------------------------------------------------------------------------------------------------
+Stops the drone controlling subsystem.
+---------------------------------------------------------------------------------------------------------------------*/
+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;
+}
+
+
+
+/*---------------------------------------------------------------------------------------------------------------------
+Tests the network connection to the drone by fetching the drone version number
+through the FTP server embedded on the drone.
+This is how FreeFlight checks if a drone sofware update is required.
+
+The FTP connection process is a quick and (very)dirty one. It uses FTP passive mode.
+---------------------------------------------------------------------------------------------------------------------*/
+int test_drone_connection()
+{
+ const char * passivdeModeHeader = "\r\n227 PASV ok (";
+ vp_com_socket_t ftp_client,ftp_client2;
+ char buffer[1024];
+ static Write ftp_write = NULL;
+ static Read ftp_read = NULL;
+ int bytes_to_send,received_bytes;
+ int i,L,x[6],port;
+ int timeout_windows = 1000; /*milliseconds*/
+
+ vp_os_memset(buffer,0,sizeof(buffer));
+
+ /* Connects to the FTP server */
+ wifi_config_socket(&ftp_client,VP_COM_CLIENT,FTP_PORT,WIFI_ARDRONE_IP);
+ ftp_client.protocol = VP_COM_TCP;
+ if(VP_FAILED(vp_com_init(wifi_com()))) return -1;
+ if(VP_FAILED(vp_com_open(wifi_com(), &ftp_client, &ftp_read, &ftp_write))) return -2;
+ setsockopt((int32_t)ftp_client.priv,
+ SOL_SOCKET,
+ SO_RCVTIMEO,
+ (const char*)&timeout_windows, sizeof(timeout_windows)
+ );
+
+ /* Request version file */
+ bytes_to_send = _snprintf(buffer,sizeof(buffer),"%s",\r "USER anonymous\r\nCWD /\r\nPWD\r\nTYPE A\r\nPASV\r\nRETR version.txt\r\n");\r ftp_write(&ftp_client,buffer,&bytes_to_send);
+ /* Dirty. We should wait for data to arrive with some kind of synchronization
+ or make the socket blocking.*/
+ Sleep(1000);
+
+ /* Gets the data port */
+ received_bytes = sizeof(buffer);
+ ftp_read(&ftp_client,buffer,&received_bytes);
+ if (received_bytes<1) { vp_com_close(wifi_com(), &ftp_client); return -3; }
+ L=received_bytes-strlen(passivdeModeHeader);
+
+ /* Searches for the passive mode acknowlegment from the FTP server */
+ for (i=0;i<L;i++) {
+ if (strncmp((buffer+i),passivdeModeHeader,strlen(passivdeModeHeader))==0) break;
+ }
+ if (i==L) {
+ vp_com_close(wifi_com(), &ftp_client); return -4;
+ }
+ i+=strlen(passivdeModeHeader);
+ if (sscanf(buffer+i,"%i,%i,%i,%i,%i,%i)",&x[0],&x[1],&x[2],&x[3],&x[4],&x[5])!=6)
+ { vp_com_close(wifi_com(), &ftp_client); return -5; }
+ port=(x[4]<<8)+x[5];
+
+ /* Connects to the FTP server data port */
+ wifi_config_socket(&ftp_client2,VP_COM_CLIENT,port,"192.168.1.1");
+ ftp_client2.protocol = VP_COM_TCP;
+ if(VP_FAILED(vp_com_init(wifi_com())))
+ { vp_com_close(wifi_com(), &ftp_client2); return -6; }
+ if(VP_FAILED(vp_com_open(wifi_com(), &ftp_client2, &ftp_read, &ftp_write)))
+ { vp_com_close(wifi_com(), &ftp_client2); return -7; }
+
+ /* Gets the data */
+ received_bytes = sizeof(buffer);
+ ftp_read(&ftp_client2,buffer,&received_bytes);
+ if (received_bytes>0) {
+ buffer[min(received_bytes,sizeof(buffer)-1)]=0;
+ printf("Drone version %s detected ... press <Enter> to start the application.\n",buffer);
+ getchar();
+ }
+
+ /* Clean up */
+ vp_com_close(wifi_com(), &ftp_client);
+ vp_com_close(wifi_com(), &ftp_client2);
+
+ return 0;
+}
+
+
+
+
+/*---------------------------------------------------------------------------------------------------------------------
+Main application function
+---------------------------------------------------------------------------------------------------------------------*/
+
+int sdk_demo_stop=0;
+
+int main(int argc, char **argv)
+{
+ C_RESULT res; // functions return value
+ //const char* old_locale=NULL;
+ const char* appname = argv[0];
+ //int argc_backup = argc;
+ //char** argv_backup = argv;
+
+ 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
+
+ /* Initializes communication sockets */
+ 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);
+
+ /* Keeps sending AT commands to control the drone as long as
+ everything is OK */
+ while( VP_SUCCEEDED(res) && ardrone_tool_exit() == FALSE ) {
+ res = ardrone_tool_update(); }
+
+ /**/
+ res = ardrone_tool_shutdown();
+
+ WSACleanup();
+
+ /* Bye bye */
+ system("cls");
+ printf("End of SDK Demo for Windows\n");
+ getchar();
+ return VP_SUCCEEDED(res) ? 0 : -1;
+}
+
+
+
+
+// Default implementation for weak functions
+ C_RESULT ardrone_tool_update_custom() { return C_OK; }
+ C_RESULT ardrone_tool_display_custom() { return C_OK; }
+ C_RESULT ardrone_tool_check_argc_custom( int32_t argc) { return C_OK; }
+ void ardrone_tool_display_cmd_line_custom( void ) {}
+ bool_t ardrone_tool_parse_cmd_line_custom( const char* cmd ) { return TRUE; }
+
+
projects / mardrone / blobdiff