git.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/Examples/Multiplatform/Protocol/stream.c

   1 /**
   2  * @file my_client.c
   3  * @author karl leplat
   4  * @date 2009/07/01
   5  */
   6 #include <assert.h>
   7 #include <sys/types.h>
   8 #include <sys/socket.h>
   9 #include <stdio.h>
  10 #include <netinet/in.h>
  11 #include <netdb.h>
  12 #include <errno.h>
  13 #include <arpa/inet.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 "vlib.h"
  28 #include "app.h"
  29
  30 /* Video constant */
  31 #define VIDEO_PORT                5555
  32 #define RAW_CAPTURE_PORT          5557
  33
  34 #define H_ACQ_WIDTH               VIDEO_WIDTH
  35 #define H_ACQ_HEIGHT              VIDEO_HEIGHT
  36 #define VIDEO_BUFFER_SIZE         (2*(VIDEO_WIDTH)*(VIDEO_HEIGHT))
  37
  38 #define DATA_RECEIVED_MAX_SIZE    8192
  39
  40 static video_controller_t controller;
  41 static vp_api_picture_t picture;
  42 static int pictureBpp;
  43
  44 // note: allocate twice the size of an image
  45 uint16_t picture_buf[VIDEO_BUFFER_SIZE];
  46 int num_picture_decoded = 0;
  47
  48 void stream_init(void)
  49 {
  50         C_RESULT status;
  51
  52         memset(&controller, 0, sizeof(controller));
  53         memset(&picture, 0, sizeof(picture));
  54         memset(picture_buf, 0, sizeof(picture_buf));
  55     pictureBpp                    = 2;
  56
  57     /// Picture configuration
  58     picture.format            = PIX_FMT_RGB565;
  59         picture.width         = H_ACQ_WIDTH;
  60     picture.height        = H_ACQ_HEIGHT;
  61     picture.framerate     = 15;
  62     picture.y_line_size   = picture.width * pictureBpp;
  63     picture.cb_line_size  = 0;
  64     picture.cr_line_size  = 0;
  65     picture.y_buf         = (uint8_t *)picture_buf;
  66     picture.cb_buf            = NULL;
  67     picture.cr_buf            = NULL;
  68
  69         status = video_codec_open( &controller, UVLC_CODEC );
  70         if (status) {
  71                 INFO("video_codec_open() failed\n");
  72         }
  73         video_controller_set_motion_estimation(&controller, FALSE);
  74         video_controller_set_format( &controller, H_ACQ_WIDTH, H_ACQ_HEIGHT );
  75 }
  76
  77 void stream_deinit(void)
  78 {
  79         video_codec_close(&controller);
  80 }
  81
  82 static int video_udp_socket  = -1;
  83
  84 void video_write (int8_t *buffer, int32_t len)
  85 {
  86   struct sockaddr_in to;
  87   int32_t flags;
  88
  89   if( video_udp_socket < 0 )
  90   {
  91     struct sockaddr_in video_udp_addr;
  92
  93     memset( (char*)&video_udp_addr, 0, sizeof(video_udp_addr) );
  94
  95     video_udp_addr.sin_family      = AF_INET;
  96     video_udp_addr.sin_addr.s_addr = INADDR_ANY;
  97     video_udp_addr.sin_port        = htons( VIDEO_PORT + 100 );
  98
  99     video_udp_socket = socket( AF_INET, SOCK_DGRAM, 0 );
 100
 101     if( video_udp_socket > 0 )
 102     {
 103                  flags = fcntl(video_udp_socket, F_GETFL, 0);
 104                  if( flags >= 0 )
 105                  {
 106                          flags |= O_NONBLOCK;
 107
 108                          flags = fcntl(video_udp_socket, F_SETFL, flags );
 109                  }
 110                  else
 111                  {
 112                          INFO("Get Socket Options failed\n");
 113                  }
 114
 115        bind(video_udp_socket, (struct sockaddr*)&video_udp_addr, sizeof(struct sockaddr) );
 116     }
 117   }
 118
 119   if( video_udp_socket > 0 )
 120   {
 121      int res;
 122
 123           memset( (char*)&to, 0, sizeof(to) );
 124           to.sin_family       = AF_INET;
 125           to.sin_addr.s_addr  = inet_addr(WIFI_MYKONOS_IP); // BROADCAST address for subnet 192.168.1.xxx
 126           to.sin_port         = htons (VIDEO_PORT);
 127
 128           res = sendto( video_udp_socket, (char*)buffer, len, 0, (struct sockaddr*)&to, sizeof(to) );
 129   }
 130 }
 131
 132 DEFINE_THREAD_ROUTINE( stream_loop, data )
 133 {
 134         C_RESULT status;
 135     int sockfd, addr_in_size;
 136         struct sockaddr_in *my_addr, *from;
 137
 138         INFO("VIDEO stream thread starting (thread=%d)...\n", (int)pthread_self());
 139
 140         stream_init();
 141
 142         addr_in_size = sizeof(struct sockaddr_in);
 143
 144         from = (struct sockaddr_in *)malloc(addr_in_size);
 145         my_addr = (struct sockaddr_in *)malloc(addr_in_size);
 146     assert(from);
 147     assert(my_addr);
 148
 149         memset((char *)my_addr,(char)0,addr_in_size);
 150         my_addr->sin_family = AF_INET;
 151         my_addr->sin_addr.s_addr = htonl(INADDR_ANY);
 152         my_addr->sin_port = htons( VIDEO_PORT );
 153
 154         if((sockfd = socket (AF_INET, SOCK_DGRAM, 0)) < 0){
 155         INFO ("socket: %s\n", strerror(errno));
 156         goto fail;
 157         };
 158
 159         if (bind(sockfd, (struct sockaddr *)my_addr, addr_in_size) < 0){
 160         INFO ("bind: %s\n", strerror(errno));
 161         goto fail;
 162         };
 163
 164         {
 165                 struct timeval tv;
 166                 // 1 second timeout
 167                 tv.tv_sec   = 1;
 168                 tv.tv_usec  = 0;
 169                 setsockopt( sockfd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv));
 170         }
 171
 172         INFO("Ready to receive video data...\n");
 173         {
 174                 int32_t one = 1;
 175                 video_write ((int8_t*)&one, sizeof( one ));
 176         }
 177
 178         while(gAppAlive)
 179         {
 180                 int size;
 181                 size = recvfrom (sockfd,
 182                          video_controller_get_stream_ptr(&controller),
 183                          DATA_RECEIVED_MAX_SIZE, 0, (struct sockaddr *)from,
 184                         (socklen_t*)&addr_in_size);
 185
 186                 if (size == 0)
 187                 {
 188                         INFO ("Lost connection \n");
 189                         int32_t one = 1;
 190                         video_write ((int8_t*)&one, sizeof(one));
 191                 }
 192                 else if (size > 0) {
 193                         int decodeOK = FALSE;
 194
 195                         controller.in_stream.used       = size;
 196                         controller.in_stream.size       = size;
 197                         controller.in_stream.index      = 0;
 198                         controller.in_stream.length     = 32;
 199                         controller.in_stream.code       = 0;
 200
 201                         //INFO("decode_blockline(size=%d)\n", size);
 202             status = video_decode_blockline(&controller, &picture, &decodeOK);
 203                         if (status) {
 204                                 INFO("video_decode_blockline() failed\n");
 205                         }
 206                         else if (decodeOK) {
 207                                 // this is ugly: update counter and notify video renderer
 208                                 num_picture_decoded = controller.num_frames;
 209                                 //INFO("num_picture_decoded = %d\n", num_picture_decoded);
 210                         }
 211                 }
 212         }
 213  fail:
 214     free(from);
 215     free(my_addr);
 216     if (sockfd >= 0){
 217         close(sockfd);
 218         sockfd = -1;
 219     }
 220
 221         INFO("VIDEO stream thread stopping\n");
 222         stream_deinit();
 223     return NULL;
 224 }