Fawkes API Fawkes Development Version
jpeg_compressor_mmal.cpp
1
2/***************************************************************************
3 * jpeg_compressor_mmal.cpp - JPEG image compressor (using MMAL)
4 *
5 * Created: Wed Feb 05 15:13:30 2014
6 * Copyright 2005-2014 Tim Niemueller [www.niemueller.de]
7 ****************************************************************************/
8
9/* This program is free software; you can redistribute it and/or modify
10 * it under the terms of the GNU General Public License as published by
11 * the Free Software Foundation; either version 2 of the License, or
12 * (at your option) any later version. A runtime exception applies to
13 * this software (see LICENSE.GPL_WRE file mentioned below for details).
14 *
15 * This program is distributed in the hope that it will be useful,
16 * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 * GNU Library General Public License for more details.
19 *
20 * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
21 */
22
23#include <core/exception.h>
24#include <core/threading/mutex.h>
25#include <core/threading/wait_condition.h>
26#include <fvutils/compression/jpeg_compressor.h>
27#include <fvutils/compression/jpeg_compressor_mmal.h>
28
29extern "C" {
30#include <mmal/mmal.h>
31#include <mmal/mmal_buffer.h>
32#include <mmal/mmal_logging.h>
33#include <mmal/util/mmal_connection.h>
34#include <mmal/util/mmal_default_components.h>
35#include <mmal/util/mmal_util.h>
36#include <mmal/util/mmal_util_params.h>
37
38#include <bcm_host.h>
39}
40
41#include <cerrno>
42#include <cstdio>
43
44using namespace fawkes;
45
46namespace firevision {
47
48///@cond INTERNALS
49
50class JpegImageCompressorMMAL::State
51{
52public:
53 State()
54 {
55 frame_complete_ = false;
56 frame_complete_mutex_ = new fawkes::Mutex();
57 frame_complete_waitcond_ = new fawkes::WaitCondition(frame_complete_mutex_);
58
60 file_handle = NULL;
61 buffer = NULL;
62 encoder_component = NULL;
63 encoder_pool_in = NULL;
64 encoder_pool_out = NULL;
65 reset();
66 }
67
68 void
69 reset()
70 {
71 jpeg_bytes = 0;
72 buffer = jpeg_buffer;
73 }
74
76 FILE * file_handle;
77 char * buffer;
78
79 char * jpeg_buffer;
80 unsigned int jpeg_buffer_size;
81 unsigned int jpeg_bytes;
82
83 MMAL_COMPONENT_T *encoder_component; /// Pointer to the encoder component
84
85 MMAL_POOL_T *encoder_pool_in; /// Pointer to the pool of buffers used by encoder output port
86 MMAL_POOL_T *encoder_pool_out; /// Pointer to the pool of buffers used by encoder input port
87
88 bool frame_complete_;
89 fawkes::Mutex * frame_complete_mutex_;
90 fawkes::WaitCondition *frame_complete_waitcond_;
91};
92
93/**
94 * buffer header callback function for encoder
95 *
96 * Callback will dump buffer data to the specific file
97 *
98 * @param port Pointer to port from which callback originated
99 * @param buffer mmal buffer header pointer
100 */
101static void
102encoder_output_buffer_callback(MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *buffer)
103{
104 bool complete = false;
105
106 // We pass our file handle and other stuff in via the userdata field.
107 JpegImageCompressorMMAL::State *state = (JpegImageCompressorMMAL::State *)port->userdata;
108
109 if (state) {
110 size_t bytes_written = buffer->length;
111
112 if (buffer->length) {
113 mmal_buffer_header_mem_lock(buffer);
114 if (state->compdest == ImageCompressor::COMP_DEST_FILE && state->file_handle) {
115 bytes_written = fwrite(buffer->data, 1, buffer->length, state->file_handle);
116 } else if (state->compdest == ImageCompressor::COMP_DEST_MEM && state->buffer) {
117 if (state->jpeg_bytes + bytes_written <= state->jpeg_buffer_size) {
118 memcpy(state->buffer, buffer->data, buffer->length);
119 state->buffer += buffer->length;
120 state->jpeg_bytes += buffer->length;
121 } else {
122 printf("Buffer overflow: %zu + %zu > %zu\n",
123 state->jpeg_bytes,
124 bytes_written,
125 state->jpeg_buffer_size);
126 }
127 }
128 mmal_buffer_header_mem_unlock(buffer);
129 }
130
131 // We need to check we wrote what we wanted - it's possible we have run out of storage.
132 if (bytes_written != buffer->length) {
133 printf("Unable to write buffer to file - aborting");
134
135 complete = true;
136 }
137
138 // Now flag if we have completed
139 if (buffer->flags
140 & (MMAL_BUFFER_HEADER_FLAG_FRAME_END | MMAL_BUFFER_HEADER_FLAG_TRANSMISSION_FAILED)) {
141 complete = true;
142 }
143 } else {
144 printf("Received a encoder buffer callback with no state");
145 }
146
147 // release buffer back to the pool
148 mmal_buffer_header_release(buffer);
149
150 // and send one back to the port (if still open)
151 if (port->is_enabled) {
152 MMAL_STATUS_T status = MMAL_SUCCESS;
153 MMAL_BUFFER_HEADER_T *new_buffer;
154
155 new_buffer = mmal_queue_get(state->encoder_pool_out->queue);
156
157 if (new_buffer) {
158 status = mmal_port_send_buffer(port, new_buffer);
159 }
160 if (!new_buffer || status != MMAL_SUCCESS)
161 printf("Unable to return a buffer to the encoder port");
162 }
163
164 if (complete) {
165 state->frame_complete_mutex_->lock();
166 state->frame_complete_ = true;
167 state->frame_complete_waitcond_->wake_all();
168 state->frame_complete_mutex_->unlock();
169 }
170}
171
172static void
173encoder_input_buffer_callback(MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *buffer)
174{
175 // The decoder is done with the data, just recycle the buffer header into its pool
176 mmal_buffer_header_release(buffer);
177}
178
179/// @endcond
180
181/** @class JpegImageCompressorMMAL <fvutils/compression/jpeg_compressor.h>
182 * Jpeg image compressor.
183 * This JPEG image compressor implementation uses the MMAL hardware encoder
184 * of the Raspberry Pi.
185 * @author Tim Niemueller
186 */
187
188/** Constructor.
189 * @param quality JPEG quality in percent (1-100)
190 */
192{
193 vflip_ = false;
194 width_ = height_ = 0;
195 quality_ = quality;
196 state_ = new State();
197 // we can always do this, it'll just do nothing the second time
198 bcm_host_init();
199}
200
201/** Destructor. */
203{
204 destroy_encoder_component();
205 delete state_;
206}
207
208bool
210{
211 return true;
212}
213
214void
216{
217 vflip_ = enable;
218}
219
220void
222{
223 state_->reset();
224
225 MMAL_PORT_T *encoder_input = NULL;
226 MMAL_PORT_T *encoder_output = NULL;
227
228 MMAL_STATUS_T status = MMAL_SUCCESS;
229
230 // Enable component
231 if (mmal_component_enable(state_->encoder_component) != MMAL_SUCCESS) {
232 mmal_component_destroy(state_->encoder_component);
233 throw Exception("Unable to enable video encoder component");
234 }
235
236 encoder_input = state_->encoder_component->input[0];
237 encoder_output = state_->encoder_component->output[0];
238
239 if (state_->compdest == ImageCompressor::COMP_DEST_FILE) {
240 state_->file_handle = fopen(filename_, "wb");
241 if (!state_->file_handle) {
242 throw Exception(errno, "Failed to open output file");
243 }
244 }
245
246 state_->frame_complete_mutex_->lock();
247 state_->frame_complete_ = false;
248 state_->frame_complete_mutex_->unlock();
249
250 encoder_output->userdata = (::MMAL_PORT_USERDATA_T *)state_;
251
252 // Enable the encoder output port and tell it its callback function
253 status = mmal_port_enable(encoder_output, encoder_output_buffer_callback);
254
255 // Send all the buffers to the encoder output port
256 int num = mmal_queue_length(state_->encoder_pool_out->queue);
257
258 for (int q = 0; q < num; ++q) {
259 MMAL_BUFFER_HEADER_T *buffer = mmal_queue_get(state_->encoder_pool_out->queue);
260
261 if (!buffer)
262 printf("Unable to get a required buffer %d from pool queue", q);
263
264 if (mmal_port_send_buffer(encoder_output, buffer) != MMAL_SUCCESS)
265 printf("Unable to send a buffer to encoder output port (%d)", q);
266 }
267
268 // Enable the encoder output port and tell it its callback function
269 status = mmal_port_enable(encoder_input, encoder_input_buffer_callback);
270
271 MMAL_BUFFER_HEADER_T *buffer;
272 if ((buffer = mmal_queue_get(state_->encoder_pool_in->queue)) != NULL) {
273 size_t exp_size = colorspace_buffer_size(YUV422_PLANAR,
274 encoder_input->format->es->video.width,
275 encoder_input->format->es->video.height);
276 if (buffer->alloc_size < exp_size) {
277 printf("Too small buffer");
278 }
279
280 buffer->cmd = 0;
281 buffer->offset = 0;
282
283 char *data = (char *)buffer->data;
284 char *imgb = (char *)buffer_;
285
286 unsigned int h;
287 if (vflip_) {
288 for (h = 0; h < encoder_input->format->es->video.height; ++h) {
289 memcpy(data, imgb + ((height_ - h - 1) * width_), width_);
290 //imgb += width_;
291 data += encoder_input->format->es->video.width;
292 }
293
294 for (h = 0; h < encoder_input->format->es->video.height; ++h) {
295 memcpy(data, imgb + (width_ * height_) + ((height_ - h - 1) * (width_ / 2)), width_ / 2);
296 //imgb += width_ / 2;
297 data += encoder_input->format->es->video.width / 2;
298 }
299
300 for (h = 0; h < encoder_input->format->es->video.height; ++h) {
301 memcpy(data,
302 imgb + (width_ * height_) + ((width_ / 2) * height_)
303 + ((height_ - h - 1) * (width_ / 2)),
304 width_ / 2);
305 //memcpy(data, imgb, width_ / 2);
306 //imgb += width_ / 2;
307 data += encoder_input->format->es->video.width / 2;
308 }
309 } else {
310 for (h = 0; h < encoder_input->format->es->video.height; ++h) {
311 memcpy(data, imgb, width_);
312 imgb += width_;
313 data += encoder_input->format->es->video.width;
314 }
315
316 for (h = 0; h < encoder_input->format->es->video.height * 2; ++h) {
317 memcpy(data, imgb, width_ / 2);
318 imgb += width_ / 2;
319 data += encoder_input->format->es->video.width / 2;
320 }
321 }
322
323 buffer->length = (size_t)(data - (char *)buffer->data);
324 buffer->flags = MMAL_BUFFER_HEADER_FLAG_EOS;
325
326 status = mmal_port_send_buffer(encoder_input, buffer);
327 if (status != MMAL_SUCCESS) {
328 printf("Unable to send input buffer: %x\n", status);
329 }
330
331 state_->frame_complete_mutex_->lock();
332 while (!state_->frame_complete_) {
333 state_->frame_complete_waitcond_->wait();
334 }
335 state_->frame_complete_mutex_->unlock();
336 }
337
338 if (encoder_input && encoder_input->is_enabled)
339 mmal_port_disable(encoder_input);
340 if (encoder_output && encoder_output->is_enabled)
341 mmal_port_disable(encoder_output);
342
343 if (state_->compdest == ImageCompressor::COMP_DEST_FILE) {
344 fclose(state_->file_handle);
345 state_->file_handle = NULL;
346 }
347}
348
349void
350JpegImageCompressorMMAL::set_image_dimensions(unsigned int width, unsigned int height)
351{
352 if (width_ != width || height_ != height) {
353 width_ = width;
354 height_ = height;
355 destroy_encoder_component();
356 create_encoder_component();
357 }
358}
359
360void
361JpegImageCompressorMMAL::set_image_buffer(colorspace_t cspace, unsigned char *buffer)
362{
363 if (cspace == YUV422_PLANAR) {
364 buffer_ = buffer;
365 } else {
366 throw Exception("JpegImageCompressorMMAL: can only accept YUV422_PLANAR buffers");
367 }
368}
369
370void
372{
373 state_->compdest = cd;
374}
375
376bool
379{
380 return true;
381}
382
383void
384JpegImageCompressorMMAL::set_destination_buffer(unsigned char *buf, unsigned int buf_size)
385{
386 state_->jpeg_buffer = (char *)buf;
387 state_->jpeg_buffer_size = buf_size;
388}
389
390size_t
392{
393 return state_->jpeg_bytes;
394}
395
396size_t
398{
399 return width_ * height_ * 2;
400}
401
402void
404{
405 filename_ = filename;
406}
407
408/** Create the encoder component, set up its ports */
409void
410JpegImageCompressorMMAL::create_encoder_component()
411{
412 MMAL_COMPONENT_T *encoder = 0;
413 MMAL_PORT_T * encoder_input = NULL, *encoder_output = NULL;
414 MMAL_STATUS_T status;
415 MMAL_POOL_T * pool;
416
417 status = mmal_component_create(MMAL_COMPONENT_DEFAULT_IMAGE_ENCODER, &encoder);
418
419 if (status != MMAL_SUCCESS) {
420 if (encoder)
421 mmal_component_destroy(encoder);
422 throw Exception("Unable to create JPEG encoder component");
423 }
424
425 if (!encoder->input_num || !encoder->output_num) {
426 mmal_component_destroy(encoder);
427 throw Exception("JPEG encoder doesn't have input/output ports");
428 }
429
430 encoder_input = encoder->input[0];
431 encoder_output = encoder->output[0];
432
433 memset(&encoder_input->format->es->video, 0, sizeof(MMAL_VIDEO_FORMAT_T));
434 encoder_input->format->es->video.width = width_;
435 encoder_input->format->es->video.height = height_;
436 encoder_input->format->es->video.crop.x = 0;
437 encoder_input->format->es->video.crop.y = 0;
438 encoder_input->format->es->video.crop.width = width_;
439 encoder_input->format->es->video.crop.height = height_;
440 encoder_input->format->es->video.frame_rate.num = 1;
441 encoder_input->format->es->video.frame_rate.den = 1;
442
443 // We want same format on input and output
444 mmal_format_copy(encoder_output->format, encoder_input->format);
445
446 // Specify input format
447 encoder_input->format->flags = 0;
448 encoder_input->format->encoding = MMAL_ENCODING_I422;
449
450 // Specify out output format
451 encoder_output->format->encoding = MMAL_ENCODING_JPEG;
452
453 encoder_output->buffer_size = encoder_output->buffer_size_recommended * 2;
454 if (encoder_output->buffer_size < encoder_output->buffer_size_min)
455 encoder_output->buffer_size = encoder_output->buffer_size_min;
456
457 // Set the JPEG quality level
458 status = mmal_port_parameter_set_uint32(encoder_output, MMAL_PARAMETER_JPEG_Q_FACTOR, quality_);
459 if (status != MMAL_SUCCESS) {
460 mmal_component_destroy(encoder);
461 throw Exception("Unable to set JPEG quality");
462 }
463
464 status = mmal_port_parameter_set_boolean(encoder_output, MMAL_PARAMETER_EXIF_DISABLE, 1);
465 if (status != MMAL_SUCCESS) {
466 mmal_component_destroy(encoder);
467 throw Exception("Unable to disable JPEG EXIF data");
468 }
469
470 // Commit the port changes to the output port
471 status = mmal_port_format_commit(encoder_output);
472
473 if (status != MMAL_SUCCESS) {
474 mmal_component_destroy(encoder);
475 throw Exception("Unable to set format on video encoder output port");
476 }
477
478 // Commit the port changes to the input port
479 status = mmal_port_format_commit(encoder_input);
480
481 if (status != MMAL_SUCCESS) {
482 mmal_component_destroy(encoder);
483 throw Exception("Unable to set format on input encoder port");
484 }
485
486 {
487 MMAL_PARAMETER_THUMBNAIL_CONFIG_T param_thumb = {{MMAL_PARAMETER_THUMBNAIL_CONFIGURATION,
488 sizeof(MMAL_PARAMETER_THUMBNAIL_CONFIG_T)},
489 0,
490 0,
491 0,
492 0};
493 status = mmal_port_parameter_set(encoder->control, &param_thumb.hdr);
494 }
495
496 /* Create pool of buffer headers for the output port to consume */
497 pool =
498 mmal_port_pool_create(encoder_output, encoder_output->buffer_num, encoder_output->buffer_size);
499
500 if (!pool) {
501 mmal_component_destroy(encoder);
502 throw Exception("Failed to create buffer header pool for encoder output port %s",
503 encoder_output->name);
504 }
505
506 state_->encoder_pool_out = pool;
507
508 /* Create pool of buffer headers for the input port to consume */
509 pool =
510 mmal_port_pool_create(encoder_input, encoder_input->buffer_num, encoder_input->buffer_size);
511
512 if (!pool) {
513 mmal_component_destroy(encoder);
514 throw Exception("Failed to create buffer header pool for encoder input port %s",
515 encoder_input->name);
516 }
517
518 state_->encoder_pool_in = pool;
519 state_->encoder_component = encoder;
520}
521
522/** Create the encoder component, set up its ports */
523void
524JpegImageCompressorMMAL::destroy_encoder_component()
525{
526 mmal_component_disable(state_->encoder_component);
527
528 if (state_->encoder_pool_in) {
529 mmal_port_pool_destroy(state_->encoder_component->input[0], state_->encoder_pool_in);
530 state_->encoder_pool_in = NULL;
531 }
532
533 if (state_->encoder_pool_out) {
534 mmal_port_pool_destroy(state_->encoder_component->output[0], state_->encoder_pool_out);
535 state_->encoder_pool_out = NULL;
536 }
537
538 if (state_->encoder_component) {
539 mmal_component_destroy(state_->encoder_component);
540 state_->encoder_component = NULL;
541 }
542}
543
544} // end namespace firevision
Base class for exceptions in Fawkes.
Definition: exception.h:36
Mutex mutual exclusion lock.
Definition: mutex.h:33
Wait until a given condition holds.
CompressionDestination
Where to put the compressed image.
@ COMP_DEST_MEM
write compressed image to buffer in memory
@ COMP_DEST_FILE
write compressed image to file
virtual bool supports_vflip()
Check if image compressor can do vflip during compress.
virtual size_t recommended_compressed_buffer_size()
Get the recommended size for the compressed buffer.
virtual size_t compressed_size()
Get compressed size.
virtual void compress()
Compress image.
virtual void set_destination_buffer(unsigned char *buf, unsigned int buf_size)
Set destination buffer (if compressing to memory).
JpegImageCompressorMMAL(unsigned int quality=80)
Constructor.
virtual void set_image_buffer(colorspace_t cspace, unsigned char *buffer)
Set image buffer to compress.
virtual void set_compression_destination(ImageCompressor::CompressionDestination cd)
Set compression destination.
virtual bool supports_compression_destination(ImageCompressor::CompressionDestination cd)
Check if compressor supports desired compression destination.
virtual void set_vflip(bool enable)
Enable or disable vflipping.
virtual void set_filename(const char *filename)
Set file name.
virtual void set_image_dimensions(unsigned int width, unsigned int height)
Set dimensions of image to compress.
Fawkes library namespace.