v4r_uvc.cpp
Go to the documentation of this file.
00001 /***************************************************************************
00002  *   Copyright (C) 2012 by Markus Bader                                    *
00003  *   markus.bader@tuwien.ac.at                                             *
00004  *                                                                         *
00005  *   This program is free software; you can redistribute it and/or modify  *
00006  *   it under the terms of the GNU General Public License as published by  *
00007  *   the Free Software Foundation; either version 2 of the License, or     *
00008  *   (at your option) any later version.                                   *
00009  *                                                                         *
00010  *   This program is distributed in the hope that it will be useful,       *
00011  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
00012  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
00013  *   GNU General Public License for more details.                          *
00014  *                                                                         *
00015  *   You should have received a copy of the GNU General Public License     *
00016  *   along with this program; if not, write to the                         *
00017  *   Free Software Foundation, Inc.,                                       *
00018  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
00019  ***************************************************************************/
00020 
00021 
00022 #include <iomanip>
00023 #include <v4r_uvc/v4r_uvc_defaults.h>
00024 #include <v4r_uvc/v4r_uvc.h>
00025 #include <boost/algorithm/string.hpp>
00026 
00027 extern "C" {
00028 #include <libv4l2.h>
00029 #include "luvcview/v4l2uvc.h"
00030 #include "luvcview/color.h"
00031 #include <linux/videodev2.h>
00032 }
00033 
00034 /* Fixed point arithmetic */
00035 #define FIXED Sint32
00036 #define FIXED_BITS 16
00037 #define TO_FIXED(X) (((Sint32)(X))<<(FIXED_BITS))
00038 #define FROM_FIXED(X) (((Sint32)(X))>>(FIXED_BITS))
00039 
00040 
00041 #define INCPANTILT 64 // 1°
00042 
00043 
00044 #include <boost/interprocess/sync/scoped_lock.hpp>
00045 typedef boost::interprocess::scoped_lock<boost::interprocess::interprocess_mutex> Lock;
00046 
00047 //static const char version[] = VERSION;
00048 
00049 
00050 
00051 char V4RCam::removeNonAlNum(char in)
00052 {
00053     if(in == ' ') return '_';
00054     if(!isalnum(in)) return '_';
00055     return in;
00056 }
00057 
00058 V4RCam::~V4RCam()
00059 {
00060     if(pVideoIn_) {
00061         close_v4l2(pVideoIn_);
00062         free(pVideoIn_);
00063     }
00064     freeLut();
00065 }
00066 
00067 V4RCam::V4RCam()
00068     : pVideoIn_(NULL)
00069     , videoDevice_(DEFAULT_VIDEODEVICE)
00070     , aviFilename_(DEFAULT_AVIFILENAME)
00071     , format_(DEFAULT_FORMAT)
00072     , grabmethod_(DEFAULT_GRABMETHODE)
00073     , width_(DEFAULT_WIDTH)
00074     , height_(DEFAULT_HEIGHT)
00075     , fps_(DEFAULT_FPS)
00076 {
00077     mutexImage_.unlock();
00078 }
00079 
00080 V4RCam::FD V4RCam::initCamera(const std::string &videoDevice)
00081 {
00082     if(!videoDevice.empty()) {
00083         videoDevice_ = videoDevice;
00084     }
00085     pVideoIn_ = (struct vdIn *) calloc(1, sizeof(struct vdIn));
00086     if(init_videoIn(pVideoIn_, (char *) videoDevice_.c_str(), width_, height_, fps_, format_, grabmethod_, (char *) aviFilename_.c_str()) < 0)
00087         exit(1);
00088     boost::this_thread::sleep(boost::posix_time::milliseconds(100));
00089     if(uvcGrab(pVideoIn_) < 0) {
00090         printf("Error grabbing first image\n");
00091         return 0;
00092     }
00093     if((width_ != pVideoIn_->width) || (height_ != pVideoIn_->height) || (fps_ != pVideoIn_->fps)){
00094         width_ = pVideoIn_->width;
00095         height_ = pVideoIn_->height;
00096         fps_ = pVideoIn_->fps;
00097         printf("Error: image format not supported changed to: %ipix x %ipix @ %3.1fHz\n", width_, height_, fps_);
00098     }
00099     initLut();
00100     gettimeofday(&timeLastFrame_, NULL);
00101     return pVideoIn_->fd;
00102 }
00103 
00104 
00105 bool V4RCam::grab()
00106 {
00107 
00108     boost::this_thread::sleep(boost::posix_time::milliseconds(10));
00109     Lock myLock(mutexImage_);
00110     if(uvcGrab(pVideoIn_) < 0) {
00111         printf("Error grabbing\n");
00112         return false;
00113     }
00114     if(pVideoIn_->signalquit != 1) {
00115         printf("videoIn->signalquit\n");
00116         return false;
00117     }
00118     timeval now;
00119     gettimeofday(&now, NULL);
00120     durationLastFrame_ = (now.tv_sec - timeLastFrame_.tv_sec) * 1000.0;      // sec to ms
00121     durationLastFrame_ += (now.tv_usec - timeLastFrame_.tv_usec) / 1000.0;   // us to ms
00122     timeLastFrame_ = now;
00123 
00124     return true;
00125 }
00126 
00127 
00128 int V4RCam::v4lgetInfo(ControlEntryPtr entry)
00129 {
00130     Lock myLock(mutexImage_);
00131     if(entry->valid == false) {
00132         entry->error_msg << "v4lgetInfo not valid\n";
00133         return ERROR;
00134     }
00135     if((v4l2_ioctl(pVideoIn_->fd, VIDIOC_QUERYCTRL, entry->queryctrl)) < 0) {
00136         entry->valid = false;
00137         entry->error_msg << "v4l2_ioctl querycontrol error\n";
00138         return ERROR;
00139     } else if(entry->queryctrl->flags & V4L2_CTRL_FLAG_DISABLED) {
00140         entry->valid = false;
00141         entry->error_msg << "control disabled\n";
00142         return ERROR;
00143     } else if(entry->hasValidType() == false) {
00144         entry->valid = false;
00145         entry->error_msg << "unsupported type\n";
00146         return ERROR;
00147     }
00148     entry->varName = std::string((const char *) entry->queryctrl->name);
00149     boost::algorithm::to_lower(entry->varName);
00150 
00151     std::transform(entry->varName.begin(), entry->varName.end(), entry->varName.begin(), V4RCam::removeNonAlNum);
00152     boost::algorithm::trim_left_if(entry->varName, boost::algorithm::is_any_of("_"));
00153     boost::algorithm::trim_right_if(entry->varName, boost::algorithm::is_any_of("_"));
00154     boost::algorithm::replace_all(entry->varName, "___", "_");
00155     boost::algorithm::replace_all(entry->varName, "__", "_");
00156 
00157     return OK;
00158 }
00159 
00160 
00161 int V4RCam::v4lget(ControlEntryPtr entry)
00162 {
00163     Lock myLock(mutexImage_);
00164     if(entry->valid == false) {
00165         entry->error_msg << "v4lget not valid\n";
00166         return ERROR;
00167     }
00168     struct v4l2_control control;
00169     control.id = entry->queryctrl->id;
00170 
00171     if(v4l2_ioctl(pVideoIn_->fd, VIDIOC_G_CTRL, &control) < 0) {
00172         entry->error_msg <<  "v4l2_ioctl get control error\n";
00173         return ERROR;
00174     }
00175     entry->currentValue = control.value;
00176     boost::this_thread::sleep(boost::posix_time::milliseconds(10));
00177     entry->info_msg << "current = " << entry->currentValue << "\n";
00178     return OK;
00179 }
00180 
00181 int V4RCam::v4lset(ControlEntryPtr entry)
00182 {
00183     Lock myLock(mutexImage_);
00184     if(entry->valid == false) {
00185         entry->error_msg << "v4lset not valid\n";
00186         return ERROR;
00187     }
00188     struct v4l2_control control;
00189     control.id = entry->queryctrl->id;
00190     control.value = entry->targetValue;
00191     if(v4l2_ioctl(pVideoIn_->fd, VIDIOC_S_CTRL, &control) < 0) {
00192         entry->error_msg <<  "v4l2_ioctl set( " << control.value << ") control error\n";
00193         return ERROR;
00194     }
00195     boost::this_thread::sleep(boost::posix_time::milliseconds(10));
00196     entry->currentValue = entry->targetValue;
00197     entry->info_msg << "current = " << entry->currentValue << "\n";
00198     return OK;
00199 }
00200 int V4RCam::v4lupdate(ControlEntryPtr entry)
00201 {
00202     Lock myLock(mutexImage_);
00203     if(entry->currentValue == entry->targetValue) {
00204         return OK;
00205     }
00206     if(entry->valid == false) {
00207         entry->currentValue = entry->targetValue;
00208         entry->error_msg << "v4lupdate not valid\n";
00209         return ERROR;
00210     }
00211 
00212     if(entry->targetValue % entry->queryctrl->step) {
00213         entry->info_msg << "target value " << entry->targetValue << "between steps " <<  entry->queryctrl->step;
00214         entry->targetValue = (entry->targetValue / entry->queryctrl->step) * entry->queryctrl->step;
00215         entry->info_msg << "new target value = " << entry->targetValue << std::endl;
00216     }
00217     if(entry->targetValue > entry->queryctrl->maximum) {
00218         entry->info_msg << "clipping taget value = " << entry->targetValue << " to maximum = " << entry->queryctrl->maximum << "\n";
00219         entry->targetValue = entry->queryctrl->maximum;
00220     }
00221     if(entry->targetValue < entry->queryctrl->minimum) {
00222         entry->info_msg << "clipping taget value = " << entry->targetValue << " to minimum = " << entry->queryctrl->minimum << "\n";
00223         entry->targetValue = entry->queryctrl->minimum;
00224     }
00225 
00226     struct v4l2_control control;
00227     control.id = entry->queryctrl->id;
00228     control.value = entry->targetValue;
00229     if(v4l2_ioctl(pVideoIn_->fd, VIDIOC_S_CTRL, &control) < 0) {
00230         entry->error_msg <<  "v4l2_ioctl set( " << control.value << ") control error\n";
00231         return ERROR;
00232     }
00233     if(entry->queryctrl->flags & V4L2_CTRL_FLAG_WRITE_ONLY) {
00234         entry->info_msg << "entry is write only\n";
00235         entry->currentValue = entry->targetValue;
00236     } else {
00237         if(v4l2_ioctl(pVideoIn_->fd, VIDIOC_G_CTRL, &control) < 0) {
00238             entry->error_msg <<  "v4l2_ioctl get control error, to verify\n";
00239             return ERROR;
00240         }
00241         entry->currentValue = control.value;
00242         if(control.value != entry->targetValue) {
00243             entry->error_msg <<  "v4l2_ioctl set and get are different. -> ";
00244             entry->error_msg <<  entry->targetValue << " != " << control.value << std::endl;
00245             return ERROR;
00246         }
00247     }
00248     entry->info_msg << "current = " << entry->currentValue << "\n";
00249     return OK;
00250 }
00251 
00252 V4RCam::ControlEntry::ControlEntry(int id)
00253     : valid(true)
00254     , varName("NA")
00255     , queryctrl(new v4l2_queryctrl)
00256     , currentValue(-1)
00257     , targetValue(-1)
00258 {
00259     queryctrl->id = id;
00260 }
00261 V4RCam::ControlEntry::~ControlEntry()
00262 {
00263     delete queryctrl;
00264 }
00265 bool V4RCam::ControlEntry::hasValidType() const
00266 {
00267     if(queryctrl->type == V4L2_CTRL_TYPE_INTEGER) return true;
00268     if(queryctrl->type == V4L2_CTRL_TYPE_BOOLEAN)  return true;
00269     if(queryctrl->type == V4L2_CTRL_TYPE_MENU) return true;
00270     if(queryctrl->type == V4L2_CTRL_TYPE_BUTTON) return true;
00271     if(queryctrl->type == V4L2_CTRL_TYPE_INTEGER64) return true;
00272     if(queryctrl->type == V4L2_CTRL_TYPE_CTRL_CLASS) return true;
00273     if(queryctrl->type == V4L2_CTRL_TYPE_STRING) return true;
00274     if(queryctrl->type == V4L2_CTRL_TYPE_BITMASK) return true;
00275     if(queryctrl->type == V4L2_CTRL_TYPE_BITMASK) return true;
00276     return false;
00277 }
00278 std::string V4RCam::ControlEntry::getQueryCtrlInfo() const
00279 {
00280     std::stringstream ss;
00281 
00282     ss  << std::setw(9) << std::hex << std::showbase << queryctrl->id << " = " << varName;
00283     if(valid == false) return ss.str();
00284     ss << " >> " << queryctrl->name << std::endl;
00285     ss << std::dec << "current = " << currentValue << ", target = "  <<  targetValue;
00286     ss << ", min = " << queryctrl->minimum << ", max = " << queryctrl->maximum;
00287     ss << ", default = " << queryctrl->default_value << ", step = " << queryctrl->step << std::endl;
00288     ss << "flags =";
00289     if(queryctrl->flags & V4L2_CTRL_FLAG_DISABLED) ss  << " disabled";
00290     if(queryctrl->flags & V4L2_CTRL_FLAG_GRABBED)  ss << " grabbed";
00291     if(queryctrl->flags & V4L2_CTRL_FLAG_READ_ONLY)   ss << " read only";
00292     if(queryctrl->flags & V4L2_CTRL_FLAG_UPDATE)  ss << " update";
00293     if(queryctrl->flags & V4L2_CTRL_FLAG_INACTIVE) ss << " inactive";
00294     if(queryctrl->flags & V4L2_CTRL_FLAG_SLIDER) ss << " slider";
00295     if(queryctrl->flags & V4L2_CTRL_FLAG_WRITE_ONLY) ss << " write only";
00296     if(queryctrl->flags & V4L2_CTRL_FLAG_VOLATILE) ss << " volatile";
00297     if(queryctrl->flags == 0) ss << " empty";
00298     ss << "; type =";
00299     if(queryctrl->type == V4L2_CTRL_TYPE_INTEGER) ss << " integer";
00300     else if(queryctrl->type == V4L2_CTRL_TYPE_BOOLEAN) ss << "boolean";
00301     else if(queryctrl->type == V4L2_CTRL_TYPE_MENU)  ss << "menu";
00302     else if(queryctrl->type == V4L2_CTRL_TYPE_BUTTON)  ss << "button";
00303     else if(queryctrl->type == V4L2_CTRL_TYPE_INTEGER64)  ss << "integer64";
00304     else if(queryctrl->type == V4L2_CTRL_TYPE_CTRL_CLASS)  ss << "ctrl_class";
00305     else if(queryctrl->type == V4L2_CTRL_TYPE_STRING)  ss << "string";
00306     else if(queryctrl->type == V4L2_CTRL_TYPE_BITMASK)  ss << "bitmask";
00307     else ss << "unsupported";
00308     return ss.str();
00309 }
00310 
00311 std::string V4RCam::ControlEntry::pullErrorMsg()
00312 {
00313     std::string str = error_msg.str();
00314     error_msg.str(std::string());
00315     return str;
00316 }
00317 std::string V4RCam::ControlEntry::pullInfoMsg()
00318 {
00319     std::string str = info_msg.str();
00320     info_msg.str(std::string());
00321     return str;
00322 }
00323 bool V4RCam::ControlEntry::hasErrorMsg() const
00324 {
00325     return !error_msg.str().empty();
00326 };
00327 bool V4RCam::ControlEntry::hasInfoMsg() const
00328 {
00329     return !info_msg.str().empty();
00330 };
00331 
00332 std::string V4RCam::pullErrorMsg()
00333 {
00334     std::string str = error_msg_.str();
00335     error_msg_.str(std::string());
00336     return str;
00337 }
00338 std::string V4RCam::pullInfoMsg()
00339 {
00340     std::string str = info_msg_.str();
00341     info_msg_.str(std::string());
00342     return str;
00343 }
00344 bool V4RCam::hasErrorMsg() const
00345 {
00346     return !error_msg_.str().empty();
00347 };
00348 bool V4RCam::hasInfoMsg() const
00349 {
00350     return !info_msg_.str().empty();
00351 };
00352 const std::vector<V4RCam::ControlEntryPtr > &V4RCam::detectControlEnties()
00353 {
00354     v4l2_queryctrl queryctrl;
00355 
00356     controlEntries_.clear();
00357 
00358     /* Try the extended control API first */
00359 #ifdef V4L2_CTRL_FLAG_NEXT_CTRL  // ref --> v4l3upc
00360     queryctrl.id = V4L2_CTRL_FLAG_NEXT_CTRL;
00361     if(0 == v4l2_ioctl(pVideoIn_->fd, VIDIOC_QUERYCTRL, &queryctrl)) {
00362         do {
00363             boost::this_thread::sleep(boost::posix_time::milliseconds(10));
00364             controlEntries_.push_back(ControlEntryPtr(new ControlEntry(queryctrl.id)));
00365             queryctrl.id |= V4L2_CTRL_FLAG_NEXT_CTRL;
00366         } while(0 == v4l2_ioctl(pVideoIn_->fd, VIDIOC_QUERYCTRL, &queryctrl));
00367     } else
00368 #endif
00369     {
00370         /* Fall back on the standard API */
00371         /* Check all the standard controls */
00372         for(int i = V4L2_CID_BASE; i < V4L2_CID_LASTP1; i++) {
00373             controlEntries_.push_back(ControlEntryPtr(new ControlEntry(i)));
00374         }
00375         /* Check any custom controls */
00376         for(queryctrl.id = V4L2_CID_PRIVATE_BASE; (v4l2_ioctl(pVideoIn_->fd, VIDIOC_QUERYCTRL, &queryctrl) == 0) ; queryctrl.id++) {
00377             controlEntries_.push_back(ControlEntryPtr(new ControlEntry(queryctrl.id)));
00378             boost::this_thread::sleep(boost::posix_time::milliseconds(10));
00379         }
00380     }
00381     for(unsigned int i = 0; i < controlEntries_.size(); i++) {
00382         v4lgetInfo(controlEntries_[i]);
00383     }
00384     return controlEntries_;
00385 }
00386 
00387 
00388 int V4RCam::save_controls(const std::string &filename)
00389 {
00390     struct v4l2_queryctrl queryctrl;
00391     struct v4l2_control   control_s;
00392     FILE *configfile;
00393     memset(&queryctrl, 0, sizeof(queryctrl));
00394     memset(&control_s, 0, sizeof(control_s));
00395     configfile = fopen(filename.c_str(), "w");
00396     if(configfile == NULL) {
00397         error_msg_ << "saving configfile: " << filename << " failed" << std::endl;
00398     } else {
00399         fprintf(configfile, "id         value      # luvcview control settings configuration file\n");
00400         for(std::vector<V4RCam::ControlEntryPtr>::const_iterator it = controlEntries_.begin(); it != controlEntries_.end(); it++) {
00401             queryctrl.id = (*it)->queryctrl->id;
00402             if(0 == ioctl(pVideoIn_->fd, VIDIOC_QUERYCTRL, &queryctrl)) {
00403                 if(queryctrl.flags & V4L2_CTRL_FLAG_DISABLED)
00404                     continue;
00405                 control_s.id = queryctrl.id;
00406                 v4l2_ioctl(pVideoIn_->fd, VIDIOC_G_CTRL, &control_s);
00407                 boost::this_thread::sleep(boost::posix_time::milliseconds(10));
00408                 fprintf(configfile, "%-10d %-10d # name:%-32s type:%d min:%-5d max:%-5d step:%-5d def:%d\n",
00409                         queryctrl.id, control_s.value, queryctrl.name, queryctrl.type, queryctrl.minimum,
00410                         queryctrl.maximum, queryctrl.step, queryctrl.default_value);
00411                 printf("%-10d %-10d # name:%-32s type:%d min:%-5d max:%-5d step:%-5d def:%d\n",
00412                        queryctrl.id, control_s.value, queryctrl.name, queryctrl.type, queryctrl.minimum,
00413                        queryctrl.maximum, queryctrl.step, queryctrl.default_value);
00414                 boost::this_thread::sleep(boost::posix_time::milliseconds(10));
00415             }
00416         }
00417         fflush(configfile);
00418         fclose(configfile);
00419         boost::this_thread::sleep(boost::posix_time::milliseconds(100));
00420         info_msg_ << "saved configfile: " << filename << std::endl;
00421     }
00422     return OK;
00423 }
00424 
00425 int V4RCam::load_controls(const std::string &filename)//struct vdIn *vd)
00426 {
00427     struct v4l2_control   control;
00428     FILE *configfile;
00429     memset(&control, 0, sizeof(control));
00430     configfile = fopen(filename.c_str(), "r");
00431     if(configfile == NULL) {
00432         error_msg_ << "configfile: " << filename << " open failed" << std::endl;
00433     } else {
00434         info_msg_ << "loading controls from luvcview.cfg\n";
00435         char buffer[0xFFF];
00436         while(NULL != fgets(buffer, sizeof(buffer), configfile)) {
00437             sscanf(buffer, "%i%i", &control.id, &control.value);
00438             if(v4l2_ioctl(pVideoIn_->fd, VIDIOC_S_CTRL, &control))
00439                 info_msg_ << "ERROR id: " << control.id << " val: " <<  control.value << std::endl;
00440             else
00441                 info_msg_ << "OK    id: " << control.id << " val: " <<  control.value << std::endl;
00442             boost::this_thread::sleep(boost::posix_time::milliseconds(20));
00443         }
00444         fclose(configfile);
00445         info_msg_ << "loaded configfile: " << filename << std::endl;
00446     }
00447 }
00448 
00449 V4RCam::ControlEntryPtr V4RCam::getControlEntry(std::string varName){
00450   for(unsigned int i = 0; i < controlEntries_.size(); i++){
00451     if(controlEntries_[i]->varName.compare(varName) == 0){
00452       return controlEntries_[i];
00453     }
00454   }
00455   return V4RCam::ControlEntryPtr();
00456 }


v4r_uvc
Author(s):
autogenerated on Wed Aug 26 2015 16:41:35