vcs.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/Examples/Multiplatform/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 static inline uint8_t* navdata_unpack_option( uint8_t* navdata_ptr, uint8_t* data, uint32_t size )
 160 {
 161     memcpy(data, navdata_ptr, size);
 162
 163     return (navdata_ptr + size);
 164 }
 165
 166 static inline navdata_option_t* navdata_next_option( navdata_option_t* navdata_options_ptr )
 167 {
 168     uint8_t* ptr;
 169
 170     ptr  = (uint8_t*) navdata_options_ptr;
 171     ptr += navdata_options_ptr->size;
 172
 173     return (navdata_option_t*) ptr;
 174 }
 175
 176 navdata_option_t* navdata_search_option( navdata_option_t* navdata_options_ptr, uint32_t tag );
 177
 178
 179 static inline uint32_t navdata_compute_cks( uint8_t* nv, int32_t size )
 180 {
 181     int32_t i;
 182     uint32_t cks;
 183     uint32_t temp;
 184
 185     cks = 0;
 186
 187     for( i = 0; i < size; i++ )
 188         {
 189             temp = nv[i];
 190             cks += temp;
 191         }
 192
 193     return cks;
 194 }
 195
 196 #define navdata_unpack( navdata_ptr, option ) (navdata_option_t*) navdata_unpack_option( (uint8_t*) navdata_ptr, \
 197                                                                                          (uint8_t*) &option, \
 198                                                                                          navdata_ptr->size )
 199
 200 void mykonos_navdata_unpack_all(navdata_unpacked_t* navdata_unpacked, navdata_t* navdata, uint32_t* cks)
 201 {
 202     navdata_cks_t navdata_cks = { 0 };
 203     navdata_option_t* navdata_option_ptr;
 204
 205     navdata_option_ptr = (navdata_option_t*) &navdata->options[0];
 206
 207     memset( navdata_unpacked, 0, sizeof(*navdata_unpacked) );
 208
 209     navdata_unpacked->mykonos_state   = navdata->mykonos_state;
 210     navdata_unpacked->vision_defined  = navdata->vision_defined;
 211
 212     while( navdata_option_ptr != NULL )
 213         {
 214             // Check if we have a valid option
 215             if( navdata_option_ptr->size == 0 )
 216                 {
 217                     INFO ("One option is not a valid because its size is zero\n");
 218                     navdata_option_ptr = NULL;
 219                 }
 220             else
 221                 {
 222                     switch( navdata_option_ptr->tag )
 223                         {
 224                         case NAVDATA_DEMO_TAG:
 225                             //INFO ("Demo tag\n");
 226                             navdata_option_ptr = navdata_unpack( navdata_option_ptr, navdata_unpacked->navdata_demo );
 227                             break;
 228
 229                         case NAVDATA_IPHONE_ANGLES_TAG:
 230                             navdata_option_ptr = navdata_unpack( navdata_option_ptr, navdata_unpacked->navdata_iphone_angles );
 231                             break;
 232
 233                         case NAVDATA_VISION_DETECT_TAG:
 234                             navdata_option_ptr = navdata_unpack( navdata_option_ptr, navdata_unpacked->navdata_vision_detect );
 235                             break;
 236
 237                         case NAVDATA_CKS_TAG:
 238                             navdata_option_ptr = navdata_unpack( navdata_option_ptr, navdata_cks );
 239                             *cks = navdata_cks.cks;
 240                             navdata_option_ptr = NULL; // End of structure
 241                             break;
 242
 243                         default:
 244                             INFO ("Tag %d is not a valid navdata option tag\n", (int) navdata_option_ptr->tag);
 245                             navdata_option_ptr = NULL;
 246                             break;
 247                         }
 248                 }
 249         }
 250 }
 251
 252 static int navdata_udp_socket  = -1;
 253 void navdata_write (int8_t *buffer, int32_t len)
 254 {
 255     struct sockaddr_in to;
 256     int32_t flags;
 257
 258     if (navdata_udp_socket < 0)
 259         {
 260             /// Open udp socket to broadcast at commands to other mykonos
 261             struct sockaddr_in navdata_udp_addr;
 262
 263             memset( (char*)&navdata_udp_addr, 0, sizeof(navdata_udp_addr) );
 264
 265             navdata_udp_addr.sin_family      = AF_INET;
 266             navdata_udp_addr.sin_addr.s_addr = INADDR_ANY;
 267             navdata_udp_addr.sin_port        = htons(NAVDATA_PORT + 100);
 268
 269             navdata_udp_socket = socket( AF_INET, SOCK_DGRAM, 0 );
 270
 271             if( navdata_udp_socket >= 0 )
 272                 {
 273                     flags = fcntl(navdata_udp_socket, F_GETFL, 0);
 274                     if( flags >= 0 )
 275                         {
 276                             flags |= O_NONBLOCK;
 277
 278                             flags = fcntl(navdata_udp_socket, F_SETFL, flags );
 279                         }
 280                     else
 281                         {
 282                             INFO("Get Socket Options failed\n");
 283                         }
 284
 285                     if (bind(navdata_udp_socket, (struct sockaddr*)&navdata_udp_addr, sizeof(struct sockaddr)) < 0) {
 286                         INFO ("navdata_write:bind: %s\n", strerror(errno));
 287                     }
 288                 }
 289         }
 290     if( navdata_udp_socket >= 0 ) {
 291         int res;
 292
 293         memset( (char*)&to, 0, sizeof(to) );
 294         to.sin_family       = AF_INET;
 295         to.sin_addr.s_addr  = inet_addr(WIFI_MYKONOS_IP); // BROADCAST address for subnet 192.168.1.xxx
 296         to.sin_port         = htons( NAVDATA_PORT );
 297
 298         res = sendto( navdata_udp_socket, (char*)buffer, len, 0, (struct sockaddr*)&to, sizeof(to) );
 299     }
 300 }
 301
 302 void navdata_open_server (void)
 303 {
 304     int32_t one = 1;
 305     navdata_write ((int8_t*)&one, sizeof( one ));
 306 }
 307
 308 static inline int get_mask_from_state( uint32_t state, uint32_t mask )
 309 {
 310     return state & mask ? TRUE : FALSE;
 311 }
 312
 313 DEFINE_THREAD_ROUTINE( navdata_loop, data )
 314 {
 315         uint8_t msg[NAVDATA_BUFFER_SIZE];
 316     navdata_unpacked_t navdata_unpacked;
 317     unsigned int cks, navdata_cks, sequence = NAVDATA_SEQUENCE_DEFAULT-1;
 318     int sockfd = -1, addr_in_size;
 319         struct sockaddr_in *my_addr, *from;
 320
 321         INFO("NAVDATA thread starting (thread=%d)...\n", (int)pthread_self());
 322
 323     navdata_open_server();
 324
 325         addr_in_size = sizeof(struct sockaddr_in);
 326
 327     navdata_t* navdata = (navdata_t*) &msg[0];
 328
 329         from = (struct sockaddr_in *)malloc(addr_in_size);
 330         my_addr = (struct sockaddr_in *)malloc(addr_in_size);
 331     assert(from);
 332     assert(my_addr);
 333
 334         memset((char *)my_addr,(char)0,addr_in_size);
 335         my_addr->sin_family = AF_INET;
 336         my_addr->sin_addr.s_addr = htonl(INADDR_ANY);
 337         my_addr->sin_port = htons( NAVDATA_PORT );
 338
 339         if((sockfd = socket (AF_INET, SOCK_DGRAM, 0)) < 0){
 340         INFO ("socket: %s\n", strerror(errno));
 341         goto fail;
 342         };
 343
 344         if(bind(sockfd, (struct sockaddr *)my_addr, addr_in_size) < 0){
 345         INFO ("bind: %s\n", strerror(errno));
 346         goto fail;
 347         };
 348
 349         {
 350                 struct timeval tv;
 351                 // 1 second timeout
 352                 tv.tv_sec   = 1;
 353                 tv.tv_usec  = 0;
 354                 setsockopt( sockfd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv));
 355         }
 356
 357         INFO("Ready to receive\n");
 358
 359         while (gAppAlive) {
 360                 int size;
 361                 size = recvfrom (sockfd, &msg[0], NAVDATA_BUFFER_SIZE, 0, (struct sockaddr *)from,
 362                          (socklen_t *)&addr_in_size);
 363
 364                 if( size == 0 )
 365             {
 366                 INFO ("Lost connection \n");
 367                 navdata_open_server();
 368                 sequence = NAVDATA_SEQUENCE_DEFAULT-1;
 369             }
 370                 if( navdata->header == NAVDATA_HEADER )
 371             {
 372                 mykonos_state = navdata->mykonos_state;
 373
 374                 if( get_mask_from_state(navdata->mykonos_state, MYKONOS_COM_WATCHDOG_MASK) )
 375                     {
 376                                                 //XXX
 377                         INFO ("[NAVDATA] Detect com watchdog\n");
 378                         sequence = NAVDATA_SEQUENCE_DEFAULT-1;
 379
 380                         if( get_mask_from_state(navdata->mykonos_state, MYKONOS_NAVDATA_BOOTSTRAP) == FALSE )
 381                             {
 382                                 const char cmds[] = "AT*COMWDG\r";
 383 #warning Ivan: locking missing here ?
 384                                 at_write ((int8_t*)cmds, strlen( cmds ));
 385                             }
 386                     }
 387
 388                 if( navdata->sequence > sequence )
 389                     {
 390                         if ( get_mask_from_state( mykonos_state, MYKONOS_NAVDATA_DEMO_MASK ))
 391                             {
 392                                 mykonos_navdata_unpack_all(&navdata_unpacked, navdata, &navdata_cks);
 393                                 cks = navdata_compute_cks( &msg[0], size - sizeof(navdata_cks_t) );
 394
 395                                 if( cks == navdata_cks )
 396                                     {
 397                                         //INFO ("Unpack navdata\n");
 398                                     }
 399                             }
 400                     }
 401                 else
 402                     {
 403                         INFO ("[Navdata] Sequence pb : %d (distant) / %d (local)\n", navdata->sequence, sequence);
 404                     }
 405
 406                 sequence = navdata->sequence;
 407             }
 408         }
 409  fail:
 410     free(from);
 411     free(my_addr);
 412
 413     if (sockfd >= 0){
 414         close(sockfd);
 415     }
 416
 417     if (navdata_udp_socket >= 0){
 418         close(navdata_udp_socket);
 419         navdata_udp_socket = -1;
 420     }
 421
 422         INFO("NAVDATA thread stopping\n");
 423     return NULL;
 424 }