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
bebop_front_camera.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2014 The Paparazzi Community
3  * 2015 Freek van Tienen <freek.v.tienen@gmail.com>
4  *
5  * This file is part of Paparazzi.
6  *
7  * Paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * Paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, see
19  * <http://www.gnu.org/licenses/>.
20  *
21  */
22 
27 // Own header
29 
30 #include <stdlib.h>
31 #include <stdio.h>
32 #include <string.h>
33 #include <unistd.h>
34 #include <sys/time.h>
35 #include <math.h>
36 
37 // Video
38 #include "lib/v4l/v4l2.h"
39 #include "lib/vision/image.h"
40 #include "lib/vision/bayer.h"
41 #include "lib/encoding/jpeg.h"
42 #include "lib/encoding/rtp.h"
43 #include "udp_socket.h"
44 
45 // Threaded computer vision
46 #include <pthread.h>
47 
48 #define MT9F002_WIDTH 1408
49 #define MT9F002_HEIGHT 2112
50 #define BEBOP_FRONT_CAMERA_WIDTH 272
51 #define BEBOP_FRONT_CAMERA_HEIGHT 272
52 
53 // The place where the shots are saved (without slash on the end)
54 #ifndef BEBOP_FRONT_CAMERA_SHOT_PATH
55 #define BEBOP_FRONT_CAMERA_SHOT_PATH /data/ftp/internal_000/images
56 #endif
57 PRINT_CONFIG_VAR(BEBOP_FRONT_CAMERA_SHOT_PATH)
58 
59 // Main thread
60 static void *bebop_front_camera_thread(void *data);
61 static void bebop_front_camera_save_shot(struct image_t *img_color, struct image_t *img_jpeg, struct image_t *raw_img);
63 
64 // Initialize the bebop_front_camera structure with the defaults
67  .take_shot = FALSE,
68  .shot_number = 0,
69  .take_shot = FALSE,
70  .shot_number = 0
71 };
72 
74 {
75  bebop_front_camera.take_shot = TRUE;
76 }
81 static void *bebop_front_camera_thread(void *data __attribute__((unused)))
82 {
83  struct UdpSocket video_sock;
84  udp_socket_create(&video_sock, STRINGIFY(BEBOP_FRONT_CAMERA_HOST), BEBOP_FRONT_CAMERA_PORT_OUT, -1,
85  BEBOP_FRONT_CAMERA_BROADCAST);
86 
87  // Create the JPEG encoded image
88  struct image_t img_jpeg;
90 
91  // Create the Color image
92  struct image_t img_color;
94 
95  // Start the streaming of the V4L2 device
96  if (!v4l2_start_capture(bebop_front_camera.dev)) {
97  printf("[bebop_front_camera-thread] Could not start capture of %s.\n", bebop_front_camera.dev->name);
98  return 0;
99  }
100 
101  // Start streaming
102  bebop_front_camera.is_streaming = TRUE;
103  while (bebop_front_camera.is_streaming) {
104  // Wait for a new frame (blocking)
105  struct image_t img;
106  v4l2_image_get(bebop_front_camera.dev, &img);
107 
108  BayerToYUV(&img, &img_color, 0, 0);
109 
110  if (bebop_front_camera.take_shot) {
111  // Save the image
112  bebop_front_camera_save_shot(&img_color, &img_jpeg, &img);
113  bebop_front_camera.take_shot = FALSE;
114  }
115 
116  jpeg_encode_image(&img_color, &img_jpeg, 80, 0);
117 
118  // Send image with RTP
120  &video_sock, // UDP socket
121  &img_jpeg,
122  0, // Format 422
123  80, // Jpeg-Quality
124  0, // DRI Header
125  0 // 90kHz time increment
126  );
127 
128 
129  // Free the image
130  v4l2_image_free(bebop_front_camera.dev, &img);
131  }
132 
133  return 0;
134 }
135 
140 {
141  // Initialize the V4L2 subdevice (TODO: fix hardcoded path, which and code)
142  if (!v4l2_init_subdev("/dev/v4l-subdev1", 0, 0, V4L2_MBUS_FMT_SGBRG10_1X10, MT9F002_WIDTH, MT9F002_HEIGHT)) {
143  printf("[bebop_front_camera] Could not initialize the v4l-subdev1 subdevice.\n");
144  return;
145  }
146 
147  // Initialize the V4L2 device
148  bebop_front_camera.dev = v4l2_init("/dev/video1", MT9F002_WIDTH, MT9F002_HEIGHT, 10, V4L2_PIX_FMT_SGBRG10);
149  if (bebop_front_camera.dev == NULL) {
150  printf("[bebop_front_camera] Could not initialize the /dev/video1 V4L2 device.\n");
151  return;
152  }
153 
154  // Create the shot directory
155  char save_name[128];
156  sprintf(save_name, "mkdir -p %s", STRINGIFY(BEBOP_FRONT_CAMERA_SHOT_PATH));
157  if (system(save_name) != 0) {
158  printf("[bebop_front_camera] Could not create shot directory %s.\n", STRINGIFY(BEBOP_FRONT_CAMERA_SHOT_PATH));
159  return;
160  }
161 }
162 
167 {
168  // Check if we are already running
169  if (bebop_front_camera.is_streaming) {
170  return;
171  }
172 
173  // Start the streaming thread
174  pthread_t tid;
175  if (pthread_create(&tid, NULL, bebop_front_camera_thread, NULL) != 0) {
176  printf("[vievideo] Could not create streaming thread.\n");
177  return;
178  }
179 }
180 
186 {
187  // Check if not already stopped streaming
188  if (!bebop_front_camera.is_streaming) {
189  return;
190  }
191 
192  // Stop the streaming thread
193  bebop_front_camera.is_streaming = FALSE;
194 
195  // Stop the capturing
196  if (!v4l2_stop_capture(bebop_front_camera.dev)) {
197  printf("[bebop_front_camera] Could not stop capture of %s.\n", bebop_front_camera.dev->name);
198  return;
199  }
200 
201  // TODO: wait for the thread to finish to be able to start the thread again!
202 }
203 
204 static void bebop_front_camera_save_shot(struct image_t *img, struct image_t *img_jpeg, struct image_t *raw_img)
205 {
206 
207  // Search for a file where we can write to
208  char save_name[128];
209  for (; bebop_front_camera.shot_number < 99999; bebop_front_camera.shot_number++) {
210  sprintf(save_name, "%s/img_%05d.pgm", STRINGIFY(BEBOP_FRONT_CAMERA_SHOT_PATH), bebop_front_camera.shot_number);
211  // Check if file exists or not
212  if (access(save_name, F_OK) == -1) {
213  FILE *fp = fopen(save_name, "w");
214  if (fp == NULL) {
215  printf("[bebop_front_camera-thread] Could not write shot %s.\n", save_name);
216  } else {
217  // Save it to the file and close it
218  char pgm_header[] = "P5\n272 272\n255\n";
219  fwrite(pgm_header, sizeof(char), 15, fp);
220  fwrite(img->buf, sizeof(uint8_t), img->buf_size, fp);
221  fclose(fp);
222 
223  // JPEG
224  jpeg_encode_image(img, img_jpeg, 99, 1);
225  sprintf(save_name, "%s/img_%05d.jpg", STRINGIFY(BEBOP_FRONT_CAMERA_SHOT_PATH), bebop_front_camera.shot_number);
226  fp = fopen(save_name, "w");
227  fwrite(img_jpeg->buf, sizeof(uint8_t), img_jpeg->buf_size, fp);
228  fclose(fp);
229 
230  sprintf(save_name, "%s/img_%05d.raw", STRINGIFY(BEBOP_FRONT_CAMERA_SHOT_PATH), bebop_front_camera.shot_number);
231  fp = fopen(save_name, "w");
232  fwrite(raw_img->buf, sizeof(uint8_t), raw_img->buf_size, fp);
233  fclose(fp);
234 
235  }
236 
237  // We don't need to seek for a next index anymore
238  break;
239  }
240  }
241 }
#define BEBOP_FRONT_CAMERA_SHOT_PATH
uint32_t buf_size
The buffer size.
Definition: image.h:49
bool_t v4l2_start_capture(struct v4l2_device *dev)
Start capturing images in streaming mode (Thread safe)
Definition: v4l2.c:387
#define BEBOP_FRONT_CAMERA_WIDTH
volatile bool_t is_streaming
When the device is streaming.
void v4l2_image_free(struct v4l2_device *dev, struct image_t *img)
Free the image and enqueue the buffer (Thread safe) This must be done after processing the image...
Definition: v4l2.c:366
void v4l2_image_get(struct v4l2_device *dev, struct image_t *img)
Get the latest image buffer and lock it (Thread safe, BLOCKING) This functions blocks until image acc...
Definition: v4l2.c:294
Capture images from a V4L2 device (Video for Linux 2)
static void * bebop_front_camera_thread(void *data)
Handles all the video streaming and saving of the image shots This is a sepereate thread...
void image_create(struct image_t *img, uint16_t width, uint16_t height, enum image_type type)
Create a new image.
Definition: image.c:38
Easily create and use UDP sockets.
Definition: image.h:42
Get live images from a RTP/UDP stream and save pictures on internal memory.
bool_t v4l2_stop_capture(struct v4l2_device *dev)
Stop capturing of the image stream (Thread safe) This function is blocking until capturing thread is ...
Definition: v4l2.c:446
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
#define FALSE
Definition: std.h:5
struct bebopfrontcamera_t bebop_front_camera
uint16_t shot_number
The last shot number.
void jpeg_encode_image(struct image_t *in, struct image_t *out, uint32_t quality_factor, bool_t add_dri_header)
Encode an YUV422 image.
Definition: jpeg.c:425
void bebop_front_camera_stop(void)
Stops the streaming This could take some time, because the thread is stopped asynchronous.
Image helper functions like resizing, color filter, converters...
#define TRUE
Definition: std.h:4
#define MT9F002_WIDTH
void BayerToYUV(struct image_t *Input, struct image_t *out, int RedX, int RedY)
Decode Bayer Pattern.
Definition: bayer.h:55
#define BEBOP_FRONT_CAMERA_HEIGHT
struct v4l2_device * dev
The V4L2 device that is used for the video stream.
void * buf
Image buffer (depending on the image_type)
Definition: image.h:50
#define MT9F002_HEIGHT
void bebop_front_camera_periodic(void)
A dummy for now.
volatile bool_t take_shot
Wether to take an image.
void bebop_front_camera_take_shot(bool_t take)
bool_t v4l2_init_subdev(char *subdev_name, uint8_t pad, uint8_t which, uint16_t code, uint16_t width, uint16_t height)
Initialize a V4L2 subdevice.
Definition: v4l2.c:130
Encode images with the use of the JPEG encoding.
void bebop_front_camera_init(void)
Initialize the view video.
char * name
The name of the device.
Definition: v4l2.h:49
void bebop_front_camera_start(void)
Start with streaming.
unsigned char uint8_t
Definition: types.h:14
static void bebop_front_camera_save_shot(struct image_t *img_color, struct image_t *img_jpeg, struct image_t *raw_img)
int udp_socket_create(struct UdpSocket *sock, char *host, int port_out, int port_in, bool_t broadcast)
Create UDP socket and bind it.
Definition: udp_socket.c:49
UYVY format (uint16 per pixel)
Definition: image.h:35
Encodes a vide stream with RTP (JPEG)
struct v4l2_device * v4l2_init(char *device_name, uint16_t width, uint16_t height, uint8_t buffers_cnt, uint32_t _pixelformat)
Initialize a V4L2(Video for Linux 2) device.
Definition: v4l2.c:177
An JPEG encoded image (not per pixel encoded)
Definition: image.h:37