git.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/ARDroneLib/Soft/Lib/ardrone_tool/Control/ardrone_control.c

   1 #include <config.h>
   2
   3 #include <VP_Os/vp_os_print.h>
   4 #include <VP_Com/vp_com.h>
   5
   6 #include <ardrone_api.h>
   7 #include <ardrone_tool/ardrone_tool.h>
   8 #include <ardrone_tool/Control/ardrone_control.h>
   9 #include <ardrone_tool/Control/ardrone_control_configuration.h>
  10 #include <ardrone_tool/Control/ardrone_control_soft_update.h>
  11 #include <ardrone_tool/Control/ardrone_control_ack.h>
  12 #include <ardrone_tool/Com/config_com.h>
  13 #include <ardrone_tool/Com/config_wifi.h>
  14
  15 #ifndef _WIN32
  16 #include <sys/socket.h>
  17 #include <sys/ioctl.h>
  18 #include <netinet/in.h>
  19 #include <netinet/tcp.h>
  20 #include <unistd.h>
  21 #endif
  22
  23 static vp_com_socket_t  control_socket;
  24 static Read             control_read    = NULL;
  25 static Write            control_write   = NULL;
  26
  27 static vp_os_mutex_t  control_mutex;
  28 static vp_os_cond_t   control_cond;
  29
  30 static bool_t   control_waited;
  31 static bool_t bContinue = TRUE;
  32 static uint32_t ardrone_state;
  33
  34 static int32_t start_index_in_queue;
  35 static int32_t end_index_in_queue;
  36 static ardrone_control_event_ptr_t  ardrone_control_event_queue[ARDRONE_CONTROL_MAX_NUM_EVENTS_IN_QUEUE];
  37
  38 static vp_os_mutex_t  event_queue_mutex;
  39
  40 C_RESULT ardrone_control_init(void)
  41 {
  42   COM_CONFIG_SOCKET_CONTROL(&control_socket, VP_COM_CLIENT, CONTROL_PORT, wifi_ardrone_ip);
  43
  44   control_waited = FALSE;
  45   ardrone_state   = 0;
  46
  47   vp_os_mutex_init(&control_mutex);
  48   vp_os_cond_init(&control_cond, &control_mutex);
  49
  50   vp_os_mutex_init(&event_queue_mutex);
  51
  52   start_index_in_queue = 0;
  53   end_index_in_queue   = (start_index_in_queue - 1) & (ARDRONE_CONTROL_MAX_NUM_EVENTS_IN_QUEUE - 1);
  54
  55   return C_OK;
  56 }
  57
  58 C_RESULT ardrone_control_shutdown(void)
  59 {
  60         ardrone_control_resume_on_navdata_received(0);
  61         /*BUG FIX : Dont destroy the mutexes here,
  62         they are still being used by the ardrone_control thread,
  63         while this function is called by another thread.*/
  64         #ifdef THIS_IS_A_BUG
  65         vp_os_mutex_destroy(&event_queue_mutex);
  66         vp_os_cond_destroy(&control_cond);
  67         vp_os_mutex_destroy(&control_mutex);*/
  68         #endif
  69
  70         bContinue = FALSE;
  71
  72         return C_OK;
  73 }
  74
  75 C_RESULT ardrone_control_connect_to_drone()
  76 {
  77         int res_open_socket;
  78
  79 #ifdef _WIN32
  80         int timeout_windows=1000;/*milliseconds*/
  81 #else
  82         struct timeval tv;
  83 #endif
  84
  85         vp_com_close(COM_CONTROL(), &control_socket);
  86
  87         res_open_socket = vp_com_open(COM_CONTROL(), &control_socket, &control_read, &control_write);
  88         if( VP_SUCCEEDED(res_open_socket) )
  89         {
  90                 tv.tv_sec   = 1;
  91                 tv.tv_usec  = 0;
  92
  93                 setsockopt((int32_t)control_socket.priv,
  94                                         SOL_SOCKET,
  95                                         SO_RCVTIMEO,
  96                                         #ifdef _WIN32
  97                                                 (const char*)&timeout_windows, sizeof(timeout_windows)
  98                                         #else
  99                                                 (const char*)&tv, sizeof(tv)
 100                                         #endif
 101                                         );
 102
 103                 control_socket.is_disable = FALSE;
 104                 return C_OK;
 105         }
 106         else
 107         {
 108                 DEBUG_PRINT_SDK("VP_Com : Failed to open socket for control\n");
 109                 perror("FTOSFC");
 110                 return C_FAIL;
 111         }
 112 }
 113
 114
 115 /**
 116  * \brief Signals the client control thread that new navdata were received.
 117  * Called by one of the navdata callbacks.
 118  */
 119 C_RESULT ardrone_control_resume_on_navdata_received(uint32_t new_ardrone_state)
 120 {
 121         vp_os_mutex_lock(&control_mutex);
 122     if( control_waited )
 123         {
 124                 ardrone_state = new_ardrone_state;
 125                 vp_os_cond_signal(&control_cond);
 126         }
 127         vp_os_mutex_unlock(&control_mutex);
 128         return C_OK;
 129 }
 130
 131 C_RESULT ardrone_control_read(int8_t* buffer, int32_t* size)
 132 {
 133   C_RESULT res = C_FAIL;
 134
 135   if( control_read != NULL )
 136   {
 137           res = control_read(&control_socket, (int8_t*) buffer, size);
 138           if(*size == 0)
 139           {
 140                   res = C_FAIL;
 141           }
 142   }
 143
 144   return res;
 145 }
 146
 147 C_RESULT ardrone_control_write(const int8_t* buffer, int32_t* size)
 148 {
 149   C_RESULT res = C_FAIL;
 150
 151   if( control_write != NULL )
 152   {
 153     res = control_write(&control_socket, buffer, size);
 154   }
 155
 156   return res;
 157 }
 158
 159 C_RESULT ardrone_control_send_event( ardrone_control_event_t* event )
 160 {
 161   C_RESULT res;
 162   int32_t next_index_in_queue;
 163
 164   res = C_FAIL;
 165
 166   vp_os_mutex_lock(&event_queue_mutex);
 167     next_index_in_queue = (start_index_in_queue + 1) & (ARDRONE_CONTROL_MAX_NUM_EVENTS_IN_QUEUE - 1);
 168     if( next_index_in_queue != end_index_in_queue )
 169     {
 170       ardrone_control_event_queue[start_index_in_queue] = event;
 171       start_index_in_queue = next_index_in_queue;
 172
 173       res = C_OK;
 174   }
 175
 176   vp_os_mutex_unlock(&event_queue_mutex);
 177
 178   return res;
 179 }
 180
 181 DEFINE_THREAD_ROUTINE( ardrone_control, nomParams )
 182 {
 183         C_RESULT res_wait_navdata = C_OK;
 184         C_RESULT res = C_OK;
 185         uint32_t retry, current_ardrone_state;
 186         int32_t next_index_in_queue;
 187         ardrone_control_event_ptr_t  current_event;
 188
 189         retry = 0;
 190         current_event = NULL;
 191
 192         DEBUG_PRINT_SDK("Thread control in progress...\n");
 193         control_socket.is_disable = TRUE;
 194
 195         ardrone_control_connect_to_drone();
 196
 197         while( bContinue
 198           && !ardrone_tool_exit() )
 199         {
 200                 vp_os_mutex_lock(&control_mutex);
 201                 control_waited = TRUE;
 202
 203                 /* Wait for new navdata to be received. */
 204                 res_wait_navdata = vp_os_cond_timed_wait(&control_cond, 1000);
 205                 vp_os_mutex_unlock(&control_mutex);
 206
 207                 /*
 208                  * In case of timeout on the navdata, we assume that there was a problem
 209                  * with the Wifi connection.
 210                  * It is then safer to close and reopen the control socket (TCP 5559) since
 211                  * some OS might stop giving data but not signal any disconnection.
 212                  */
 213                 if(VP_FAILED(res_wait_navdata))
 214                 {
 215                         DEBUG_PRINT_SDK("Timeout while waiting for new navdata.\n");
 216                         if(!control_socket.is_disable)
 217                                 control_socket.is_disable = TRUE;
 218                 }
 219
 220                 if(control_socket.is_disable)
 221                 {
 222                         ardrone_control_connect_to_drone();
 223                 }
 224
 225                 if(VP_SUCCEEDED(res_wait_navdata) && (!control_socket.is_disable))
 226                 {
 227                         vp_os_mutex_lock(&control_mutex);
 228                         current_ardrone_state = ardrone_state;
 229                         control_waited = FALSE;
 230                         vp_os_mutex_unlock(&control_mutex);
 231
 232                         if( ardrone_tool_exit() ) // Test if we received a signal because we are quitting the application
 233                                 THREAD_RETURN( res );
 234
 235                         if( current_event == NULL )
 236                         {
 237                                 vp_os_mutex_lock(&event_queue_mutex);
 238                                 next_index_in_queue = (end_index_in_queue + 1) & (ARDRONE_CONTROL_MAX_NUM_EVENTS_IN_QUEUE - 1);
 239
 240                                 if( next_index_in_queue != start_index_in_queue )
 241                                 { // There's an event to process
 242                                         current_event = ardrone_control_event_queue[next_index_in_queue];
 243                                         if( current_event != NULL )
 244                                         {
 245                                                 if( current_event->ardrone_control_event_start != NULL )
 246                                                 {
 247                                                         current_event->ardrone_control_event_start( current_event );
 248                                                 }
 249                                         }
 250                                         end_index_in_queue = next_index_in_queue;
 251
 252                                         retry = 0;
 253                                 }
 254
 255                                 vp_os_mutex_unlock(&event_queue_mutex);
 256                         }
 257
 258                         if( current_event != NULL )
 259                         {
 260                                 switch( current_event->event )
 261                                 {
 262                                         case ARDRONE_UPDATE_CONTROL_MODE:
 263                                                 res = ardrone_control_soft_update_run( current_ardrone_state, (ardrone_control_soft_update_event_t*) current_event );
 264                                                 break;
 265
 266                                         case PIC_UPDATE_CONTROL_MODE:
 267                                                 res = ardrone_control_soft_update_run( current_ardrone_state, (ardrone_control_soft_update_event_t*) current_event );
 268                                                 break;
 269
 270                                         case LOGS_GET_CONTROL_MODE:
 271                                                 break;
 272
 273                                         case CFG_GET_CONTROL_MODE:
 274                                         case CUSTOM_CFG_GET_CONTROL_MODE: /* multiconfiguration support */
 275                                                 res = ardrone_control_configuration_run( current_ardrone_state, (ardrone_control_configuration_event_t*) current_event );
 276                                                 break;
 277
 278                                         case ACK_CONTROL_MODE:
 279                                                 res = ardrone_control_ack_run( current_ardrone_state, (ardrone_control_ack_event_t *) current_event);
 280                                                 break;
 281
 282                                         default:
 283                                                 break;
 284                                 }
 285
 286                                 if( VP_FAILED(res) )
 287                                 {
 288                                         retry ++;
 289                                         if( retry > current_event->num_retries)
 290                                                 current_event->status = ARDRONE_CONTROL_EVENT_FINISH_FAILURE;
 291                                 }
 292                                 else
 293                                 {
 294                                         retry = 0;
 295                                 }
 296
 297                                 if( current_event->status & ARDRONE_CONTROL_EVENT_FINISH )
 298                                 {
 299                                         if( current_event->ardrone_control_event_end != NULL )
 300                                                 current_event->ardrone_control_event_end( current_event );
 301
 302                                         /* Make the thread read a new event on the next loop iteration */
 303                                         current_event = NULL;
 304                                 }
 305                                 else
 306                                 {
 307                                         /* Not changing 'current_event' makes the loop process the same
 308                                          * event when the next navdata packet arrives. */
 309                                 }
 310                         }
 311                 }
 312   }// while
 313
 314   /* Stephane : Bug fix - mutexes were previously detroyed by another thread,
 315   which made ardrone_control crash.*/
 316           vp_os_mutex_destroy(&event_queue_mutex);
 317           vp_os_cond_destroy(&control_cond);
 318           vp_os_mutex_destroy(&control_mutex);
 319
 320   vp_com_close(COM_CONTROL(), &control_socket);
 321
 322   THREAD_RETURN( res );
 323 }