Paparazzi UAS  v5.8.2_stable-0-g6260b7c
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
rtp.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2014 The Paparazzi Community
3  *
4  * This file is part of Paparazzi.
5  *
6  * Paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * Paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, see
18  * <http://www.gnu.org/licenses/>.
19  *
20  */
21 
28 #include <stdint.h>
29 #include <string.h>
30 #include <sys/time.h>
31 
32 #include "rtp.h"
33 
34 static void rtp_packet_send(struct UdpSocket *udp, uint8_t *Jpeg, int JpegLen, uint32_t m_SequenceNumber,
35  uint32_t m_Timestamp, uint32_t m_offset, uint8_t marker_bit, int w, int h, uint8_t format_code, uint8_t quality_code,
36  uint8_t has_dri_header);
37 
38 // http://www.ietf.org/rfc/rfc3550.txt
39 
40 #define KJpegCh1ScanDataLen 32
41 #define KJpegCh2ScanDataLen 56
42 
43 // RGB JPEG images as RTP payload - 64x48 pixel
45  0xf8, 0xbe, 0x8a, 0x28, 0xaf, 0xe5, 0x33, 0xfd,
46  0xfc, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
47  0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
48  0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
49  0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
50  0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
51  0x80, 0x0a, 0x28, 0xa2, 0x80, 0x3f, 0xff, 0xd9
52 };
54  0xf5, 0x8a, 0x28, 0xa2, 0xbf, 0xca, 0xf3, 0xfc,
55  0x53, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
56  0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
57  0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
58  0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
59  0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
60  0x80, 0x0a, 0x28, 0xa2, 0x80, 0x3f, 0xff, 0xd9
61 };
62 
67 void rtp_frame_test(struct UdpSocket *udp)
68 {
69  static uint32_t framecounter = 0;
70  static uint32_t timecounter = 0;
71  static uint8_t toggle = 0;
72  toggle = ! toggle;
73 
74  uint8_t format_code = 0x01;
75  uint8_t quality_code = 0x54;
76 
77  if (toggle) {
78  rtp_packet_send(udp, JpegScanDataCh2A, KJpegCh2ScanDataLen, framecounter, timecounter, 0, 1, 64, 48, format_code,
79  quality_code, 0);
80  } else {
81  rtp_packet_send(udp, JpegScanDataCh2B, KJpegCh2ScanDataLen, framecounter, timecounter, 0, 1, 64, 48, format_code,
82  quality_code, 0);
83  }
84  framecounter++;
85  timecounter += 3600;
86 }
87 
97 void rtp_frame_send(struct UdpSocket *udp, struct image_t *img, uint8_t format_code,
98  uint8_t quality_code, uint8_t has_dri_header, uint32_t delta_t)
99 {
100  static uint32_t packetcounter = 0;
101  static uint32_t timecounter = 0;
102  uint32_t offset = 0;
103  uint32_t jpeg_size = img->buf_size;
104  uint8_t *jpeg_ptr = img->buf;
105 
106 #define MAX_PACKET_SIZE 1400
107 
108  if (delta_t <= 0) {
109  struct timeval tv;
110  gettimeofday(&tv, 0);
111  uint32_t t = (tv.tv_sec % (256 * 256)) * 90000 + tv.tv_usec * 9 / 100;
112  timecounter = t;
113  }
114 
115  // Split frame into packets
116  for (; jpeg_size > 0;) {
118  uint8_t lastpacket = 0;
119 
120  if (jpeg_size <= len) {
121  lastpacket = 1;
122  len = jpeg_size;
123  }
124 
125  rtp_packet_send(udp, jpeg_ptr, len, packetcounter, timecounter, offset, lastpacket, img->w, img->h, format_code,
126  quality_code, has_dri_header);
127 
128  jpeg_size -= len;
129  jpeg_ptr += len;
130  offset += len;
131  packetcounter++;
132  }
133 
134 
135  if (delta_t > 0) {
136  // timestamp = 1 / 90 000 seconds
137  timecounter += delta_t;
138  }
139 }
140 
141 /*
142  * The RTP timestamp is in units of 90000Hz. The same timestamp MUST
143  appear in each fragment of a given frame. The RTP marker bit MUST be
144  set in the last packet of a frame.
145  * @param[in] *udp The UDP socket to send the RTP packet over
146  * @param[in] *Jpeg JPEG encoded image byte buffer
147  * @param[in] JpegLen The length of the byte buffer
148  * @param[in] m_SequenceNumber RTP sequence number
149  * @param[in] m_Timestamp Timestamp of the image
150  * @param[in] m_offset 3 byte fragmentation offset for fragmented images
151  * @param[in] marker_bit RTP marker bit
152  * @param[in] w The width of the JPEG image
153  * @param[in] h The height of the image
154  * @param[in] format_code 0 for YUV422 and 1 for YUV421
155  * @param[in] quality_code The JPEG encoding quality
156  * @param[in] has_dri_header Whether we have an DRI header or not
157  */
158 static void rtp_packet_send(
159  struct UdpSocket *udp,
160  uint8_t *Jpeg, int JpegLen,
161  uint32_t m_SequenceNumber, uint32_t m_Timestamp,
162  uint32_t m_offset, uint8_t marker_bit,
163  int w, int h,
164  uint8_t format_code, uint8_t quality_code,
165  uint8_t has_dri_header)
166 {
167 
168 #define KRtpHeaderSize 12 // size of the RTP header
169 #define KJpegHeaderSize 8 // size of the special JPEG payload header
170 
171  uint8_t RtpBuf[2048];
172  int RtpPacketSize = JpegLen + KRtpHeaderSize + KJpegHeaderSize;
173 
174  memset(RtpBuf, 0x00, sizeof(RtpBuf));
175 
176  /*
177  The RTP header has the following format:
178 
179  0 1 2 3
180  0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
181  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
182  |V=2|P|X| CC |M| PT | sequence number |
183  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
184  | timestamp |
185  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
186  | synchronization source (SSRC) identifier |
187  +=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+
188  | contributing source (CSRC) identifiers |
189  | .... |
190  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
191  * */
192 
193  // Prepare the 12 byte RTP header
194  RtpBuf[0] = 0x80; // RTP version
195  RtpBuf[1] = 0x1a + (marker_bit << 7); // JPEG payload (26) and marker bit
196  RtpBuf[2] = m_SequenceNumber >> 8;
197  RtpBuf[3] = m_SequenceNumber & 0x0FF; // each packet is counted with a sequence counter
198  RtpBuf[4] = (m_Timestamp & 0xFF000000) >> 24; // each image gets a timestamp
199  RtpBuf[5] = (m_Timestamp & 0x00FF0000) >> 16;
200  RtpBuf[6] = (m_Timestamp & 0x0000FF00) >> 8;
201  RtpBuf[7] = (m_Timestamp & 0x000000FF);
202  RtpBuf[8] = 0x13; // 4 byte SSRC (sychronization source identifier)
203  RtpBuf[9] = 0xf9; // we just an arbitrary number here to keep it simple
204  RtpBuf[10] = 0x7e;
205  RtpBuf[11] = 0x67;
206 
207  /* JPEG header", are as follows:
208  *
209  * http://tools.ietf.org/html/rfc2435
210 
211  0 1 2 3
212  0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
213  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
214  | Type-specific | Fragment Offset |
215  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
216  | Type | Q | Width | Height |
217  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
218  */
219 
220  // Prepare the 8 byte payload JPEG header
221  RtpBuf[12] = 0x00; // type specific
222  RtpBuf[13] = (m_offset & 0x00FF0000) >> 16; // 3 byte fragmentation offset for fragmented images
223  RtpBuf[14] = (m_offset & 0x0000FF00) >> 8;
224  RtpBuf[15] = (m_offset & 0x000000FF);
225  RtpBuf[16] = 0x00; // type: 0 422 or 1 421
226  RtpBuf[17] = 60; // quality scale factor
227  RtpBuf[16] = format_code; // type: 0 422 or 1 421
228  if (has_dri_header) {
229  RtpBuf[16] |= 0x40; // DRI flag
230  }
231  RtpBuf[17] = quality_code; // quality scale factor
232  RtpBuf[18] = w / 8; // width / 8 -> 48 pixel
233  RtpBuf[19] = h / 8; // height / 8 -> 32 pixel
234  // append the JPEG scan data to the RTP buffer
235  memcpy(&RtpBuf[20], Jpeg, JpegLen);
236 
237  udp_socket_send_dontwait(udp, RtpBuf, RtpPacketSize);
238 };
int udp_socket_send_dontwait(struct UdpSocket *sock, uint8_t *buffer, uint32_t len)
Send a packet from buffer, non-blocking.
Definition: udp_socket.c:140
uint32_t buf_size
The buffer size.
Definition: image.h:49
static void rtp_packet_send(struct UdpSocket *udp, uint8_t *Jpeg, int JpegLen, uint32_t m_SequenceNumber, uint32_t m_Timestamp, uint32_t m_offset, uint8_t marker_bit, int w, int h, uint8_t format_code, uint8_t quality_code, uint8_t has_dri_header)
Definition: rtp.c:158
#define KJpegCh2ScanDataLen
Definition: rtp.c:41
Definition: image.h:42
#define MAX_PACKET_SIZE
void rtp_frame_send(struct UdpSocket *udp, struct image_t *img, uint8_t format_code, uint8_t quality_code, uint8_t has_dri_header, uint32_t delta_t)
Send an RTP frame.
Definition: rtp.c:97
uint16_t w
Image width.
Definition: image.h:44
unsigned long uint32_t
Definition: types.h:18
uint16_t h
Image height.
Definition: image.h:45
void * buf
Image buffer (depending on the image_type)
Definition: image.h:50
uint8_t JpegScanDataCh2A[KJpegCh2ScanDataLen]
Definition: rtp.c:44
unsigned char uint8_t
Definition: types.h:14
uint8_t JpegScanDataCh2B[KJpegCh2ScanDataLen]
Definition: rtp.c:53
Encodes a vide stream with RTP (JPEG)
#define KJpegHeaderSize
#define KRtpHeaderSize
void rtp_frame_test(struct UdpSocket *udp)
Send a test RTP frame.
Definition: rtp.c:67