Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
video_usb_logger.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015 Christophe De Wagter
3  * Copyright (C) 2016 Roland Meertens
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, write to
19  * the Free Software Foundation, 59 Temple Place - Suite 330,
20  * Boston, MA 02111-1307, USA.
21  *
22  */
23 
27 #include "video_usb_logger.h"
28 
29 #include <stdio.h>
30 #include "state.h"
31 #include "viewvideo.h"
32 #include "cv.h"
33 #include <unistd.h>
34 
35 #include <sys/types.h>
36 #include <sys/stat.h>
37 #include <unistd.h>
40 
42 #ifndef VIDEO_USB_LOGGER_PATH
43 #define VIDEO_USB_LOGGER_PATH /data/video/usb
44 #endif
45 
46 #ifndef VIDEO_USB_LOGGER_FPS
47 #define VIDEO_USB_LOGGER_FPS 0
48 #endif
49 PRINT_CONFIG_VAR(VIDEO_USB_LOGGER_FPS)
50 
51 
52 static FILE *video_usb_logger = NULL;
55 char foldername[512];
56 int shotNumber = 0;
57 
58 static void save_shot_on_disk(struct image_t *img, struct image_t *img_jpeg)
59 {
60 
61  // Search for a file where we can write to
62  char save_name[128];
63 
64  snprintf(save_name, sizeof(save_name), "%s/img_%05d.jpg", foldername, shotNumber);
65 
66  shotNumber++;
67  // Check if file exists or not
68  if (access(save_name, F_OK) == -1) {
69 
70  // Create a high quality image (99% JPEG encoded)
71  jpeg_encode_image(img, img_jpeg, 99, TRUE);
72 
73 #if VIDEO_USB_LOGGER_JPEG_WITH_EXIF_HEADER
74  write_exif_jpeg(save_name, img_jpeg->buf, img_jpeg->buf_size, img_jpeg->w, img_jpeg->h);
75 #else
76  FILE *fp = fopen(save_name, "w");
77  if (fp == NULL) {
78  printf("[video_thread-thread] Could not write shot %s.\n", save_name);
79  } else {
80  // Save it to the file and close it
81  fwrite(img_jpeg->buf, sizeof(uint8_t), img_jpeg->buf_size, fp);
82  fclose(fp);
83  printf("Wrote image\n");
84  }
85 #endif
86 
87 
88 
90  if (video_usb_logger == NULL) {
91  return;
92  }
93 
94  static uint32_t counter = 0;
95  struct pose_t pose = get_rotation_at_timestamp(img->pprz_ts);
96  struct NedCoor_i *ned = stateGetPositionNed_i();
97  struct NedCoor_i *accel = stateGetAccelNed_i();
98  static uint32_t sonar = 0;
99 
100 
101  // Save current information to a file
102  fprintf(video_usb_logger, "%d,%d,%f,%f,%f,%d,%d,%d,%d,%d,%d,%f,%f,%f,%d\n", counter,
103  shotNumber,
104  pose.eulers.phi, pose.eulers.theta, pose.eulers.psi,
105  ned->x, ned->y, ned->z,
106  accel->x, accel->y, accel->z,
107  pose.rates.p, pose.rates.q, pose.rates.r,
108  sonar);
109  counter++;
110  }
111 
112 }
113 
114 static struct image_t *log_image(struct image_t *img, uint8_t camera_id)
115 {
116  if (!created_jpeg) {
117 
118  // Create the jpeg image used later
119  image_create(&img_jpeg_global, img->w, img->h, IMAGE_JPEG);
120  created_jpeg = TRUE;
121  }
123  return img;
124 }
125 
128 {
129 
130  uint32_t counter = 0;
131  char filename[512];
132  struct stat st = {0};
133 
134  // Search and create a new folder
135  do {
136  snprintf(foldername, sizeof(foldername), "%s/pprzvideo%05d", STRINGIFY(VIDEO_USB_LOGGER_PATH), counter);
137  counter++;
138  } while (stat(foldername, &st) >= 0);
139 
140  mkdir(foldername, 0700);
141 
142 // In this folder create a textlog
143  snprintf(filename, sizeof(filename), "%s/log.csv", foldername);
144  video_usb_logger = fopen(filename, "w");
145 
146  if (video_usb_logger != NULL) {
147  fprintf(video_usb_logger, "counter,image,roll,pitch,yaw,x,y,z,accelx,accely,accelz,ratep,rateq,rater,sonar\n");
148  }
149 
150  // Subscribe to a camera
151  cv_add_to_device(&VIDEO_USB_LOGGER_CAMERA, log_image, VIDEO_USB_LOGGER_FPS, 0);
152 }
153 
156 {
157  if (video_usb_logger != NULL) {
158  fclose(video_usb_logger);
159  video_usb_logger = NULL;
160  }
161 }
162 
164 {
165 
166 }
shotNumber
int shotNumber
Definition: video_usb_logger.c:56
image_create
void image_create(struct image_t *img, uint16_t width, uint16_t height, enum image_type type)
Create a new image.
Definition: image.c:43
jpeg_encode_image
void jpeg_encode_image(struct image_t *in, struct image_t *out, uint32_t quality_factor, bool add_dri_header)
Encode an YUV422 image.
Definition: jpeg.c:408
sonar
std::shared_ptr< gazebo::sensors::SonarSensor > sonar
Definition: nps_fdm_gazebo.cpp:131
log_image
static struct image_t * log_image(struct image_t *img, uint8_t camera_id)
Definition: video_usb_logger.c:114
NedCoor_i::y
int32_t y
East.
Definition: pprz_geodetic_int.h:70
cv_add_to_device
struct video_listener * cv_add_to_device(struct video_config_t *device, cv_function func, uint16_t fps, uint8_t id)
Definition: cv.c:46
uint32_t
unsigned long uint32_t
Definition: types.h:18
image_t::w
uint16_t w
Image width.
Definition: image.h:46
stateGetPositionNed_i
static struct NedCoor_i * stateGetPositionNed_i(void)
Get position in local NED coordinates (int).
Definition: state.h:665
counter
int32_t counter
Definition: avoid_navigation.c:55
image_t::h
uint16_t h
Image height.
Definition: image.h:47
FloatEulers::theta
float theta
in radians
Definition: pprz_algebra_float.h:86
NedCoor_i::z
int32_t z
Down.
Definition: pprz_geodetic_int.h:71
get_rotation_at_timestamp
struct pose_t get_rotation_at_timestamp(uint32_t timestamp)
Given a pprz timestamp in used (obtained with get_sys_time_usec) we return the pose in FloatEulers cl...
Definition: pose_history.c:53
video_usb_logger.h
Camera image logger for Linux based autopilots.
cv.h
created_jpeg
bool created_jpeg
Definition: video_usb_logger.c:54
FloatEulers::phi
float phi
in radians
Definition: pprz_algebra_float.h:85
VIDEO_USB_LOGGER_PATH
#define VIDEO_USB_LOGGER_PATH
Set the default File logger path to the USB drive.
Definition: video_usb_logger.c:43
write_exif_jpeg
int write_exif_jpeg(char *filename, const unsigned char *image_jpg, const unsigned int image_jpg_len, const unsigned int image_jpg_x, const unsigned int image_jpg_y)
Definition: exif_module.c:146
NedCoor_i
vector in North East Down coordinates
Definition: pprz_geodetic_int.h:68
save_shot_on_disk
static void save_shot_on_disk(struct image_t *img, struct image_t *img_jpeg)
Definition: video_usb_logger.c:58
video_usb_logger_periodic
void video_usb_logger_periodic(void)
Definition: video_usb_logger.c:163
uint8_t
unsigned char uint8_t
Definition: types.h:14
FloatRates::r
float r
in rad/s
Definition: pprz_algebra_float.h:96
IMAGE_JPEG
@ IMAGE_JPEG
An JPEG encoded image (not per pixel encoded)
Definition: image.h:38
video_usb_logger
static FILE * video_usb_logger
The file pointer.
Definition: video_usb_logger.c:52
pose_history.h
img_jpeg_global
struct image_t img_jpeg_global
Definition: video_usb_logger.c:53
video_usb_logger_stop
void video_usb_logger_stop(void)
Stop the logger an nicely close the file.
Definition: video_usb_logger.c:155
FloatRates::q
float q
in rad/s
Definition: pprz_algebra_float.h:95
pose_t::eulers
struct FloatEulers eulers
Definition: pose_history.h:33
viewvideo.h
video_usb_logger_start
void video_usb_logger_start(void)
Start the file logger and open a new file.
Definition: video_usb_logger.c:127
NedCoor_i::x
int32_t x
North.
Definition: pprz_geodetic_int.h:69
stateGetAccelNed_i
static struct NedCoor_i * stateGetAccelNed_i(void)
Get acceleration in NED coordinates (int).
Definition: state.h:1020
state.h
FALSE
#define FALSE
Definition: std.h:5
FloatEulers::psi
float psi
in radians
Definition: pprz_algebra_float.h:87
TRUE
#define TRUE
Definition: std.h:4
foldername
char foldername[512]
Definition: video_usb_logger.c:55
VIDEO_USB_LOGGER_FPS
#define VIDEO_USB_LOGGER_FPS
Default FPS (zero means run at camera fps)
Definition: video_usb_logger.c:47
pose_t::rates
struct FloatRates rates
Definition: pose_history.h:34
FloatRates::p
float p
in rad/s
Definition: pprz_algebra_float.h:94
image_t
Definition: image.h:44
jpeg.h
pose_t
Definition: pose_history.h:31