source: pacpusframework/branches/2.0-beta1/src/TestComponents/Video/sensor/Camera1394Unix.cpp@ 89

Last change on this file since 89 was 89, checked in by morasjul, 11 years ago

PACPUS 2.0 Beta deployed in new branch

Major changes:
-Add communication interface between components
-Add examples for communications interface (TestComponents)
-Move to Qt5 support

  • Property svn:executable set to *
File size: 34.7 KB
Line 
1/// purpose: Definition of the Camera1394 class
2///
3/// @date created 2011/02/28 - 15:21
4/// @author Sergio Rodriguez
5/// @copyright Heudiasyc UMR UTC/CNRS 6599
6/// @version: $Id$
7
8#include "Camera1394Unix.h"
9
10#include "structure/genericStructures.h"
11#include <kernel/inputOutputInterface.h>
12//#include <qimage.h>
13#include <qimagewriter.h>
14#include <qmutex.h>
15
16#include "kernel/ComponentFactory.h"
17#include "kernel/DbiteFileTypes.h"
18#include "kernel/Log.h"
19#include "PacpusTools/ShMem.h"
20
21namespace pacpus {
22
23using namespace std;
24
25DECLARE_STATIC_LOGGER("pacpus.base.Camera1394");
26
27// Construct the factory
28ComponentFactory<Camera1394> sFactory("Camera1394");
29
30void Camera1394::addInputOutput()
31{
32 output.insert("image",new OutputInterface<PacpusImage,Camera1394> ("image",this));
33}
34
35//////////////////////////////////////////////////////////////////////////
36// Contructor
37//////////////////////////////////////////////////////////////////////////
38Camera1394::Camera1394(QString name)
39 : ComponentBase(name)
40{
41 qimage_ = NULL;
42 acquiring_ = FALSE;
43 recording_ = FALSE;
44
45 width_ = 0;
46 height_ = 0;
47
48 tr_ = 0;
49 addInputOutput();
50}
51
52//////////////////////////////////////////////////////////////////////////
53// Destructor
54//////////////////////////////////////////////////////////////////////////
55Camera1394::~Camera1394()
56{
57 // verify that the camera has been initialized
58 //if (theCamera->m_cameraInitialized)
59 //if (theCamera->StopImageAcquisition() != CAM_SUCCESS)
60 // qFatal("Problem Stopping Image Acquisition");
61
62 delete qimage_;
63 qimage_ = NULL;
64}
65
66//////////////////////////////////////////////////////////////////////////
67// Configure the component
68// Parameters are:
69// - node: the number of the camera in the driver list. Normally, node 0
70// is for the first camera connected, node is incremented for other
71//////////////////////////////////////////////////////////////////////////
72ComponentBase::COMPONENT_CONFIGURATION Camera1394::configureComponent(XmlComponentConfig config)
73{
74 cameraNode_ = param.getProperty("node").toInt();
75 videoFormat_ = param.getProperty("format").toInt();
76 videoMode_ = param.getProperty("mode").toInt();
77 frameRate_ = param.getProperty("framerate").toInt();
78 recording_ = (param.getProperty("recording") == "true" ? true : false );
79 recordFormat_ = param.getProperty("recordFormat");
80 reverseImage_ = (param.getProperty("reverseImage") == "true" ? true : false);
81 displaying_ = (param.getProperty("display") == "true" ? true : false);
82 zoom_ = param.getProperty("zoom").toInt();
83
84 imageMutex_ = new QMutex;
85
86 if (displaying_)
87 {
88 viewer_ = new ImageViewer;
89 viewer_->setMutex(imageMutex_);
90 viewer_->show();
91 viewer_->setWindowTitle(componentName);
92 connect(this, SIGNAL( newImage(QImage*) ), viewer_, SLOT( display(QImage*) ) );
93 }
94
95 switch(frameRate_){
96 case 0:
97 theFramerate = DC1394_FRAMERATE_1_875;
98 break;
99 case 1:
100 theFramerate = DC1394_FRAMERATE_3_75;
101 break;
102 case 2:
103 theFramerate = DC1394_FRAMERATE_7_5;
104 break;
105 case 3:
106 theFramerate = DC1394_FRAMERATE_15;
107 break;
108 case 4:
109 theFramerate = DC1394_FRAMERATE_30;
110 break;
111 case 5:
112 theFramerate = DC1394_FRAMERATE_60;
113 break;
114 default:
115 qFatal("Framerate not supported");
116 break;
117 }
118
119 return ComponentBase::CONFIGURED_OK;
120}
121
122//////////////////////////////////////////////////////////////////////////
123// Stop the acquisition of data
124//////////////////////////////////////////////////////////////////////////
125void Camera1394::stopActivity()
126{
127 stopAcquiring();
128
129 // wait the thread termination
130 if (!wait(3000)) {
131 terminate();
132 LOG_WARN(componentName << ": The thread doesn't respond for 1 second, it has been terminated");
133 }
134 if (recording_) {
135 file_.close();
136 }
137}
138
139//////////////////////////////////////////////////////////////////////////
140// Start the camera
141// Set all video parameters (resolution, mode and framerate)
142//////////////////////////////////////////////////////////////////////////
143void Camera1394::startActivity()
144{
145 dc1394error_t err;
146
147 if (recording_) {
148 dbtFileName_ = componentName + ".dbt";
149 file_.open(dbtFileName_.toStdString(), WriteMode, FILE_JPEG, MAX_CHAR_DIRECTORY_NAME+MAX_CHAR_PICTURE_NAME);
150 }
151 if(!initCamera())
152 return;
153
154 // make the calls to set up the capture mode
155 dc1394_video_set_iso_speed( theCamera, DC1394_ISO_SPEED_400 );
156 dc1394_video_set_mode( theCamera, theCamera_video_mode );
157 dc1394_video_set_framerate( theCamera, theFramerate);
158
159 err = dc1394_capture_setup( theCamera, 8, DC1394_CAPTURE_FLAGS_DEFAULT );
160 if ( err != DC1394_SUCCESS )
161 {
162 cout << "Can't setup capture" << endl;
163 return;
164 }
165 // have the camera start sending us data
166 err = dc1394_video_set_transmission( theCamera, DC1394_ON );
167 if ( err != DC1394_SUCCESS )
168 {
169 fprintf( stderr, "Unable to start camera iso transmission\n" );
170 return;
171 }
172 printf( "Waiting for transmission... \n" );
173 // Sleep untill the camera has a transmission
174 dc1394switch_t status = DC1394_OFF;
175
176 for ( int i = 0; i <= 5; i++ )
177 {
178 usleep(50000);
179 err = dc1394_video_get_transmission( theCamera, &status );
180 if ( err != DC1394_SUCCESS )
181 {
182 fprintf( stderr, "Unable to get transmision status\n" );
183 return;
184 }
185 if ( status != DC1394_OFF )
186 break;
187
188 if( i == 5 )
189 {
190 fprintf(stderr,"Camera doesn't seem to want to turn on!\n");
191 return;
192 }
193 }
194
195// // switch some controls in Automatic mode
196// theCamera->m_controlWhiteBalance.SetAutoMode(TRUE);
197// theCamera->m_controlShutter.SetAutoMode(TRUE);
198// theCamera->m_controlGain.SetAutoMode(TRUE);
199// theCamera->m_controlIris.SetAutoMode(TRUE);
200//
201// theCamera->SetZoom(zoom_);
202
203 // alloc images
204 if (!qimage_)
205 qimage_ = new QImage( width_, height_, QImage::Format_RGB32 );
206
207 startAcquiring();
208 start();
209}
210
211//////////////////////////////////////////////////////////////////////////
212// Initialize the C1394Camera Driver
213// Verify the presence of a compatible camera and select it
214// If it doesn't work, please have a look on the Windows driver in the
215// hardware manager
216//////////////////////////////////////////////////////////////////////////
217bool Camera1394::initCamera()
218{
219 dc1394error_t err;
220 dc1394_t * d;
221 dc1394camera_list_t * list;
222
223 dc1394video_modes_t video_modes;
224 dc1394video_mode_t required_video_mode;
225 dc1394color_coding_t coding;
226
227 bool printSupportedFormats = false;
228 bool foundFormatVideoMode = false;
229
230 // Find cameras on the 1394 buses
231 d = dc1394_new ();
232
233 // Enumerate cameras connected to the PC
234 err = dc1394_camera_enumerate (d, &list);
235 if ( err != DC1394_SUCCESS )
236 {
237 fprintf( stderr, "Unable to look for cameras\n\n"
238 "Please check \n"
239 " - if the kernel modules `ieee1394',`raw1394' and `ohci1394' "
240 "are loaded \n"
241 " - if you have read/write access to /dev/raw1394\n\n");
242 return false;
243 }
244
245 if (list->num == 0)
246 {
247 cout << "No cameras found!" << endl;
248 return false;
249 }
250
251 cout << "There is(are)" << list->num <<" camera(s) connected"<< endl;
252 if(list->num>cameraNode_){
253 theCamera = dc1394_camera_new(d, list->ids[cameraNode_].guid);
254 if (!theCamera){
255 LOG_ERROR("Failed to initialize camera with guid " << list->ids[cameraNode_].guid);
256 } else {
257 LOG_INFO("Camera model = '"<< theCamera->model << " was initialized");
258 }
259 }
260
261 err = dc1394_video_get_supported_modes( theCamera, &video_modes );
262 if ( err != DC1394_SUCCESS )
263 {
264 fprintf( stderr, "Can't get video modes\n" );
265 return err;
266 }
267
268 //Camera video mode selection
269 switch(videoFormat_){
270 case 0:
271 switch(videoMode_){
272 case 0:
273 required_video_mode = DC1394_VIDEO_MODE_160x120_YUV444;
274 width_ = 160;
275 height_ = 120;
276 break;
277 case 1:
278 required_video_mode = DC1394_VIDEO_MODE_320x240_YUV422;
279 width_ = 320;
280 height_ = 240;
281 break;
282 case 2:
283 required_video_mode = DC1394_VIDEO_MODE_640x480_YUV411;
284 width_ = 640;
285 height_ = 480;
286 break;
287 case 3:
288 required_video_mode = DC1394_VIDEO_MODE_640x480_YUV422;
289 width_ = 640;
290 height_ = 480;
291 break;
292 case 4:
293 required_video_mode = DC1394_VIDEO_MODE_640x480_RGB8;
294 width_ = 640;
295 height_ = 480;
296 break;
297 case 5:
298 required_video_mode = DC1394_VIDEO_MODE_640x480_MONO8;
299 width_ = 640;
300 height_ = 480;
301 break;
302 case 6:
303 required_video_mode = DC1394_VIDEO_MODE_640x480_MONO16;
304 width_ = 640;
305 height_ = 480;
306 break;
307 default:
308 cout << "The selected video mode is not supported or available"<< endl;
309 return false;
310 break;
311 }
312 break;
313 case 1:
314 switch(videoMode_){
315 case 0:
316 required_video_mode = DC1394_VIDEO_MODE_800x600_YUV422;
317 width_ = 800;
318 height_ = 600;
319 break;
320 case 1:
321 required_video_mode = DC1394_VIDEO_MODE_800x600_RGB8;
322 width_ = 800;
323 height_ = 600;
324 break;
325 case 2:
326 required_video_mode = DC1394_VIDEO_MODE_800x600_MONO8;
327 width_ = 800;
328 height_ = 600;
329 break;
330 case 3:
331 required_video_mode = DC1394_VIDEO_MODE_1024x768_YUV422;
332 width_ = 1024;
333 height_ = 768;
334 break;
335 case 4:
336 required_video_mode = DC1394_VIDEO_MODE_1024x768_RGB8;
337 width_ = 1024;
338 height_ = 768;
339 break;
340 case 5:
341 required_video_mode = DC1394_VIDEO_MODE_1024x768_MONO8;
342 width_ = 1024;
343 height_ = 768;
344 break;
345 case 6:
346 required_video_mode = DC1394_VIDEO_MODE_800x600_MONO16;
347 width_ = 800;
348 height_ = 600;
349 break;
350 case 7:
351 required_video_mode = DC1394_VIDEO_MODE_1024x768_MONO16;
352 width_ = 1024;
353 height_ = 768;
354 break;
355 default:
356 cout << "The selected video mode is not supported or available"<< endl;
357 printSupportedFormats =true;
358 break;
359 }
360 break;
361 case 2:
362 switch(videoMode_){
363 case 0:
364 required_video_mode = DC1394_VIDEO_MODE_1280x960_YUV422;
365 width_ = 1280;
366 height_ = 960;
367 break;
368 case 1:
369 required_video_mode = DC1394_VIDEO_MODE_1280x960_RGB8;
370 width_ = 1280;
371 height_ = 960;
372 break;
373 case 2:
374 required_video_mode = DC1394_VIDEO_MODE_1280x960_MONO8;
375 width_ = 1280;
376 height_ = 960;
377 break;
378 case 6:
379 required_video_mode = DC1394_VIDEO_MODE_1280x960_MONO16;
380 width_ = 1280;
381 height_ = 960;
382 break;
383 default:
384 cout << "The selected video mode is not supported or available"<< endl;
385 printSupportedFormats =true;
386 break;
387 }
388 break;
389 default:
390 cout << "The selected format is not supported or available"<< endl;
391 printSupportedFormats =true;
392 break;
393 }
394
395 //looking for selected mode on camera
396 for ( int i = video_modes.num-1; i >= 0; i-- )
397 {
398 // don't consider FORMAT 7 modes (i.e. "scalable")
399 if ( !dc1394_is_video_mode_scalable( video_modes.modes[i] ) )
400 {
401 dc1394_get_color_coding_from_video_mode( theCamera, video_modes.modes[i], &coding );
402 if(required_video_mode==video_modes.modes[i]){
403 foundFormatVideoMode=true;
404 theCamera_video_mode=required_video_mode;
405 cout << "Correct format/video mode." << endl;
406 }
407 }
408 }
409 if (!foundFormatVideoMode)
410 printSupportedFormats=true;
411
412 if (printSupportedFormats){
413 cout << "Error, only the followng format/video modes are supported: " << endl;
414 //Supported modes
415 for ( int i = video_modes.num-1; i >= 0; i-- )
416 {
417 // don't consider FORMAT 7 modes (i.e. "scalable")
418 if ( !dc1394_is_video_mode_scalable( video_modes.modes[i] ) )
419 {
420 dc1394_get_color_coding_from_video_mode( theCamera, video_modes.modes[i], &coding );
421
422 switch(video_modes.modes[i])
423 {
424 case (DC1394_VIDEO_MODE_160x120_YUV444):
425 cout << "DC1394_VIDEO_MODE_160x120_YUV444" << endl;
426 break;
427 case (DC1394_VIDEO_MODE_320x240_YUV422):
428 cout << "DC1394_VIDEO_MODE_320x240_YUV422 " << endl;
429 break;
430 case (DC1394_VIDEO_MODE_640x480_YUV411):
431 cout << "DC1394_VIDEO_MODE_640x480_YUV411 " << endl;
432 break;
433 case (DC1394_VIDEO_MODE_640x480_YUV422):
434 cout << "DC1394_VIDEO_MODE_640x480_YUV422 " << endl;
435 break;
436 case (DC1394_VIDEO_MODE_640x480_RGB8):
437 cout << "DC1394_VIDEO_MODE_640x480_RGB8 " << endl;
438 break;
439 case (DC1394_VIDEO_MODE_640x480_MONO8):
440 cout << "DC1394_VIDEO_MODE_640x480_MONO8 " << endl;
441 break;
442 case (DC1394_VIDEO_MODE_640x480_MONO16):
443 cout << "DC1394_VIDEO_MODE_640x480_MONO16 " << endl;
444 break;
445 case (DC1394_VIDEO_MODE_800x600_YUV422):
446 cout << "DC1394_VIDEO_MODE_800x600_YUV422 " << endl;
447 break;
448 case (DC1394_VIDEO_MODE_800x600_RGB8):
449 cout << "DC1394_VIDEO_MODE_800x600_RGB8 " << endl;
450 break;
451 case (DC1394_VIDEO_MODE_800x600_MONO8):
452 cout << "DC1394_VIDEO_MODE_800x600_MONO8 " << endl;
453 break;
454 case (DC1394_VIDEO_MODE_1024x768_YUV422):
455 cout << "DC1394_VIDEO_MODE_1024x768_YUV422 " << endl;
456 break;
457 case (DC1394_VIDEO_MODE_1024x768_RGB8):
458 cout << "DC1394_VIDEO_MODE_1024x768_RGB8 " << endl;
459 break;
460 case (DC1394_VIDEO_MODE_1024x768_MONO8):
461 cout << "DC1394_VIDEO_MODE_1024x768_MONO8 " << endl;
462 break;
463 case (DC1394_VIDEO_MODE_800x600_MONO16):
464 cout << "DC1394_VIDEO_MODE_800x600_MONO16 " << endl;
465 break;
466 case (DC1394_VIDEO_MODE_1024x768_MONO16):
467 cout << "DC1394_VIDEO_MODE_1024x768_MONO16 " << endl;
468 break;
469 case (DC1394_VIDEO_MODE_1280x960_YUV422):
470 cout << "DC1394_VIDEO_MODE_1280x960_YUV422 " << endl;
471 break;
472 case (DC1394_VIDEO_MODE_1280x960_RGB8):
473 cout << "DC1394_VIDEO_MODE_1280x960_RGB8 " << endl;
474 break;
475 case (DC1394_VIDEO_MODE_1280x960_MONO8):
476 cout << "DC1394_VIDEO_MODE_1280x960_MONO8 " << endl;
477 break;
478 case (DC1394_VIDEO_MODE_1280x960_MONO16):
479 cout << "DC1394_VIDEO_MODE_1280x960_MONO16 " << endl;
480 break;
481
482 }
483 }
484
485 }
486 return false;
487 }
488
489 if(recording_){
490 char commandline[64];
491 sprintf(commandline,"mkdir %s\n",componentName.toLatin1().data());
492 system(commandline);
493 puts(commandline);
494 }
495
496 // FIXME: uncomplete type of d: dc1394_t
497 delete d;
498 delete list;
499 return true;
500}
501
502bool Camera1394::recordImage(QString format)
503{
504 imageCounter_=imageCounter_+1;
505 bool formatSupported = false;
506
507 format = format.toLower();
508
509 if (format == "jpeg")
510 format = "jpg";
511
512 QImageWriter imageWriter;
513 if( imageWriter.supportedImageFormats().contains(format.toLatin1()) )
514 formatSupported = true;
515
516 if (formatSupported == false) {
517 LOG_ERROR(componentName << "Format" << format << "not supported for recording");
518 } else {
519 // write in the dbt file
520 stringstream imageNameSs;
521 imageNameSs << componentName.toStdString();
522 imageNameSs << "/" << imageCounter_ << "." << format.toStdString();
523 const char * imageName = imageNameSs.str().c_str();
524 imageWriter.setFileName(imageName);
525 imageWriter.setFormat( format.toLatin1() );
526 imageWriter.write(*qimage_);
527 file_.writeRecord(time_, tr_, imageName, MAX_CHAR_DIRECTORY_NAME+MAX_CHAR_PICTURE_NAME);
528 }
529 return formatSupported;
530}
531
532//#define MEASURE_TIME
533
534//////////////////////////////////////////////////////////////////////////
535// Main loop for acquisition
536//////////////////////////////////////////////////////////////////////////
537void Camera1394::run()
538{
539 dc1394error_t err;
540
541 bool firstTime = true;
542 ShMem * shMem;
543 imageCounter_ = 0;
544 time_ = road_time();
545
546 road_time_t t;
547
548 while (acquiring_)
549 {
550 err = dc1394_capture_dequeue( theCamera, DC1394_CAPTURE_POLICY_WAIT, &theFrame );//DC1394_CAPTURE_POLICY_POLL
551 if ( err != DC1394_SUCCESS ){
552 fprintf( stderr, "Camera acquisition: Cannot dequeue image!\n" );
553 return;
554 }
555
556 prevTime_ = time_;
557 time_ = road_time();
558
559 // verify that the acquisition period is good relatively to the selected format
560 if ( isPeriodGood((int)(time_ - prevTime_)) )
561 setState(MONITOR_OK);
562 else
563 setState(MONITOR_NOK);
564
565 t = road_time();
566 if ( !fillRGB32Image(qimage_->bits()) ) continue;
567
568 // return buffer for use
569 dc1394_capture_enqueue( theCamera, theFrame );
570
571#ifdef MEASURE_TIME
572 LOG_INFO(componentName << "duration conversion: " << static_cast<int64_t>(road_time() - t) ;
573#endif
574
575 imageMutex_->lock();
576
577 t = road_time();
578 if (reverseImage_)
579 *qimage_ = qimage_->mirrored(true,true);
580
581 // send image in shared memory
582 if (firstTime)
583 {
584 shMem = new ShMem("IMAGE", qimage_->byteCount());
585 firstTime = false;
586 }
587 shMem->write(qimage_->bits(), qimage_->byteCount());
588
589#ifdef MEASURE_TIME
590 LOG_INFO(componentName << "duration mirror: " << static_cast<int64_t>(road_time() - t) ;
591#endif
592
593 imageMutex_->unlock();
594 emit newImage(qimage_);
595
596 t = road_time();
597 PacpusImage data;
598 data.time = t;
599 data.image = *qimage_;
600 OutputInterface<PacpusImage,Camera1394> * out = static_cast<OutputInterface<PacpusImage,Camera1394> *> (output.value("image"));
601 out->send(data);
602
603#ifdef MEASURE_TIME
604 LOG_INFO(componentName << "duration mirror: " << static_cast<int64_t>(road_time() - t) ;
605#endif
606
607 t = road_time();
608 if (recording_)
609 {
610 recordImage(recordFormat_);
611 }
612#ifdef MEASURE_TIME
613 LOG_INFO(componentName << "duration record: " << static_cast<int64_t>(road_time() - t));
614#endif
615
616 } // END while (acquiring_)
617
618 LOG_INFO(componentName << ":Stopping the camera");
619
620 dc1394_capture_stop(theCamera);
621 dc1394_video_set_transmission(theCamera,DC1394_OFF);
622 dc1394_camera_free(theCamera);
623
624 //if (theCamera->StopImageAcquisition() != CAM_SUCCESS)
625 // qFatal("Problem Stopping Image Acquisition");
626
627 if (!firstTime)
628 delete shMem;
629 setState(STOPPED);
630}
631
632//////////////////////////////////////////////////////////////////////////
633// return the actual period in µs corresponding to the framerate code
634//////////////////////////////////////////////////////////////////////////
635double Camera1394::getPeriod()
636{
637 switch(frameRate_) {
638 case 0:
639 return 1.0e6/1.875;
640 break;
641
642 case 1:
643 return 1.0e6/3.75;
644 break;
645
646 case 2:
647 return 1.0e6/7.5;
648 break;
649
650 case 3:
651 return 1.0e6/15;
652 break;
653
654 case 4:
655 return 1.0e6/30;
656 break;
657
658 case 5:
659 return 1.0e6/60;
660 break;
661
662 default:
663 LOG_WARN("Unknown framerate. The component " << componentName << " may encounter a problem !");
664 return 0;
665 }
666}
667
668//////////////////////////////////////////////////////////////////////////
669// retrn true if the period T corresponds to the framerate (at +/-20%)
670// T is given in microseconds
671//////////////////////////////////////////////////////////////////////////
672bool Camera1394::isPeriodGood(int T)
673{
674 float period = getPeriod();
675 if ( (T < period*0.8) || (T > period*1.2) )
676 return false;
677 else
678 return true;
679}
680
681//////////////////////////////////////////////////////////////////////////
682// useful macro to keep 'a' between 0 and 255
683//////////////////////////////////////////////////////////////////////////
684#define CLAMP_TO_UCHAR(a) (unsigned char)((a) < 0 ? 0 : ((a) > 255 ? 255 : (a)))
685
686//////////////////////////////////////////////////////////////////////////
687// Convert the image from YUV444 to RGB32
688// The image is stored using a 32-bit RGB format (0xffRRGGBB)
689//////////////////////////////////////////////////////////////////////////
690void Camera1394::YUV444toRGB32(unsigned char* buffer)
691{
692 long Y, U, V, deltaG;
693 unsigned char *srcptr, *srcend, *destptr;
694
695 // single-stage idiotproofing
696// if(theCamera->m_pData == NULL || buffer == NULL)
697// return;
698
699 srcptr = theFrame->image;
700 srcend = srcptr + (width_ * height_ * 3);
701 destptr = buffer;
702
703 // data pattern: UYV
704 // unroll it to 4 pixels/round
705
706 while(srcptr < srcend)
707 {
708 U = (*srcptr++) - 128;
709 Y = (*srcptr++);
710 V = (*srcptr++) - 128;
711
712 deltaG = (12727 * U + 33384 * V);
713 deltaG += (deltaG > 0 ? 32768 : -32768);
714 deltaG >>= 16;
715
716 *destptr++ = CLAMP_TO_UCHAR( Y + U );
717 *destptr++ = CLAMP_TO_UCHAR( Y - deltaG );
718 *destptr++ = CLAMP_TO_UCHAR( Y + V );
719 destptr++;//A
720
721 U = (*srcptr++) - 128;
722 Y = (*srcptr++);
723 V = (*srcptr++) - 128;
724
725 deltaG = (12727 * U + 33384 * V);
726 deltaG += (deltaG > 0 ? 32768 : -32768);
727 deltaG >>= 16;
728
729 *destptr++ = CLAMP_TO_UCHAR( Y + U );
730 *destptr++ = CLAMP_TO_UCHAR( Y - deltaG );
731 *destptr++ = CLAMP_TO_UCHAR( Y + V );
732 destptr++;//A
733
734 U = (*srcptr++) - 128;
735 Y = (*srcptr++);
736 V = (*srcptr++) - 128;
737
738 deltaG = (12727 * U + 33384 * V);
739 deltaG += (deltaG > 0 ? 32768 : -32768);
740 deltaG >>= 16;
741
742 *destptr++ = CLAMP_TO_UCHAR( Y + U );
743 *destptr++ = CLAMP_TO_UCHAR( Y - deltaG );
744 *destptr++ = CLAMP_TO_UCHAR( Y + V );
745 destptr++;//A
746
747 U = (*srcptr++) - 128;
748 Y = (*srcptr++);
749 V = (*srcptr++) - 128;
750
751 deltaG = (12727 * U + 33384 * V);
752 deltaG += (deltaG > 0 ? 32768 : -32768);
753 deltaG >>= 16;
754
755 *destptr++ = CLAMP_TO_UCHAR( Y + U );
756 *destptr++ = CLAMP_TO_UCHAR( Y - deltaG );
757 *destptr++ = CLAMP_TO_UCHAR( Y + V );
758 destptr++;//A
759 }
760}
761
762//////////////////////////////////////////////////////////////////////////
763// Convert the image from YUV422 to RGB32
764// The image is stored using a 32-bit RGB format (0xffRRGGBB)
765//////////////////////////////////////////////////////////////////////////
766void Camera1394::YUV422toRGB32(unsigned char *buffer)
767{
768 long Y, U, V, deltaG;
769 unsigned char *srcptr, *srcend, *destptr;
770
771 srcptr = theFrame->image;
772 srcend = srcptr + ((width_ * height_) << 1);
773 destptr = buffer;
774
775 // single-stage idiotproofing
776// if(theCamera->m_pData == NULL || buffer == NULL)
777// {
778// return;
779// }
780
781 // data pattern: UYVY
782
783 while(srcptr < srcend)
784 {
785 U = *srcptr;
786 U -= 128;
787 V = *(srcptr+2);
788 V -= 128;
789
790 deltaG = (12727 * U + 33384 * V);
791 deltaG += (deltaG > 0 ? 32768 : -32768);
792 deltaG >>= 16;
793
794 Y = *(srcptr + 1);
795 *destptr++ = CLAMP_TO_UCHAR( Y + U );
796 *destptr++ = CLAMP_TO_UCHAR( Y - deltaG );
797 *destptr++ = CLAMP_TO_UCHAR( Y + V );
798 destptr++;//A
799
800
801 Y = *(srcptr + 3);
802 *destptr++ = CLAMP_TO_UCHAR( Y + U );
803 *destptr++ = CLAMP_TO_UCHAR( Y - deltaG );
804 *destptr++ = CLAMP_TO_UCHAR( Y + V );
805 destptr++;//A
806
807 srcptr += 4;
808
809 // twice in the same loop... just like halving the loop overhead
810
811 U = (*srcptr) - 128;
812 V = (*(srcptr+2)) - 128;
813
814 deltaG = (12727 * U + 33384 * V);
815 deltaG += (deltaG > 0 ? 32768 : -32768);
816 deltaG >>= 16;
817
818 Y = *(srcptr + 1);
819 *destptr++ = CLAMP_TO_UCHAR( Y + U );
820 *destptr++ = CLAMP_TO_UCHAR( Y - deltaG );
821 *destptr++ = CLAMP_TO_UCHAR( Y + V );
822 destptr++;//A
823
824 Y = *(srcptr + 3);
825 *destptr++ = CLAMP_TO_UCHAR( Y + U );
826 *destptr++ = CLAMP_TO_UCHAR( Y - deltaG );
827 *destptr++ = CLAMP_TO_UCHAR( Y + V );
828 destptr++;//A
829
830 srcptr += 4;
831
832 }
833}
834
835//////////////////////////////////////////////////////////////////////////
836// Convert the image from YUV411 to RGB32
837// The image is stored using a 32-bit RGB format (0xffRRGGBB)
838//////////////////////////////////////////////////////////////////////////
839void Camera1394::YUV411toRGB32(unsigned char *buffer)
840{
841 long Y, U, V, deltaG;
842 unsigned char *srcptr, *srcend, *destptr;
843
844 // single-stage idiotproofing
845// if(theCamera->m_pData == NULL || buffer == NULL)
846// return;
847
848 // data pattern: UYYVYY
849
850 srcptr = theFrame->image;
851 srcend = srcptr + ((width_ * height_ * 3) >> 1);
852 destptr = buffer;
853
854 while(srcptr < srcend)
855 {
856 U = (*srcptr) - 128;
857 V = (*(srcptr+3)) - 128;
858
859 deltaG = (12727 * U + 33384 * V);
860 deltaG += (deltaG > 0 ? 32768 : -32768);
861 deltaG >>= 16;
862
863 Y = *(srcptr + 1);
864 *destptr++ = CLAMP_TO_UCHAR( Y + U );
865 *destptr++ = CLAMP_TO_UCHAR( Y - deltaG );
866 *destptr++ = CLAMP_TO_UCHAR( Y + V );
867 destptr++;//A
868
869 Y = *(srcptr + 2);
870 *destptr++ = CLAMP_TO_UCHAR( Y + U );
871 *destptr++ = CLAMP_TO_UCHAR( Y - deltaG );
872 *destptr++ = CLAMP_TO_UCHAR( Y + V );
873 destptr++;//A
874
875 Y = *(srcptr + 4);
876 *destptr++ = CLAMP_TO_UCHAR( Y + U );
877 *destptr++ = CLAMP_TO_UCHAR( Y - deltaG );
878 *destptr++ = CLAMP_TO_UCHAR( Y + V );
879 destptr++;//A
880
881 Y = *(srcptr + 5);
882 *destptr++ = CLAMP_TO_UCHAR( Y + U );
883 *destptr++ = CLAMP_TO_UCHAR( Y - deltaG );
884 *destptr++ = CLAMP_TO_UCHAR( Y + V );
885 destptr++;//A
886
887 srcptr += 6;
888 }
889}
890
891//////////////////////////////////////////////////////////////////////////
892// Convert the image from Y to RGB32
893// The image is stored using a 32-bit RGB format (0xffRRGGBB)
894//////////////////////////////////////////////////////////////////////////
895void Camera1394::YtoRGB32(unsigned char * buffer)
896{
897 unsigned char *srcptr, *srcend, *destptr;
898
899 srcptr = theFrame->image;
900 srcend = srcptr + (width_ * height_);
901 destptr = buffer;
902
903 // single-stage idiotproofing
904// if(theCamera->m_pData == NULL || buffer == NULL)
905// return;
906
907 // just Y's (monochrome)
908
909 // unroll it to 4 per cycle
910
911 while(srcptr < srcend)
912 {
913 *destptr++ = *srcptr;
914 *destptr++ = *srcptr;
915 *destptr++ = *srcptr;
916 destptr++;//A
917 srcptr++;
918
919 *destptr++ = *srcptr;
920 *destptr++ = *srcptr;
921 *destptr++ = *srcptr;
922 destptr++;//A
923 srcptr++;
924
925 *destptr++ = *srcptr;
926 *destptr++ = *srcptr;
927 *destptr++ = *srcptr;
928 destptr++;//A
929 srcptr++;
930
931 *destptr++ = *srcptr;
932 *destptr++ = *srcptr;
933 *destptr++ = *srcptr;
934 destptr++;//A
935 srcptr++;
936 }
937}
938
939//////////////////////////////////////////////////////////////////////////
940// Convert the image from Y16 to RGB32
941// The image is stored using a 32-bit RGB format (0xffRRGGBB)
942// 16-bit monochrome (new to spec 1.3)
943// the first of each pair of bytes is the high byte, so just copy those to RGB
944//////////////////////////////////////////////////////////////////////////
945void Camera1394::Y16toRGB32(unsigned char *buffer)
946{
947 unsigned char *srcptr, *srcend, *destptr;
948
949 srcptr = theFrame->image;
950 srcend = srcptr + 2 * (width_ * height_);
951 destptr = buffer;
952
953 // single-stage idiotproofing
954// if(theCamera->m_pData == NULL || buffer == NULL)
955// return;
956
957 // just Y's (monochrome, 16-bit little endian)
958
959 // unroll it to 4 per cycle
960
961 while(srcptr < srcend)
962 {
963 *destptr++ = *srcptr;
964 *destptr++ = *srcptr;
965 *destptr++ = *srcptr;
966 destptr++;//A
967 srcptr += 2;
968
969 *destptr++ = *srcptr;
970 *destptr++ = *srcptr;
971 *destptr++ = *srcptr;
972 destptr++;//A
973 srcptr += 2;
974
975 *destptr++ = *srcptr;
976 *destptr++ = *srcptr;
977 *destptr++ = *srcptr;
978 destptr++;//A
979 srcptr += 2;
980
981 *destptr++ = *srcptr;
982 *destptr++ = *srcptr;
983 *destptr++ = *srcptr;
984 destptr++;//A
985 srcptr += 2;
986 }
987}
988
989//////////////////////////////////////////////////////////////////////////
990// Convert the image from RGB16 to RGB32
991// The image is stored using a 32-bit RGB format (0xffRRGGBB)
992// 16-bit RGB (new to spec 1.3)
993// the first of each pair of bytes is the high byte, so just copy those to RGB
994// near duplicate of Y16toRGB
995//////////////////////////////////////////////////////////////////////////
996void Camera1394::RGB16toRGB32(unsigned char * buffer)
997{
998 unsigned char *srcptr, *srcend, *destptr;
999
1000 srcptr = theFrame->image;
1001 srcend = srcptr + 6 * (width_ * height_);
1002 destptr = buffer;
1003
1004 // single-stage idiotproofing
1005// if(theCamera->m_pData == NULL || buffer == NULL)
1006// return;
1007
1008 // R,G,B are 16-bit source, chop of the top 8 and feed
1009
1010 // unroll it to 3 per cycle
1011
1012 while(srcptr < srcend)
1013 {
1014 *destptr++ = *srcptr;
1015 *destptr++ = *srcptr;
1016 *destptr++ = *srcptr;
1017 destptr++;//A
1018 srcptr += 2;
1019
1020 *destptr++ = *srcptr;
1021 *destptr++ = *srcptr;
1022 *destptr++ = *srcptr;
1023 destptr++;//A
1024 srcptr += 2;
1025
1026 *destptr++ = *srcptr;
1027 *destptr++ = *srcptr;
1028 *destptr++ = *srcptr;
1029 destptr++;//A
1030 srcptr += 2;
1031 }
1032}
1033
1034
1035
1036//////////////////////////////////////////////////////////////////////////
1037// Convert the image from RGB24 to RGB32
1038// The image is stored using a 32-bit RGB format (0xffRRGGBB)
1039//////////////////////////////////////////////////////////////////////////
1040void Camera1394::RGB24toRGB32(unsigned char * buffer)
1041{
1042 unsigned char *srcptr, *srcend, *destptr;
1043
1044 srcptr = theFrame->image;
1045 srcend = srcptr + 3 * (width_ * height_);
1046 destptr = buffer;
1047
1048 // single-stage idiotproofing
1049// if(theCamera->m_pData == NULL || buffer == NULL)
1050// return;
1051
1052 // unroll it to 3 per cycle
1053
1054 while(srcptr < srcend)
1055 {
1056 *destptr++ = *(srcptr+2); //B
1057 *destptr++ = *(srcptr+1); //G
1058 *destptr++ = *srcptr; //R
1059 destptr++;//A
1060 srcptr += 3;
1061
1062 *destptr++ = *(srcptr+2);
1063 *destptr++ = *(srcptr+1);
1064 *destptr++ = *srcptr;
1065 destptr++;//A
1066 srcptr += 3;
1067
1068 *destptr++ = *(srcptr+2);
1069 *destptr++ = *(srcptr+1);
1070 *destptr++ = *srcptr;
1071 destptr++;//A
1072 srcptr += 3;
1073 }
1074}
1075
1076
1077bool Camera1394::fillRGB32Image(unsigned char * buffer)
1078{
1079 if(buffer == NULL)
1080 return false;
1081
1082 bool retValue = true;
1083
1084 switch(videoFormat_)
1085 {
1086 case 0:
1087 switch(videoMode_)
1088 {
1089 case 0:
1090 // 160x120 YUV444
1091 YUV444toRGB32(buffer);
1092 break;
1093 case 1:
1094 // 320x240 YUV222
1095 YUV422toRGB32(buffer);
1096 break;
1097 case 2:
1098 // 640x480 YUV411
1099 YUV411toRGB32(buffer);
1100 break;
1101 case 3:
1102 // 640x480 YUV422
1103 YUV422toRGB32(buffer);
1104 break;
1105 case 4:
1106 // 640x480 RGB
1107 RGB24toRGB32(buffer);
1108 //memcpy(buffer,theCamera->m_pData,640 * 480 * 3);
1109 break;
1110 case 5:
1111 // 640x480 MONO
1112 YtoRGB32(buffer);
1113 break;
1114 case 6:
1115 // 640x480 MONO16
1116 Y16toRGB32(buffer);
1117 break;
1118 default:
1119 retValue = false;
1120 qWarning("Current video mode is not supported for display and recording");
1121 break;
1122 } break;
1123
1124 case 1:
1125 switch(videoMode_)
1126 {
1127 case 0:
1128 // 800x600 YUV422
1129 YUV422toRGB32(buffer);
1130 break;
1131 case 1:
1132 // 800x600 RGB
1133 //memcpy(buffer,theCamera->m_pData,800 * 600 * 3);
1134 break;
1135 case 2:
1136 // 800x600 MONO
1137 YtoRGB32(buffer);
1138 break;
1139 case 3:
1140 // 1024x768 YUV422
1141 YUV422toRGB32(buffer);
1142 break;
1143 case 4:
1144 // 1024x768 RGB
1145 //memcpy(buffer,theCamera->m_pData,1024 * 768 * 3);
1146 break;
1147 case 5:
1148 // 1024x768 MONO
1149 YtoRGB32(buffer);
1150 break;
1151 case 6:
1152 // 800x600 MONO16
1153 case 7:
1154 // 1024x768 MONO16
1155 Y16toRGB32(buffer);
1156 break;
1157 default:
1158 retValue = false;
1159 qWarning("Current video mode is not supported for display and recording");
1160 break;
1161 } break;
1162
1163 case 2:
1164 switch(videoMode_)
1165 {
1166 case 0:
1167 // 1280x960 YUV422
1168 YUV422toRGB32(buffer);
1169 break;
1170 case 1:
1171 // 1280x960 RGB
1172 //memcpy(buffer,theCamera->m_pData,1280 * 960 * 3);
1173 break;
1174 case 2:
1175 // 1280x960 MONO
1176 YtoRGB32(buffer);
1177 break;
1178 case 3:
1179 // 1600x1200 YUV422
1180 YUV422toRGB32(buffer);
1181 break;
1182 case 4:
1183 // 1600x1200 RGB
1184 //memcpy(buffer,theCamera->m_pData,1600 * 1200 * 3);
1185 break;
1186 case 5:
1187 // 1600x1200 MONO
1188 YtoRGB32(buffer);
1189 break;
1190 case 6:
1191 // 1280x1024 MONO16
1192 case 7:
1193 // 1600x1200 MONO16
1194 Y16toRGB32(buffer);
1195 break;
1196 default:
1197 retValue = false;
1198 qWarning("Current video mode is not supported for display and recording");
1199 break;
1200 } break;
1201
1202 case 7:
1203// switch(theCamera->m_controlSize.m_colorCode)
1204// {
1205// case 0:
1206// // Mono8
1207// YtoRGB32(buffer);
1208// break;
1209// case 1:
1210// // YUV 411
1211// YUV411toRGB32(buffer);
1212// break;
1213// case 2:
1214// // YUV 422
1215// YUV422toRGB32(buffer);
1216// break;
1217// case 3:
1218// // YUV 444
1219// YUV444toRGB32(buffer);
1220// break;
1221// case 4:
1222// // RGB8
1223// memcpy(buffer,theCamera->m_pData,1280 * 960 * 3);
1224// break;
1225// case 5:
1226// // Mono16
1227// Y16toRGB32(buffer);
1228// break;
1229// case 6:
1230// // RGB16
1231// RGB16toRGB32(buffer);
1232// break;
1233// default:
1234// // unsupported
1235// retValue = false;
1236// qWarning("Current video mode is not supported for display and recording");
1237// break;
1238// }
1239 break;
1240
1241 default:
1242 retValue = false;
1243 qWarning("Current video mode is not supported for display and recording");
1244 break;
1245 }
1246
1247 return retValue;
1248}
1249
1250} // namespace pacpus
Note: See TracBrowser for help on using the repository browser.