git.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/Examples/Linux/Protocol/navdata.c

   1 /**
   2  * @file my_client.c
   3  * @author karl leplat
   4  * @date 2009/07/01
   5  */
   6 #include <sys/types.h>
   7 #include <sys/socket.h>
   8 #include <stdio.h>
   9 #include <netinet/in.h>
  10 #include <netdb.h>
  11 #include <errno.h>
  12 #include <arpa/inet.h>
  13 #include <assert.h>
  14
  15 #include <stdio.h>
  16 #include <stdlib.h>
  17 #include <unistd.h>
  18 #include <string.h>
  19 #include <errno.h>
  20
  21 #include <sys/time.h>
  22
  23 #include <fcntl.h>
  24 #include <sys/ioctl.h>
  25 #include <net/if.h>
  26
  27 #include "app.h"
  28
  29 /* Navdata constant */
  30 #define NAVDATA_SEQUENCE_DEFAULT  1
  31 #define NAVDATA_PORT              5554
  32 #define NAVDATA_HEADER            0x55667788
  33 #define NAVDATA_BUFFER_SIZE       2048
  34
  35 typedef enum _navdata_tag_t {
  36     NAVDATA_DEMO_TAG = 0,
  37     NAVDATA_VISION_DETECT_TAG = 16,
  38     NAVDATA_IPHONE_ANGLES_TAG = 18,
  39     NAVDATA_CKS_TAG = 0xFFFF
  40 } navdata_tag_t;
  41
  42 typedef struct _matrix33_t
  43 {
  44     float32_t m11;
  45     float32_t m12;
  46     float32_t m13;
  47     float32_t m21;
  48     float32_t m22;
  49     float32_t m23;
  50     float32_t m31;
  51     float32_t m32;
  52     float32_t m33;
  53 } matrix33_t;
  54
  55 typedef struct _vector31_t {
  56     union {
  57         float32_t v[3];
  58         struct
  59         {
  60             float32_t x;
  61             float32_t y;
  62             float32_t z;
  63         };
  64     };
  65 } vector31_t;
  66
  67 typedef struct _navdata_option_t {
  68     uint16_t  tag;
  69     uint16_t  size;
  70
  71     uint8_t   data[];
  72 } navdata_option_t;
  73 typedef struct _navdata_t {
  74     uint32_t    header;
  75     uint32_t    mykonos_state;
  76     uint32_t    sequence;
  77     int      vision_defined;
  78
  79     navdata_option_t  options[1];
  80 } __attribute__ ((packed)) navdata_t;
  81
  82 typedef struct _navdata_cks_t {
  83     uint16_t  tag;
  84     uint16_t  size;
  85
  86     // Checksum for all navdatas (including options)
  87     uint32_t  cks;
  88 } __attribute__ ((packed)) navdata_cks_t;
  89
  90 typedef struct _navdata_demo_t {
  91     uint16_t    tag;
  92     uint16_t    size;
  93
  94     uint32_t    ctrl_state;             /*!< instance of #def_mykonos_state_mask_t */
  95     uint32_t    vbat_flying_percentage; /*!< battery voltage filtered (mV) */
  96
  97     float32_t   theta;                  /*!< UAV's attitude */
  98     float32_t   phi;                    /*!< UAV's attitude */
  99     float32_t   psi;                    /*!< UAV's attitude */
 100
 101     int32_t     altitude;               /*!< UAV's altitude */
 102
 103     float32_t   vx;                     /*!< UAV's estimated linear velocity */
 104     float32_t   vy;                     /*!< UAV's estimated linear velocity */
 105     float32_t   vz;                     /*!< UAV's estimated linear velocity */
 106
 107     uint32_t    num_frames;                       /*!< streamed frame index */
 108
 109     // Camera parameters compute by detection
 110     matrix33_t  detection_camera_rot;
 111     matrix33_t  detection_camera_homo;
 112     vector31_t  detection_camera_trans;
 113
 114     // Camera parameters compute by drone
 115     matrix33_t  drone_camera_rot;
 116     vector31_t  drone_camera_trans;
 117 } __attribute__ ((packed)) navdata_demo_t;
 118
 119 typedef struct _navdata_iphone_angles_t {
 120     uint16_t   tag;
 121     uint16_t   size;
 122
 123     int32_t    enable;
 124     float32_t  ax;
 125     float32_t  ay;
 126     float32_t  az;
 127     uint32_t   elapsed;
 128 } __attribute__ ((packed)) navdata_iphone_angles_t;
 129
 130 typedef struct _navdata_time_t {
 131     uint16_t  tag;
 132     uint16_t  size;
 133
 134     uint32_t  time;
 135 } __attribute__ ((packed)) navdata_time_t;
 136
 137 typedef struct _navdata_vision_detect_t {
 138     uint16_t   tag;
 139     uint16_t   size;
 140
 141     uint32_t   nb_detected;
 142     uint32_t   type[4];
 143     uint32_t   xc[4];
 144     uint32_t   yc[4];
 145     uint32_t   width[4];
 146     uint32_t   height[4];
 147     uint32_t   dist[4];
 148 } __attribute__ ((packed)) navdata_vision_detect_t;
 149
 150 typedef struct _navdata_unpacked_t {
 151     uint32_t  mykonos_state;
 152     int    vision_defined;
 153
 154     navdata_demo_t           navdata_demo;
 155     navdata_iphone_angles_t  navdata_iphone_angles;
 156     navdata_vision_detect_t  navdata_vision_detect;
 157 } navdata_unpacked_t;
 158
 159 /* Private variables */
 160 static pthread_t nav_thread = 0;
 161 static int32_t nav_thread_alive = 1;
 162
 163 static void navdata_open_server (void);
 164 static void navdata_write (int8_t *buffer, int32_t len);
 165 static void mykonos_navdata_unpack_all(navdata_unpacked_t* navdata_unpacked, navdata_t* navdata, uint32_t* cks);
 166 static void* navdata_loop(void *arg);
 167
 168 static inline int get_mask_from_state( uint32_t state, uint32_t mask )
 169 {
 170     return state & mask ? TRUE : FALSE;
 171 }
 172
 173 static inline uint8_t* navdata_unpack_option( uint8_t* navdata_ptr, uint8_t* data, uint32_t size )
 174 {
 175     memcpy(data, navdata_ptr, size);
 176
 177     return (navdata_ptr + size);
 178 }
 179
 180
 181 static inline navdata_option_t* navdata_next_option( navdata_option_t* navdata_options_ptr )
 182 {
 183     uint8_t* ptr;
 184
 185     ptr  = (uint8_t*) navdata_options_ptr;
 186     ptr += navdata_options_ptr->size;
 187
 188     return (navdata_option_t*) ptr;
 189 }
 190
 191 navdata_option_t* navdata_search_option( navdata_option_t* navdata_options_ptr, uint32_t tag );
 192
 193
 194 static inline uint32_t navdata_compute_cks( uint8_t* nv, int32_t size )
 195 {
 196     int32_t i;
 197     uint32_t cks;
 198     uint32_t temp;
 199
 200     cks = 0;
 201
 202     for( i = 0; i < size; i++ )
 203         {
 204             temp = nv[i];
 205             cks += temp;
 206         }
 207
 208     return cks;
 209 }
 210
 211 #define navdata_unpack( navdata_ptr, option ) (navdata_option_t*) navdata_unpack_option( (uint8_t*) navdata_ptr, \
 212                                                                                          (uint8_t*) &option, \
 213                                                                                          navdata_ptr->size )
 214 static void mykonos_navdata_unpack_all(navdata_unpacked_t* navdata_unpacked, navdata_t* navdata, uint32_t* cks)
 215 {
 216     navdata_cks_t navdata_cks = { 0 };
 217     navdata_option_t* navdata_option_ptr;
 218
 219     navdata_option_ptr = (navdata_option_t*) &navdata->options[0];
 220
 221     memset( navdata_unpacked, 0, sizeof(*navdata_unpacked) );
 222
 223     navdata_unpacked->mykonos_state   = navdata->mykonos_state;
 224     navdata_unpacked->vision_defined  = navdata->vision_defined;
 225
 226     while( navdata_option_ptr != NULL )
 227         {
 228             // Check if we have a valid option
 229             if( navdata_option_ptr->size == 0 )
 230                 {
 231                     INFO ("One option is not a valid because its size is zero\n");
 232                     navdata_option_ptr = NULL;
 233                 }
 234             else
 235                 {
 236                     switch( navdata_option_ptr->tag )
 237                         {
 238                         case NAVDATA_DEMO_TAG:
 239                             //INFO ("Demo tag\n");
 240                             navdata_option_ptr = navdata_unpack( navdata_option_ptr, navdata_unpacked->navdata_demo );
 241                             break;
 242
 243                         case NAVDATA_IPHONE_ANGLES_TAG:
 244                             navdata_option_ptr = navdata_unpack( navdata_option_ptr, navdata_unpacked->navdata_iphone_angles );
 245                             break;
 246
 247                         case NAVDATA_VISION_DETECT_TAG:
 248                             navdata_option_ptr = navdata_unpack( navdata_option_ptr, navdata_unpacked->navdata_vision_detect );
 249                             break;
 250
 251                         case NAVDATA_CKS_TAG:
 252                             navdata_option_ptr = navdata_unpack( navdata_option_ptr, navdata_cks );
 253                             *cks = navdata_cks.cks;
 254                             navdata_option_ptr = NULL; // End of structure
 255                             break;
 256
 257                         default:
 258                             INFO ("Tag %d is not a valid navdata option tag\n", (int) navdata_option_ptr->tag);
 259                             navdata_option_ptr = NULL;
 260                             break;
 261                         }
 262                 }
 263         }
 264 }
 265
 266 static int navdata_udp_socket  = -1;
 267 static void navdata_write (int8_t *buffer, int32_t len)
 268 {
 269     struct sockaddr_in to;
 270     int32_t flags;
 271
 272     if (navdata_udp_socket < 0)
 273         {
 274             /// Open udp socket to broadcast at commands to other mykonos
 275             struct sockaddr_in navdata_udp_addr;
 276
 277             memset( (char*)&navdata_udp_addr, 0, sizeof(navdata_udp_addr) );
 278
 279             navdata_udp_addr.sin_family      = AF_INET;
 280             navdata_udp_addr.sin_addr.s_addr = INADDR_ANY;
 281             navdata_udp_addr.sin_port        = htons(NAVDATA_PORT + 100);
 282
 283             navdata_udp_socket = socket( AF_INET, SOCK_DGRAM, 0 );
 284
 285             if( navdata_udp_socket >= 0 )
 286                 {
 287                     flags = fcntl(navdata_udp_socket, F_GETFL, 0);
 288                     if( flags >= 0 )
 289                         {
 290                             flags |= O_NONBLOCK;
 291
 292                             flags = fcntl(navdata_udp_socket, F_SETFL, flags );
 293                         }
 294                     else
 295                         {
 296                             INFO("Get Socket Options failed\n");
 297                         }
 298
 299                     if (bind(navdata_udp_socket, (struct sockaddr*)&navdata_udp_addr, sizeof(struct sockaddr)) < 0) {
 300                         INFO ("navdata_write:bind: %s\n", strerror(errno));
 301                     }
 302                 }
 303         }
 304     if( navdata_udp_socket >= 0 ) {
 305         int res;
 306
 307         memset( (char*)&to, 0, sizeof(to) );
 308         to.sin_family       = AF_INET;
 309         to.sin_addr.s_addr  = inet_addr(WIFI_MYKONOS_IP); // BROADCAST address for subnet 192.168.1.xxx
 310         to.sin_port         = htons( NAVDATA_PORT );
 311
 312         res = sendto( navdata_udp_socket, (char*)buffer, len, 0, (struct sockaddr*)&to, sizeof(to) );
 313     }
 314 }
 315
 316 static void navdata_open_server (void)
 317 {
 318     int32_t one = 1;
 319     navdata_write ((int8_t*)&one, sizeof( one ));
 320 }
 321
 322 static void* navdata_loop(void *arg)
 323 {
 324         uint8_t msg[NAVDATA_BUFFER_SIZE];
 325     navdata_unpacked_t navdata_unpacked;
 326     unsigned int cks, navdata_cks, sequence = NAVDATA_SEQUENCE_DEFAULT-1;
 327     int sockfd = -1, addr_in_size;
 328         struct sockaddr_in *my_addr, *from;
 329
 330         INFO("NAVDATA thread starting (thread=%d)...\n", (int)pthread_self());
 331
 332     navdata_open_server();
 333
 334         addr_in_size = sizeof(struct sockaddr_in);
 335
 336     navdata_t* navdata = (navdata_t*) &msg[0];
 337
 338         from = (struct sockaddr_in *)malloc(addr_in_size);
 339         my_addr = (struct sockaddr_in *)malloc(addr_in_size);
 340     assert(from);
 341     assert(my_addr);
 342
 343         memset((char *)my_addr,(char)0,addr_in_size);
 344         my_addr->sin_family = AF_INET;
 345         my_addr->sin_addr.s_addr = htonl(INADDR_ANY);
 346         my_addr->sin_port = htons( NAVDATA_PORT );
 347
 348         if((sockfd = socket (AF_INET, SOCK_DGRAM, 0)) < 0){
 349         INFO ("socket: %s\n", strerror(errno));
 350         goto fail;
 351         };
 352
 353         if(bind(sockfd, (struct sockaddr *)my_addr, addr_in_size) < 0){
 354         INFO ("bind: %s\n", strerror(errno));
 355         goto fail;
 356         };
 357
 358         {
 359                 struct timeval tv;
 360                 // 1 second timeout
 361                 tv.tv_sec   = 1;
 362                 tv.tv_usec  = 0;
 363                 setsockopt( sockfd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv));
 364         }
 365
 366         INFO("Ready to receive\n");
 367
 368         while ( nav_thread_alive ) {
 369                 int size;
 370                 size = recvfrom (sockfd, &msg[0], NAVDATA_BUFFER_SIZE, 0, (struct sockaddr *)from,
 371                          (socklen_t *)&addr_in_size);
 372
 373                 if( size == 0 )
 374             {
 375                 INFO ("Lost connection \n");
 376                 navdata_open_server();
 377                 sequence = NAVDATA_SEQUENCE_DEFAULT-1;
 378             }
 379                 if( navdata->header == NAVDATA_HEADER )
 380             {
 381                 mykonos_state = navdata->mykonos_state;
 382
 383                 if( get_mask_from_state(navdata->mykonos_state, MYKONOS_COM_WATCHDOG_MASK) )
 384                     {
 385                         INFO ("[NAVDATA] Detect com watchdog\n");
 386                         sequence = NAVDATA_SEQUENCE_DEFAULT-1;
 387
 388                         if( get_mask_from_state(navdata->mykonos_state, MYKONOS_NAVDATA_BOOTSTRAP) == FALSE )
 389                             {
 390                                 const char cmds[] = "AT*COMWDG\r";
 391                                 at_write ((int8_t*)cmds, strlen( cmds ));
 392                             }
 393                     }
 394
 395                 if( navdata->sequence > sequence )
 396                     {
 397                         if ( get_mask_from_state( mykonos_state, MYKONOS_NAVDATA_DEMO_MASK ))
 398                             {
 399                                 mykonos_navdata_unpack_all(&navdata_unpacked, navdata, &navdata_cks);
 400                                 cks = navdata_compute_cks( &msg[0], size - sizeof(navdata_cks_t) );
 401
 402                                 if( cks == navdata_cks )
 403                                     {
 404                                         //INFO ("Unpack navdata\n");
 405                                     }
 406                             }
 407                     }
 408                 else
 409                     {
 410                         INFO ("[Navdata] Sequence pb : %d (distant) / %d (local)\n", navdata->sequence, sequence);
 411                     }
 412
 413                 sequence = navdata->sequence;
 414             }
 415         }
 416  fail:
 417     free(from);
 418     free(my_addr);
 419
 420     if (sockfd >= 0){
 421         close(sockfd);
 422     }
 423
 424     if (navdata_udp_socket >= 0){
 425         close(navdata_udp_socket);
 426         navdata_udp_socket = -1;
 427     }
 428
 429         INFO("NAVDATA thread stopping\n");
 430     return NULL;
 431 }
 432
 433 /* Public functions */
 434 void navdata_stop( void )
 435 {
 436         if ( !nav_thread )
 437                 return;
 438
 439    nav_thread_alive = 0;
 440         pthread_join(nav_thread, NULL);
 441         nav_thread = 0;
 442
 443         if (navdata_udp_socket >= 0){
 444                 close(navdata_udp_socket);
 445                 navdata_udp_socket = -1;
 446         }
 447 }
 448
 449 void navdata_run( void )
 450 {
 451    if ( nav_thread )
 452       return;
 453
 454    nav_thread_alive = 1;
 455         if ( pthread_create( &nav_thread, NULL, navdata_loop, NULL ) )
 456    {
 457                 INFO("pthread_create: %s\n", strerror(errno));
 458         }
 459 }
 460