Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
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, uint16_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 /*
39  * RTP Protocol documentation
40  *
41  * Full description:
42  * - http://www.ietf.org/rfc/rfc3550.txt
43  *
44  * Packet content:
45  * - https://tools.ietf.org/html/rfc3550#section-5.1
46  *
47  * Format specific details:
48  * - https://en.wikipedia.org/wiki/RTP_audio_video_profile
49  * - protocol 26 (Jpeg) has a clock rate of 90 000
50  *
51  * The timestamp reflects the sampling instant of the first octet in
52  * the RTP data packet. The sampling instant MUST be derived from a
53  * clock that increments monotonically and linearly in time to allow
54  * synchronization and jitter calculations (see Section 6.4.1). The
55  * resolution of the clock MUST be sufficient for the desired
56  * synchronization accuracy and for measuring packet arrival jitter
57  * (one tick per video frame is typically not sufficient). The clock
58  * frequency is dependent on the format of data carried as payload
59  * and is specified statically in the profile or payload format
60  * specification that defines the format, or MAY be specified
61  * dynamically for payload formats defined through non-RTP means. If
62  * RTP packets are generated periodically, the nominal sampling
63  * instant as determined from the sampling clock is to be used, not a
64  * reading of the system clock.
65  */
66 
67 
68 #define KJpegCh1ScanDataLen 32
69 #define KJpegCh2ScanDataLen 56
70 
71 // RGB JPEG images as RTP payload - 64x48 pixel
73  0xf8, 0xbe, 0x8a, 0x28, 0xaf, 0xe5, 0x33, 0xfd,
74  0xfc, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
75  0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
76  0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
77  0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
78  0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
79  0x80, 0x0a, 0x28, 0xa2, 0x80, 0x3f, 0xff, 0xd9
80 };
82  0xf5, 0x8a, 0x28, 0xa2, 0xbf, 0xca, 0xf3, 0xfc,
83  0x53, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
84  0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
85  0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
86  0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
87  0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
88  0x80, 0x0a, 0x28, 0xa2, 0x80, 0x3f, 0xff, 0xd9
89 };
90 
95 void rtp_frame_test(struct UdpSocket *udp)
96 {
97  static uint32_t framecounter = 0;
98  static uint32_t timecounter = 0;
99  static uint8_t toggle = 0;
100  toggle = ! toggle;
101 
102  uint8_t format_code = 0x01;
103  uint8_t quality_code = 0x54;
104 
105  if (toggle) {
106  rtp_packet_send(udp, JpegScanDataCh2A, KJpegCh2ScanDataLen, framecounter, timecounter, 0, 1, 64, 48, format_code,
107  quality_code, 0);
108  } else {
109  rtp_packet_send(udp, JpegScanDataCh2B, KJpegCh2ScanDataLen, framecounter, timecounter, 0, 1, 64, 48, format_code,
110  quality_code, 0);
111  }
112  framecounter++;
113  timecounter += 3600;
114 }
115 
127 void rtp_frame_send(struct UdpSocket *udp, struct image_t *img, uint8_t format_code,
128  uint8_t quality_code, uint8_t has_dri_header, float average_frame_rate, uint16_t *packet_number, uint32_t *rtp_time_counter)
129 {
130  uint32_t offset = 0;
131  uint32_t jpeg_size = img->buf_size;
132  uint8_t *jpeg_ptr = img->buf;
133 
134  *rtp_time_counter += ((uint32_t) (90000.0f / average_frame_rate));
135 
136 #define MAX_PACKET_SIZE 1400
137 
138  // Split frame into packets
139  for (; jpeg_size > 0;) {
141  uint8_t lastpacket = 0;
142 
143  if (jpeg_size <= len) {
144  lastpacket = 1;
145  len = jpeg_size;
146  }
147 
148  rtp_packet_send(udp, jpeg_ptr, len, *packet_number, *rtp_time_counter, offset, lastpacket, img->w, img->h, format_code,
149  quality_code, has_dri_header);
150 
151  (*packet_number)++;
152  jpeg_size -= len;
153  jpeg_ptr += len;
154  offset += len;
155  }
156 
157 }
158 
159 /*
160  * The same timestamp MUST appear in each fragment of a given frame.
161  * The RTP marker bit MUST be set in the last packet of a frame.
162  * Extra note: When the time difference between frames is non-constant,
163  there seems to introduce some lag or jitter in the video streaming.
164  * @param[in] *udp The UDP socket to send the RTP packet over
165  * @param[in] *Jpeg JPEG encoded image byte buffer
166  * @param[in] JpegLen The length of the byte buffer
167  * @param[in] m_SequenceNumber RTP sequence number
168  * @param[in] m_Timestamp Time counter: RTP requires monolitically lineraly increasing timecount. FMT26 uses 90kHz clock.
169  * @param[in] m_offset 3 byte fragmentation offset for fragmented images
170  * @param[in] marker_bit RTP marker bit: must be set in last packet of a frame.
171  * @param[in] w The width of the JPEG image
172  * @param[in] h The height of the image
173  * @param[in] format_code 0 for YUV422 and 1 for YUV421
174  * @param[in] quality_code The JPEG encoding quality
175  * @param[in] has_dri_header Whether we have an DRI header or not
176  */
177 static void rtp_packet_send(
178  struct UdpSocket *udp,
179  uint8_t *Jpeg, int JpegLen,
180  uint16_t m_SequenceNumber, uint32_t m_Timestamp,
181  uint32_t m_offset, uint8_t marker_bit,
182  int w, int h,
183  uint8_t format_code, uint8_t quality_code,
184  uint8_t has_dri_header)
185 {
186 
187 #define KRtpHeaderSize 12 // size of the RTP header
188 #define KJpegHeaderSize 8 // size of the special JPEG payload header
189 
190  uint8_t RtpBuf[2048];
191  int RtpPacketSize = JpegLen + KRtpHeaderSize + KJpegHeaderSize;
192 
193  memset(RtpBuf, 0x00, sizeof(RtpBuf));
194 
195  /*
196  The RTP header has the following format:
197 
198  0 1 2 3
199  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
200  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
201  |V=2|P|X| CC |M| PT | sequence number |
202  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
203  | timestamp |
204  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
205  | synchronization source (SSRC) identifier |
206  +=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+
207  | contributing source (CSRC) identifiers |
208  | .... |
209  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
210  * */
211 
212  // Prepare the 12 byte RTP header
213  RtpBuf[0] = 0x80; // RTP version
214  RtpBuf[1] = 0x1a + (marker_bit << 7); // JPEG payload (26) and marker bit
215  RtpBuf[2] = m_SequenceNumber >> 8;
216  RtpBuf[3] = m_SequenceNumber & 0x0FF; // each packet is counted with a sequence counter
217  RtpBuf[4] = (m_Timestamp & 0xFF000000) >> 24; // each image gets a timestamp
218  RtpBuf[5] = (m_Timestamp & 0x00FF0000) >> 16;
219  RtpBuf[6] = (m_Timestamp & 0x0000FF00) >> 8;
220  RtpBuf[7] = (m_Timestamp & 0x000000FF);
221  RtpBuf[8] = 0x13; // 4 byte SSRC (sychronization source identifier)
222  RtpBuf[9] = 0xf9; // we just an arbitrary number here to keep it simple
223  RtpBuf[10] = 0x7e;
224  RtpBuf[11] = 0x67;
225 
226  /* JPEG header", are as follows:
227  *
228  * http://tools.ietf.org/html/rfc2435
229 
230  0 1 2 3
231  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
232  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
233  | Type-specific | Fragment Offset |
234  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
235  | Type | Q | Width | Height |
236  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
237  */
238 
239  // Prepare the 8 byte payload JPEG header
240  RtpBuf[12] = 0x00; // type specific
241  RtpBuf[13] = (m_offset & 0x00FF0000) >> 16; // 3 byte fragmentation offset for fragmented images
242  RtpBuf[14] = (m_offset & 0x0000FF00) >> 8;
243  RtpBuf[15] = (m_offset & 0x000000FF);
244  RtpBuf[16] = 0x00; // type: 0 422 or 1 421
245  RtpBuf[17] = 60; // quality scale factor
246  RtpBuf[16] = format_code; // type: 0 422 or 1 421
247  if (has_dri_header) {
248  RtpBuf[16] |= 0x40; // DRI flag
249  }
250  RtpBuf[17] = quality_code; // quality scale factor
251  RtpBuf[18] = w / 8; // width / 8 -> 48 pixel
252  RtpBuf[19] = h / 8; // height / 8 -> 32 pixel
253  // append the JPEG scan data to the RTP buffer
254  memcpy(&RtpBuf[20], Jpeg, JpegLen);
255 
256  udp_socket_send_dontwait(udp, RtpBuf, RtpPacketSize);
257 };
uint16_t
unsigned short uint16_t
Definition: types.h:16
KJpegHeaderSize
#define KJpegHeaderSize
h
static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
Definition: UKF_Wind_Estimator.c:821
uint32_t
unsigned long uint32_t
Definition: types.h:18
rtp_frame_send
void rtp_frame_send(struct UdpSocket *udp, struct image_t *img, uint8_t format_code, uint8_t quality_code, uint8_t has_dri_header, float average_frame_rate, uint16_t *packet_number, uint32_t *rtp_time_counter)
Send an RTP frame.
Definition: rtp.c:127
image_t::w
uint16_t w
Image width.
Definition: image.h:46
udp_socket_send_dontwait
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:141
image_t::buf_size
uint32_t buf_size
The buffer size.
Definition: image.h:53
KJpegCh2ScanDataLen
#define KJpegCh2ScanDataLen
Definition: rtp.c:69
image_t::h
uint16_t h
Image height.
Definition: image.h:47
rtp_packet_send
static void rtp_packet_send(struct UdpSocket *udp, uint8_t *Jpeg, int JpegLen, uint16_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:177
MAX_PACKET_SIZE
#define MAX_PACKET_SIZE
uint8_t
unsigned char uint8_t
Definition: types.h:14
JpegScanDataCh2A
uint8_t JpegScanDataCh2A[KJpegCh2ScanDataLen]
Definition: rtp.c:72
f
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Definition: wedgebug.c:204
rtp.h
offset
static const float offset[]
Definition: dw1000_arduino.c:199
rtp_frame_test
void rtp_frame_test(struct UdpSocket *udp)
Send a test RTP frame.
Definition: rtp.c:95
KRtpHeaderSize
#define KRtpHeaderSize
image_t::buf
void * buf
Image buffer (depending on the image_type)
Definition: image.h:54
JpegScanDataCh2B
uint8_t JpegScanDataCh2B[KJpegCh2ScanDataLen]
Definition: rtp.c:81
UdpSocket
Definition: udp_socket.h:34
image_t
Definition: image.h:44