Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
exif_module.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015
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 
22 
23 #include "exif_module.h"
24 #include "state.h"
25 #include "modules/gps/gps.h"
26 
27 
29 // Multithreaded part
30 
31 volatile int32_t lat_em7deg = 0;
32 volatile int32_t lon_em7deg = 0;
33 volatile int32_t alt_mm = 0;
34 
36 // Paparazzi part
37 
39 {
40  struct LlaCoor_i *c = stateGetPositionLla_i();
41  lat_em7deg = c->lat;
42  lon_em7deg = c->lon;
43  // alt_mm = c->alt;
44  alt_mm = gps.hmsl;
45 }
46 
48 // Vision part
49 
50 #include <libexif/exif-data.h>
51 #include <stdio.h>
52 #include <string.h>
53 #include <stdlib.h>
54 
55 /* Get an existing tag, or create one if it doesn't exist */
56 static ExifEntry *init_tag(ExifData *exif, ExifIfd ifd, ExifTag tag)
57 {
58  ExifEntry *entry;
59  /* Return an existing tag if one exists */
60  if (!((entry = exif_content_get_entry(exif->ifd[ifd], tag)))) {
61  /* Allocate a new entry */
62  entry = exif_entry_new();
63  //assert(entry != NULL); /* catch an out of memory condition */
64  entry->tag = tag; /* tag must be set before calling
65  exif_content_add_entry */
66 
67  /* Attach the ExifEntry to an IFD */
68  exif_content_add_entry(exif->ifd[ifd], entry);
69 
70  /* Allocate memory for the entry and fill with default data */
71  exif_entry_initialize(entry, tag);
72 
73  /* Ownership of the ExifEntry has now been passed to the IFD.
74  * One must be very careful in accessing a structure after
75  * unref'ing it; in this case, we know "entry" won't be freed
76  * because the reference count was bumped when it was added to
77  * the IFD.
78  */
79  exif_entry_unref(entry);
80  }
81  return entry;
82 }
83 
84 /* Create a brand-new tag with a data field of the given length, in the
85  * given IFD. This is needed when exif_entry_initialize() isn't able to create
86  * this type of tag itself, or the default data length it creates isn't the
87  * correct length.
88  */
89 static ExifEntry *create_tag(ExifData *exif, ExifIfd ifd, ExifTag tag, size_t len)
90 {
91  void *buf;
92  ExifEntry *entry;
93 
94  /* Create a memory allocator to manage this ExifEntry */
95  ExifMem *mem = exif_mem_new_default();
96  //assert(mem != NULL); /* catch an out of memory condition */
97 
98  /* Create a new ExifEntry using our allocator */
99  entry = exif_entry_new_mem(mem);
100  //assert(entry != NULL);
101 
102  /* Allocate memory to use for holding the tag data */
103  buf = exif_mem_alloc(mem, len);
104  //assert(buf != NULL);
105 
106  /* Fill in the entry */
107  entry->data = buf;
108  entry->size = len;
109  entry->tag = tag;
110  entry->components = len;
111  entry->format = EXIF_FORMAT_UNDEFINED;
112 
113  /* Attach the ExifEntry to an IFD */
114  exif_content_add_entry(exif->ifd[ifd], entry);
115 
116  /* The ExifMem and ExifEntry are now owned elsewhere */
117  exif_mem_unref(mem);
118  exif_entry_unref(entry);
119 
120  return entry;
121 }
122 
123 /* start of JPEG image data section */
124 static const unsigned int image_data_offset = 2;
125 #define image_data_len (image_jpg_len - image_data_offset)
126 
127 /* raw EXIF header data */
128 static const unsigned char exif_header[] = {
129  0xff, 0xd8, 0xff, 0xe1
130 };
131 /* length of data in exif_header */
132 static const unsigned int exif_header_len = sizeof(exif_header);
133 
134 /* byte order to use in the EXIF block */
135 #define FILE_BYTE_ORDER EXIF_BYTE_ORDER_INTEL
136 
137 /* comment to write into the EXIF block */
138 #define FILE_COMMENT "Paparazzi autopilot"
139 
140 /* special header required for EXIF_TAG_USER_COMMENT */
141 #define ASCII_COMMENT "ASCII\0\0\0"
142 
143 
144 
145 
146 int write_exif_jpeg(char *filename, const unsigned char *image_jpg, const unsigned int image_jpg_len,
147  const unsigned int image_jpg_x, const unsigned int image_jpg_y)
148 {
149  int rc = 1;
150  FILE *f;
151  unsigned char *exif_data;
152  unsigned int exif_data_len;
153  ExifEntry *entry;
154  ExifData *exif = exif_data_new();
155  if (!exif) {
156  fprintf(stderr, "Out of memory\n");
157  return 2;
158  }
159 
160  /* Set the image options */
161  exif_data_set_option(exif, EXIF_DATA_OPTION_FOLLOW_SPECIFICATION);
162  exif_data_set_data_type(exif, EXIF_DATA_TYPE_COMPRESSED);
163  exif_data_set_byte_order(exif, FILE_BYTE_ORDER);
164 
165  /* Create the mandatory EXIF fields with default data */
166  exif_data_fix(exif);
167 
168  /* All these tags are created with default values by exif_data_fix() */
169  /* Change the data to the correct values for this image. */
170  entry = init_tag(exif, EXIF_IFD_EXIF, EXIF_TAG_PIXEL_X_DIMENSION);
171  exif_set_long(entry->data, FILE_BYTE_ORDER, image_jpg_x);
172 
173  entry = init_tag(exif, EXIF_IFD_EXIF, EXIF_TAG_PIXEL_Y_DIMENSION);
174  exif_set_long(entry->data, FILE_BYTE_ORDER, image_jpg_y);
175 
176  entry = init_tag(exif, EXIF_IFD_EXIF, EXIF_TAG_COLOR_SPACE);
177  exif_set_short(entry->data, FILE_BYTE_ORDER, 1);
178 
179  /* Create a EXIF_TAG_USER_COMMENT tag. This one must be handled
180  * differently because that tag isn't automatically created and
181  * allocated by exif_data_fix(), nor can it be created using
182  * exif_entry_initialize() so it must be explicitly allocated here.
183  */
184  entry = create_tag(exif, EXIF_IFD_EXIF, EXIF_TAG_USER_COMMENT,
185  sizeof(ASCII_COMMENT) + sizeof(FILE_COMMENT) - 2);
186  /* Write the special header needed for a comment tag */
187  memcpy(entry->data, ASCII_COMMENT, sizeof(ASCII_COMMENT) - 1);
188  /* Write the actual comment text, without the trailing NUL character */
189  memcpy(entry->data + 8, FILE_COMMENT, sizeof(FILE_COMMENT) - 1);
190  /* create_tag() happens to set the format and components correctly for
191  * EXIF_TAG_USER_COMMENT, so there is nothing more to do. */
192 
193  /* Create a EXIF_TAG_SUBJECT_AREA tag */
194  entry = create_tag(exif, EXIF_IFD_EXIF, EXIF_TAG_SUBJECT_AREA,
195  4 * exif_format_get_size(EXIF_FORMAT_SHORT));
196  entry->format = EXIF_FORMAT_SHORT;
197  entry->components = 4;
198  exif_set_short(entry->data, FILE_BYTE_ORDER, image_jpg_x / 2);
199  exif_set_short(entry->data + 2, FILE_BYTE_ORDER, image_jpg_y / 2);
200  exif_set_short(entry->data + 4, FILE_BYTE_ORDER, image_jpg_x);
201  exif_set_short(entry->data + 6, FILE_BYTE_ORDER, image_jpg_y);
202 
203  entry = create_tag(exif, EXIF_IFD_GPS, EXIF_TAG_GPS_LATITUDE_REF, 2);
204  entry->format = EXIF_FORMAT_ASCII;
205  entry->components = 1;
206  entry->data[1] = 0;
207  if (lat_em7deg < 0) {
208  entry->data[0] = 'S';
209  // from now on: go positive only
211  }
212  else {
213  entry->data[0] = 'N';
214  }
215 
216  entry = create_tag(exif, EXIF_IFD_GPS, EXIF_TAG_GPS_LONGITUDE_REF, 2);
217  entry->format = EXIF_FORMAT_ASCII;
218  entry->components = 1;
219  entry->data[1] = 0;
220  if (lon_em7deg < 0) {
221  entry->data[0] = 'W';
222  // from now on: go positive only
224  }
225  else {
226  entry->data[0] = 'E';
227  }
228 
229 
230 
231  entry = create_tag(exif, EXIF_IFD_GPS, EXIF_TAG_GPS_LATITUDE, 24);
232  // Set the field's format and number of components, this is very important!
233  entry->format = EXIF_FORMAT_RATIONAL;
234  entry->components = 3;
235  // Degrees
237  uint32_t lati = lat / 1e7;
238  ExifRational loc;
239  loc.numerator = lati;
240  loc.denominator = 1;
241  exif_set_rational(entry->data, EXIF_BYTE_ORDER_INTEL, loc);
242  lat -= lati * 1e7;
243  lat *= 60.0;
244  lati = lat / 1e7;
245  loc.numerator = lati;
246  exif_set_rational(entry->data + 8, EXIF_BYTE_ORDER_INTEL, loc);
247  lat -= lati * 1e7;
248  lat *= 60.0;
249  lati = lat / 1e4;
250  loc.numerator = lati;
251  loc.denominator = 1000;
252  exif_set_rational(entry->data + 16, EXIF_BYTE_ORDER_INTEL, loc);
253 
254  entry = create_tag(exif, EXIF_IFD_GPS, EXIF_TAG_GPS_LONGITUDE, 24);
255  // Set the field's format and number of components, this is very important!
256  entry->format = EXIF_FORMAT_RATIONAL;
257  entry->components = 3;
258  // Degrees
259  // Degrees
261  uint32_t loni = lon / 1e7;
262  loc.numerator = loni;
263  loc.denominator = 1;
264  exif_set_rational(entry->data, EXIF_BYTE_ORDER_INTEL, loc);
265  lon -= loni * 1e7;
266  lon *= 60.0;
267  loni = lon / 1e7;
268  loc.numerator = loni;
269  loc.denominator = 1;
270  exif_set_rational(entry->data + 8, EXIF_BYTE_ORDER_INTEL, loc);
271  lon -= loni * 1e7;
272  lon *= 60.0;
273  loni = lon / 1e4;
274  loc.numerator = loni;
275  loc.denominator = 1000;
276  exif_set_rational(entry->data + 16, EXIF_BYTE_ORDER_INTEL, loc);
277 
278  entry = create_tag(exif, EXIF_IFD_GPS, EXIF_TAG_GPS_ALTITUDE_REF, 1);
279  entry->format = EXIF_FORMAT_BYTE;
280  entry->components = 1;
281  if (alt_mm < 0) {
282  entry->data[0] = 1; // Below MSL
283  // from now on: go positive only
284  alt_mm = -alt_mm;
285  }
286  else {
287  entry->data[0] = 0; // Above MSL
288  }
289 
290  entry = create_tag(exif, EXIF_IFD_GPS, EXIF_TAG_GPS_ALTITUDE, 8);
291  // Set the field's format and number of components, this is very important!
292  entry->format = EXIF_FORMAT_RATIONAL;
293  entry->components = 1;
294  // Height
295  ExifRational alt;
296  alt.numerator = alt_mm;
297  alt.denominator = 1000;
298  exif_set_rational(entry->data, EXIF_BYTE_ORDER_INTEL, alt);
299 
300 
301  /* Get a pointer to the EXIF data block we just created */
302  exif_data_save_data(exif, &exif_data, &exif_data_len);
303 // assert(exif_data != NULL);
304 
305  f = fopen(filename, "wb");
306  if (!f) {
307  fprintf(stderr, "Error creating file %s\n", filename);
308  exif_data_unref(exif);
309  return rc;
310  }
311  /* Write EXIF header */
312  if (fwrite(exif_header, exif_header_len, 1, f) != 1) {
313  fprintf(stderr, "Error writing to file %s\n", filename);
314  goto errout;
315  }
316  /* Write EXIF block length in big-endian order */
317  if (fputc((exif_data_len + 2) >> 8, f) < 0) {
318  fprintf(stderr, "Error writing to file %s\n", filename);
319  goto errout;
320  }
321  if (fputc((exif_data_len + 2) & 0xff, f) < 0) {
322  fprintf(stderr, "Error writing to file %s\n", filename);
323  goto errout;
324  }
325  /* Write EXIF data block */
326  if (fwrite(exif_data, exif_data_len, 1, f) != 1) {
327  fprintf(stderr, "Error writing to file %s\n", filename);
328  goto errout;
329  }
330  /* Write JPEG image data, skipping the non-EXIF header */
331  if (fwrite(image_jpg + image_data_offset, image_data_len, 1, f) != 1) {
332  fprintf(stderr, "Error writing to file %s\n", filename);
333  goto errout;
334  }
335  printf("Wrote file %s\n", filename);
336  rc = 0;
337 
338 errout:
339  if (fclose(f)) {
340  fprintf(stderr, "Error writing to file %s\n", filename);
341  rc = 1;
342  }
343  /* The allocator we're using for ExifData is the standard one, so use
344  * it directly to free this pointer.
345  */
346  free(exif_data);
347  exif_data_unref(exif);
348 
349  return rc;
350 }
351 
352 
volatile int32_t alt_mm
Definition: exif_module.c:33
#define ASCII_COMMENT
Definition: exif_module.c:141
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
static const unsigned char exif_header[]
Definition: exif_module.c:128
static ExifEntry * create_tag(ExifData *exif, ExifIfd ifd, ExifTag tag, size_t len)
Definition: exif_module.c:89
#define FILE_COMMENT
Definition: exif_module.c:138
static ExifEntry * init_tag(ExifData *exif, ExifIfd ifd, ExifTag tag)
Definition: exif_module.c:56
#define FILE_BYTE_ORDER
Definition: exif_module.c:135
void push_gps_to_vision(void)
Definition: exif_module.c:38
volatile int32_t lat_em7deg
Definition: exif_module.c:31
static const unsigned int image_data_offset
Definition: exif_module.c:124
volatile int32_t lon_em7deg
Definition: exif_module.c:32
#define image_data_len
Definition: exif_module.c:125
static const unsigned int exif_header_len
Definition: exif_module.c:132
Write JPEG images containing EXIF headers with GPS coordinates.
struct GpsState gps
global GPS state
Definition: gps.c:69
Device independent GPS code (interface)
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:92
int32_t lat
in degrees*1e7
int32_t alt
in millimeters above WGS84 reference ellipsoid
int32_t lon
in degrees*1e7
vector in Latitude, Longitude and Altitude
static struct LlaCoor_i * stateGetPositionLla_i(void)
Get position in LLA coordinates (int).
Definition: state.h:683
API to get/set the generic vehicle states.
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Definition: wedgebug.c:204