// camera.cc // buzz - Programme du robot Efrei Robotique I1-I2 2003 // Copyright (C) 2003 Nicolas Schodet // #include "camera.h" #include "erreur/erreur.h" #include "kernel/pbus.h" #include "config/config.h" #include "date/date.h" #include #include #include // Debug. #include // #include #define CAM_FILE "/dev/robotcam" #define CAMERA_DEBUG // Constructeur. Camera::Camera () { // Taille par défaut. m_w = 352; m_h = 288; m_d = 1; m_cut = 0; // Ouvre le périphérique. m_fd = open (CAM_FILE, O_RDONLY); if (m_fd == -1) throw ErreurFatale ("Impossible d'ouvrir le périphérique de la" " camera.\n"); // Paramètre la camera. sccb_io param; Config rc ("rc/camera"); int addr, data; while (!rc.eof ()) { if (rc.isId ("width")) { rc.getId (); m_w = rc.getNum (); #ifdef CAMERA_DEBUG cout << "camera width " << m_w << endl; #endif // CAMERA_DEBUG } else if (rc.isId ("height")) { rc.getId (); m_h = rc.getNum (); #ifdef CAMERA_DEBUG cout << "camera height " << m_h << endl; #endif // CAMERA_DEBUG } else if (rc.isId ("depth")) { rc.getId (); m_d = rc.getNum (); #ifdef CAMERA_DEBUG cout << "camera depth " << m_d << endl; #endif // CAMERA_DEBUG } else if (rc.isId ("cut")) { rc.getId (); m_cut = rc.getNum (); #ifdef CAMERA_DEBUG cout << "camera cut " << m_cut << endl; #endif // CAMERA_DEBUG } else if (rc.isId ("setup")) { rc.getId (); addr = rc.getNum (); data = rc.getNum (); param.addr = addr; param.data = data; #ifdef CAMERA_DEBUG cout << "camera sccbwrite 0x" << hex << addr << " 0x" << data << endl; #endif // CAMERA_DEBUG ioctl (m_fd, CAM_SCCBWRITE, ¶m); } else rc.noId (); } // Affiche les paramètres. #ifdef CAMERA_DEBUG cout << hex << "camera sccbdump"; for (addr = 0; addr < 0x50; ++addr) { if (!(addr % 16)) cout << endl; param.addr = addr; ioctl (m_fd, CAM_SCCBREAD, ¶m); data = param.data; cout << setw (2) << setfill ('0') << data << ' '; } cout << endl << dec; #endif // CAMERA_DEBUG m_lastRead = 0; // Paramètre la taille de frame. int hcount = m_w * m_d; ioctl (m_fd, CAM_SETHCOUNT, &hcount); m_frameSize = m_w * m_h * m_d + m_cut; ioctl (m_fd, CAM_SETFRAMESIZE, &m_frameSize); } // Destructeur. Camera::~Camera () { // Ferme le périphérique de camera. close (m_fd); } // Lit une image. int Camera::read (unsigned char *image) { int r; int t = Date::getInstance ().start (); // Attention, pas trops vite. if (m_lastRead + 100 > t) return 0; m_lastRead = t; // Lit les données sur la camera. if (m_cut) r = ::read (m_fd, image, m_cut); if (!m_cut || r) r = ::read (m_fd, image, m_frameSize - m_cut); #ifdef CAMERA_DEBUG cout << "camera read " << r << endl; #endif // CAMERA_DEBUG return r; } // Lit une image en RGB. int Camera::readRGB (unsigned char *rgb) { int ret; int t = Date::getInstance ().start (); // Attention, pas trops vite. if (m_lastRead + 100 > t) return 0; m_lastRead = t; // Alloue de la mémoire. unsigned char *image = new unsigned char[m_frameSize]; // Lit les données sur la camera. ret = ::read (m_fd, image, m_frameSize); #ifdef CAMERA_DEBUG cout << "camera read " << ret << endl; #endif // CAMERA_DEBUG if (!ret) return 0; // Positions de débuts. unsigned char *r, *g, *b; r = image + m_cut + 2; g = image + m_cut + 1; b = image + m_cut; // Décode les couleurs. for (int i = 0; i < m_w * m_h; ++i) { *rgb++ = *r; r += 4; *rgb++ = *g; g += 4; *rgb++ = *b; b += 4; } delete image; return ret - m_cut; }