git.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/ARDroneLib/Soft/Lib/ardrone_tool/Navdata/ardrone_navdata_client.c

   1 #include <errno.h>
   2 #include <string.h>
   3
   4 #include <config.h>
   5
   6 #include <VP_Os/vp_os_print.h>
   7 #include <VP_Com/vp_com.h>
   8
   9 #include <ardrone_api.h>
  10 #include <ardrone_tool/ardrone_tool.h>
  11 #include <ardrone_tool/Navdata/ardrone_navdata_client.h>
  12 #include <ardrone_tool/Com/config_com.h>
  13
  14 #ifndef _WIN32
  15 #include <sys/socket.h>
  16 #include <sys/ioctl.h>
  17 #include <netinet/in.h>
  18 #include <netinet/tcp.h>
  19 #include <unistd.h>
  20 #endif
  21
  22 // PROTO_THREAD_ROUTINE( navdata_update , nomParams );
  23
  24 static bool_t navdata_thread_in_pause = TRUE;
  25 static bool_t bContinue = TRUE;
  26 static uint32_t num_retries = 0;
  27 static vp_os_cond_t navdata_client_condition;
  28 static vp_os_mutex_t navdata_client_mutex;
  29
  30 static vp_com_socket_t navdata_socket;
  31 static Read navdata_read      = NULL;
  32 static Write navdata_write    = NULL;
  33
  34 static uint8_t navdata_buffer[NAVDATA_MAX_SIZE];
  35 navdata_unpacked_t navdata_unpacked;
  36
  37 C_RESULT ardrone_navdata_client_init(void)
  38 {
  39   C_RESULT res;
  40
  41   COM_CONFIG_SOCKET_NAVDATA(&navdata_socket, VP_COM_CLIENT, NAVDATA_PORT, wifi_ardrone_ip);
  42   navdata_socket.protocol = VP_COM_UDP;
  43   navdata_socket.is_multicast = 1;      // enable multicast for Navdata
  44   navdata_socket.multicast_base_addr = MULTICAST_BASE_ADDR;
  45
  46   vp_os_mutex_init(&navdata_client_mutex);
  47   vp_os_cond_init(&navdata_client_condition, &navdata_client_mutex);
  48
  49   res = C_OK;
  50
  51   return res;
  52 }
  53
  54 C_RESULT ardrone_navdata_client_suspend(void)
  55 {
  56         vp_os_mutex_lock(&navdata_client_mutex);
  57         navdata_thread_in_pause = TRUE;
  58         vp_os_mutex_unlock(&navdata_client_mutex);
  59
  60         return C_OK;
  61 }
  62
  63 C_RESULT ardrone_navdata_client_resume(void)
  64 {
  65         vp_os_mutex_lock(&navdata_client_mutex);
  66         vp_os_cond_signal(&navdata_client_condition);
  67         navdata_thread_in_pause = FALSE;
  68         vp_os_mutex_unlock(&navdata_client_mutex);
  69
  70         return C_OK;
  71 }
  72
  73 C_RESULT ardrone_navdata_open_server(void)
  74 {
  75   // Flag value :
  76   // 1 -> Unicast
  77   // 2 -> Multicast
  78   int32_t flag = 1, len = sizeof(flag);
  79
  80   if( navdata_write != NULL )
  81   {
  82     if (navdata_socket.is_multicast == 1)
  83       flag = 2;
  84
  85     navdata_write(&navdata_socket, (const int8_t*) &flag, &len);
  86   }
  87
  88   return C_OK;
  89 }
  90
  91 DEFINE_THREAD_ROUTINE( navdata_update, nomParams )
  92 {
  93   C_RESULT res;
  94   int32_t  i, size;
  95   uint32_t cks, navdata_cks, sequence = NAVDATA_SEQUENCE_DEFAULT-1;
  96   struct timeval tv;
  97 #ifdef _WIN32
  98   int timeout_for_windows=1000/*milliseconds*/;
  99 #endif
 100
 101
 102   navdata_t* navdata = (navdata_t*) &navdata_buffer[0];
 103
 104   tv.tv_sec   = 1/*second*/;
 105   tv.tv_usec  = 0;
 106
 107   res = C_OK;
 108
 109   if( VP_FAILED(vp_com_open(COM_NAVDATA(), &navdata_socket, &navdata_read, &navdata_write)) )
 110   {
 111     printf("VP_Com : Failed to open socket for navdata\n");
 112     res = C_FAIL;
 113   }
 114
 115   if( VP_SUCCEEDED(res) )
 116   {
 117     PRINT("Thread navdata_update in progress...\n");
 118
 119 #ifdef _WIN32
 120         setsockopt((int32_t)navdata_socket.priv, SOL_SOCKET, SO_RCVTIMEO, (const char*)&timeout_for_windows, sizeof(timeout_for_windows));
 121         /* Added by Stephane to force the drone start sending data. */
 122         if(navdata_write)
 123         {       int sizeinit = 5; navdata_write( (void*)&navdata_socket, (int8_t*)"Init", &sizeinit ); }
 124 #else
 125         setsockopt((int32_t)navdata_socket.priv, SOL_SOCKET, SO_RCVTIMEO, (const char*)&tv, sizeof(tv));
 126 #endif
 127
 128
 129     i = 0;
 130     while( ardrone_navdata_handler_table[i].init != NULL )
 131     {
 132       // if init failed for an handler we set its process function to null
 133       // We keep its release function for cleanup
 134       if( VP_FAILED( ardrone_navdata_handler_table[i].init(ardrone_navdata_handler_table[i].data) ) )
 135         ardrone_navdata_handler_table[i].process = NULL;
 136
 137       i ++;
 138     }
 139
 140    navdata_thread_in_pause = FALSE;
 141     while( VP_SUCCEEDED(res)
 142            && !ardrone_tool_exit()
 143            && bContinue )
 144     {
 145           if(navdata_thread_in_pause)
 146           {
 147                 vp_os_mutex_lock(&navdata_client_mutex);
 148                 num_retries = NAVDATA_MAX_RETRIES + 1;
 149                 vp_os_cond_wait(&navdata_client_condition);
 150                 vp_os_mutex_unlock(&navdata_client_mutex);
 151           }
 152
 153                 if( navdata_read == NULL )
 154       {
 155         res = C_FAIL;
 156         continue;
 157       }
 158
 159       size = NAVDATA_MAX_SIZE;
 160       navdata->header = 0; // Soft reset
 161       res = navdata_read( (void*)&navdata_socket, (int8_t*)&navdata_buffer[0], &size );
 162
 163 #ifdef _WIN32
 164           if( size <= 0 )
 165 #else
 166           if( size == 0 )
 167 #endif
 168                 {
 169                         // timeout
 170                         PRINT("Timeout when reading navdatas - resending a navdata request on port %i\n",NAVDATA_PORT);
 171                         /* Resend a request to the drone to get navdatas */
 172                         ardrone_navdata_open_server();
 173                         sequence = NAVDATA_SEQUENCE_DEFAULT-1;
 174                         num_retries++;
 175                 }
 176                 else
 177          num_retries = 0;
 178
 179       if( VP_SUCCEEDED( res ) )
 180       {
 181         if( navdata->header == NAVDATA_HEADER )
 182         {
 183           if( ardrone_get_mask_from_state(navdata->ardrone_state, ARDRONE_COM_WATCHDOG_MASK) )
 184           {
 185             // reset sequence number because of com watchdog
 186             // This code is mandatory because we can have a com watchdog without detecting it on mobile side :
 187             //        Reconnection is fast enough (less than one second)
 188             sequence = NAVDATA_SEQUENCE_DEFAULT-1;
 189
 190             if( ardrone_get_mask_from_state(navdata->ardrone_state, ARDRONE_NAVDATA_BOOTSTRAP) == FALSE )
 191               ardrone_tool_send_com_watchdog(); // acknowledge
 192           }
 193
 194           if( navdata->sequence > sequence )
 195           {
 196             i = 0;
 197
 198             ardrone_navdata_unpack_all(&navdata_unpacked, navdata, &navdata_cks);
 199             cks = ardrone_navdata_compute_cks( &navdata_buffer[0], size - sizeof(navdata_cks_t) );
 200
 201             if( cks == navdata_cks )
 202             {
 203               while( ardrone_navdata_handler_table[i].init != NULL )
 204               {
 205                 if( ardrone_navdata_handler_table[i].process != NULL )
 206                   ardrone_navdata_handler_table[i].process( &navdata_unpacked );
 207
 208                 i++;
 209               }
 210             }
 211             else
 212             {
 213               PRINT("[Navdata] Checksum failed : %d (distant) / %d (local)\n", navdata_cks, cks);
 214             }
 215           }
 216           else
 217           {
 218             PRINT("[Navdata] Sequence pb : %d (distant) / %d (local)\n", navdata->sequence, sequence);
 219           }
 220
 221           // remaining = sizeof(navdata);
 222
 223           sequence = navdata->sequence;
 224         }
 225       }
 226     }
 227
 228     // Release resources alllocated by handlers
 229     i = 0;
 230     while( ardrone_navdata_handler_table[i].init != NULL )
 231     {
 232       ardrone_navdata_handler_table[i].release();
 233
 234       i ++;
 235     }
 236   }
 237
 238   vp_com_close(COM_NAVDATA(), &navdata_socket);
 239
 240   DEBUG_PRINT_SDK("Thread navdata_update ended\n");
 241
 242   return (THREAD_RET)res;
 243 }
 244
 245 uint32_t ardrone_navdata_client_get_num_retries(void)
 246 {
 247   return num_retries;
 248 }
 249
 250 C_RESULT ardrone_navdata_client_shutdown(void)
 251 {
 252    bContinue = FALSE;
 253
 254    return C_OK;
 255 }
 256
 257