- Timestamp:
- Nov 28, 2019, 5:08:04 PM (5 years ago)
- Location:
- trunk/lib/FlairSensorActuator/src
- Files:
-
- 6 added
- 19 edited
Legend:
- Unmodified
- Added
- Removed
-
trunk/lib/FlairSensorActuator/src/Bldc.cpp
r318 r340 58 58 } 59 59 60 Bldc::Bldc(const Object *parent, string name, uint8_t motors_count)61 : IODevice(parent, name) {62 pimpl_ = new Bldc_impl(this, motors_count);63 64 SetIsReady(true);65 }66 67 60 Bldc::~Bldc() { delete pimpl_; } 68 61 -
trunk/lib/FlairSensorActuator/src/Bldc.h
r214 r340 51 51 Bldc(const core::IODevice *parent, gui::Layout *layout, std::string name, 52 52 uint8_t motors_count); 53 54 /*!55 * \brief Constructor56 *57 * Construct a Bldc. \n58 * This contructor must only be called for a simulated device.59 *60 * \param parent parent61 * \param name name62 * \param motors_count number of motors63 */64 Bldc(const core::Object *parent, std::string name, uint8_t motors_count);65 53 66 54 /*! -
trunk/lib/FlairSensorActuator/src/Bldc_impl.cpp
r254 r340 84 84 } 85 85 86 Bldc_impl::Bldc_impl(Bldc *self, uint8_t motors_count) {87 this->self = self;88 this->motors_count = motors_count;89 values = NULL;90 button_test = NULL;91 power = NULL;92 }93 94 86 Bldc_impl::~Bldc_impl() { 95 87 if (values != NULL) … … 124 116 for (int i = 0; i < motors_count; i++) { 125 117 if (are_enabled) { 126 // 118 //Printf("%i %f %f\n",i,input->ValueNoMutex(i,0),power[i]); 127 119 values[i] = power[i] * Sat(input->ValueNoMutex(i, 0)); 128 120 // Printf("%i %f\n",i,values[i]); -
trunk/lib/FlairSensorActuator/src/Camera.cpp
r338 r340 51 51 } 52 52 53 //This contructor must only be called for a simulated device.54 Camera::Camera(const IODevice *parent, std::string name)55 : IODevice(parent, name) {56 plot_tab = NULL;57 main_tab = NULL;58 tab = NULL;59 sensor_tab = NULL;60 setup_groupbox = NULL;61 62 output = NULL;63 logFormat=LogFormat::NONE;64 }65 66 53 Camera::~Camera() { 67 if (main_tab != NULL) 68 delete main_tab; 54 delete main_tab; 69 55 } 70 56 … … 94 80 95 81 void Camera::UseDefaultPlot(const core::Image *image) { 96 if (tab == NULL) {97 Err("not applicable for simulation part.\n");98 return;99 }100 101 82 plot_tab = new Tab(tab, "Picture"); 102 83 Picture *plot = new Picture(plot_tab->NewRow(), ObjectName(), image); -
trunk/lib/FlairSensorActuator/src/Camera.h
r338 r340 52 52 Camera(std::string name, uint16_t width, 53 53 uint16_t height, core::Image::Type::Format format); 54 55 /*!56 * \brief Constructor57 *58 * Construct a Camera. \n59 * This contructor must only be called for a simulated device.60 *61 * \param parent parent62 * \param name name63 */64 Camera(const core::IODevice *parent, std::string name);65 54 66 55 /*! -
trunk/lib/FlairSensorActuator/src/Imu.cpp
r198 r340 49 49 } 50 50 51 Imu::Imu(const IODevice *parent,std::string name) : IODevice(parent, name) {52 imuData = new ImuData(this);53 mainTab = NULL;54 tab = NULL;55 sensorTab = NULL;56 setupGroupbox = NULL;57 rotation = NULL;58 }59 60 51 Imu::~Imu() { 61 if (mainTab != NULL) 62 delete mainTab; 52 delete mainTab; 63 53 } 64 54 … … 97 87 98 88 void Imu::LockUserInterface(void) const { 99 if (sensorTab == NULL) {100 Err("not applicable for simulation part.\n");101 return;102 }103 89 sensorTab->setEnabled(false); 104 90 } 105 91 106 92 void Imu::UnlockUserInterface(void) const { 107 if (sensorTab == NULL) {108 Err("not applicable for simulation part.\n");109 return;110 }111 93 sensorTab->setEnabled(true); 112 94 } 113 95 114 96 void Imu::UseDefaultPlot(void) { 115 if (tab == NULL) {116 Err("not applicable for simulation part.\n");117 return;118 }119 97 120 98 plotTab = new Tab(tab, "IMU"); … … 126 104 azPlot->AddCurve(imuData->Element(ImuData::RawAz)); 127 105 128 if (plotTab == NULL)129 plotTab = new Tab(tab, "IMU");130 106 gxPlot = new DataPlot1D(plotTab->NewRow(), "gyr_x", -500, 500); 131 107 gxPlot->AddCurve(imuData->Element(ImuData::RawGxDeg)); … … 135 111 gzPlot->AddCurve(imuData->Element(ImuData::RawGzDeg)); 136 112 137 if (plotTab == NULL)138 plotTab = new Tab(tab, "IMU");139 113 mxPlot = new DataPlot1D(plotTab->NewRow(), "mag_x", -500, 500); 140 114 mxPlot->AddCurve(imuData->Element(ImuData::RawMx)); -
trunk/lib/FlairSensorActuator/src/Imu.h
r198 r340 56 56 */ 57 57 Imu(std::string name,bool needRotation=true); 58 59 /*!60 * \brief Constructor61 *62 * Construct an Imu. \n63 * This contructor must only be called for a simulated device.64 *65 * \param parent parent66 * \param name name67 */68 Imu(const core::IODevice *parent, std::string name);69 58 70 59 /*! -
trunk/lib/FlairSensorActuator/src/LaserRangeFinder.cpp
r318 r340 49 49 } 50 50 51 LaserRangeFinder::LaserRangeFinder(const IODevice *parent, std::string name)52 : IODevice(parent, name) {53 plot_tab = NULL;54 main_tab = NULL;55 tab = NULL;56 sensor_tab = NULL;57 setup_groupbox = NULL;58 59 output = NULL;60 }61 62 51 LaserRangeFinder::~LaserRangeFinder() {} 63 52 … … 69 58 70 59 void LaserRangeFinder::UseDefaultPlot(void) { 71 if (tab == NULL) {72 Err("not applicable for simulation part.\n");73 return;74 }75 76 60 plot_tab = new Tab(tab, "Mesures"); 77 61 plot = new RangeFinderPlot(plot_tab->NewRow(), "plot", "x", -30, 30, "y", -30, -
trunk/lib/FlairSensorActuator/src/LaserRangeFinder.h
r214 r340 47 47 */ 48 48 LaserRangeFinder(std::string name); 49 /*! 50 * \brief Constructor 51 * 52 * Construct a UsRangeFinder. Simulation part. 53 * 54 * \param parent parent 55 * \param name name 56 */ 57 LaserRangeFinder(const core::IODevice *parent, std::string name); 49 58 50 /*! 59 51 * \brief Destructor -
trunk/lib/FlairSensorActuator/src/NmeaGps.cpp
r219 r340 73 73 } 74 74 75 NmeaGps::NmeaGps(const IODevice *parent, std::string name) : IODevice(parent, name) {76 gpsData = new GpsData(this);77 mainTab = NULL;78 tab = NULL;79 sensorTab = NULL;80 }81 82 75 NmeaGps::~NmeaGps() { 83 if (mainTab != NULL) { 84 nmea_parser_destroy(&parser); 85 delete mainTab; 86 } 76 nmea_parser_destroy(&parser); 77 delete mainTab; 87 78 } 88 79 -
trunk/lib/FlairSensorActuator/src/NmeaGps.h
r193 r340 67 67 NmeaGps(std::string name, 68 68 NMEAFlags_t NMEAFlags); 69 70 /*!71 * \brief Constructor72 *73 * Construct a NmeaGps. \n74 * This contructor must only be called for a simulated device.75 *76 * \param parent parent77 * \param name name78 */79 NmeaGps(const core::IODevice *parent, std::string name);80 69 81 70 /*! -
trunk/lib/FlairSensorActuator/src/PressureSensor.cpp
r318 r340 48 48 } 49 49 50 PressureSensor::PressureSensor(const IODevice *parent, std::string name)51 : IODevice(parent, name) {52 plot_tab = NULL;53 main_tab = NULL;54 tab = NULL;55 sensor_tab = NULL;56 setup_groupbox = NULL;57 58 output = NULL;59 }60 61 50 PressureSensor::~PressureSensor() { 62 if (main_tab != NULL) 63 delete main_tab; 51 delete main_tab; 64 52 } 65 53 … … 73 61 74 62 void PressureSensor::UseDefaultPlot(void) { 75 if (tab == NULL) {76 Err("not applicable for simulation part.\n");77 return;78 }79 80 63 plot_tab = new Tab(tab, "Mesures"); 81 64 plot = new DataPlot1D(plot_tab->NewRow(), ObjectName(), 101000, 101500); -
trunk/lib/FlairSensorActuator/src/PressureSensor.h
r216 r340 49 49 */ 50 50 PressureSensor(std::string name); 51 52 /*!53 * \brief Constructor54 *55 * Construct a PressureSensor. Simulation part.56 *57 * \param parent parent58 * \param name name59 */60 PressureSensor(const core::IODevice *parent, std::string name);61 51 62 52 /*! -
trunk/lib/FlairSensorActuator/src/SimulatedBldc.h
r286 r340 20 20 class SharedMem; 21 21 class IODevice; 22 class Matrix;23 22 } 24 23 namespace gui { -
trunk/lib/FlairSensorActuator/src/UsRangeFinder.cpp
r318 r340 49 49 } 50 50 51 UsRangeFinder::UsRangeFinder(const IODevice *parent, std::string name)52 : IODevice(parent, name) {53 plot_tab = NULL;54 main_tab = NULL;55 tab = NULL;56 sensor_tab = NULL;57 setup_groupbox = NULL;58 59 output = NULL;60 }61 62 51 UsRangeFinder::~UsRangeFinder() { 63 if (main_tab != NULL)64 52 delete main_tab; 65 53 } … … 76 64 77 65 void UsRangeFinder::UseDefaultPlot(void) { 78 if (tab == NULL) {79 Err("not applicable for simulation part.\n");80 return;81 }82 83 66 plot_tab = new Tab(tab, "Mesures"); 84 67 plot = new DataPlot1D(plot_tab->NewRow(), ObjectName(), 0, 2); -
trunk/lib/FlairSensorActuator/src/UsRangeFinder.h
r221 r340 49 49 */ 50 50 UsRangeFinder(std::string name); 51 52 /*! 53 * \brief Constructor 54 * 55 * Construct a UsRangeFinder. Simulation part. 56 * 57 * \param parent parent 58 * \param name name 59 */ 60 UsRangeFinder(const core::IODevice *parent, std::string name); 61 51 62 52 /*! 63 53 * \brief Destructor -
trunk/lib/FlairSensorActuator/src/V4LCamera.cpp
r338 r340 33 33 #define DEFAULT_V4L_BUFFERS 4 34 34 35 #define CHANNEL_NUMBER 1 36 #define CLEAR(x) memset (&(x), 0, sizeof (x)) 37 #define DEFAULT_V4L_WIDTH 640 38 #define DEFAULT_V4L_HEIGHT 480 39 #define CV_CAP_PROP_POS_MSEC 0 40 #define CV_CAP_PROP_POS_FRAMES 1 41 #define CV_CAP_PROP_POS_AVI_RATIO 2 42 #define CV_CAP_PROP_FRAME_WIDTH 3 43 #define CV_CAP_PROP_FRAME_HEIGHT 4 44 #define CV_CAP_PROP_FPS 5 45 #define CV_CAP_PROP_FOURCC 6 46 #define CV_CAP_PROP_FRAME_COUNT 7 47 #define CV_CAP_PROP_FORMAT 8 48 #define CV_CAP_PROP_MODE 9 49 #define CV_CAP_PROP_BRIGHTNESS 10 50 #define CV_CAP_PROP_CONTRAST 11 51 #define CV_CAP_PROP_SATURATION 12 52 #define CV_CAP_PROP_HUE 13 53 #define CV_CAP_PROP_GAIN 14 54 #define CV_CAP_PROP_CONVERT_RGB 15 55 #define CV_CAP_PROP_SHARPNESS 16 56 #define CV_CAP_PROP_EXPOSURE 17 57 #define CV_CAP_PROP_AUTOGAIN 18 58 #define CV_CAP_PROP_AWB 19 59 60 #define HAVE_CAMV4L2 61 35 62 using std::string; 36 63 using namespace flair::core; … … 39 66 namespace flair { 40 67 namespace sensor { 41 68 69 V4LCamera::V4LCamera(string name, 70 uint8_t camera_index, uint16_t width, uint16_t height, 71 Image::Type::Format format, uint8_t priority) 72 : Thread(getFrameworkManager(), name, priority), 73 Camera(name, width, height, format) { 74 75 string deviceName="/dev/video"+std::to_string(camera_index); 76 device = open(deviceName.c_str(), O_RDWR | O_NONBLOCK); 77 if (device == -1) { 78 Thread::Err("Cannot open %s\n",deviceName.c_str()); 79 } else { 80 Printf("V4LCamera %s, opened %s\n",name.c_str(),deviceName.c_str()); 81 } 82 83 /* w/o memset some parts arent initialized - AKA: Fill it with zeros so it is clean */ 84 memset(&capture,0,sizeof(CvCaptureCAM_V4L)); 85 /* Present the routines needed for V4L funtionality. They are inserted as part of 86 the standard set of cv calls promoting transparency. "Vector Table" insertion. */ 87 capture.FirstCapture = 1; 88 capture.deviceHandle = device; 89 90 PALETTE_YUYV = 0; 91 PALETTE_UYVY= 0; 92 93 n_buffers = 0; 94 if (_capture_V4L2 (&capture) == -1) { 95 Thread::Err("_capture_V4L2 failed\n"); 96 } 97 98 if (icvSetPropertyCAM_V4L(&capture, CV_CAP_PROP_FRAME_WIDTH, width)<0) 99 Thread::Err("cvSetCaptureProperty error\n"); 100 if (icvSetPropertyCAM_V4L(&capture, CV_CAP_PROP_FRAME_HEIGHT, height)<0) 101 Thread::Err("cvSetCaptureProperty error\n"); 102 103 if (format == Image::Type::Format::UYVY) { 104 if (icvSetPropertyCAM_V4L(&capture, CV_CAP_PROP_FORMAT, V4L2_PIX_FMT_UYVY)<0) 105 Thread::Err("cvSetCaptureProperty error\n"); 106 } else if (format == Image::Type::Format::YUYV) { 107 if (icvSetPropertyCAM_V4L(&capture, CV_CAP_PROP_FORMAT, V4L2_PIX_FMT_YUYV) < 108 0) 109 Thread::Err("cvSetCaptureProperty error\n"); 110 } else { 111 Thread::Err("format not supported\n"); 112 } 113 // ground station 114 gain = new DoubleSpinBox(GetGroupBox()->NewRow(), "gain:", 0, 1, 0.1); 115 exposure = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "exposure:", 0,1, 0.1); 116 bright = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "bright:", 0, 1, 0.1); 117 contrast = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "contrast:", 0,1, 0.1); 118 hue = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "hue:", 0, 1, 0.1); 119 sharpness = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "sharpness:", 0, 1, 0.1); 120 sat = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "saturation:", 0, 1,0.1); 121 autogain = new CheckBox(GetGroupBox()->NewRow(), "autogain:"); 122 autoexposure = new CheckBox(GetGroupBox()->LastRowLastCol(), "autoexposure:"); 123 awb = new CheckBox(GetGroupBox()->LastRowLastCol(), "awb:"); 124 fps = new Label(GetGroupBox()->NewRow(), "fps"); 125 126 hasProblems=false; 127 } 128 129 V4LCamera::~V4LCamera() { 130 for (int n_buffers = 0; n_buffers < nbBuffers; n_buffers++) { 131 //FreeFunction((char*)buffers[n_buffers].start); 132 } 133 SafeStop(); 134 Join(); 135 } 136 int V4LCamera::icvSetPropertyCAM_V4L( CvCaptureCAM_V4L* capture, 137 int property_id, double value ){ 138 static int width = 0, height = 0; 139 int retval; 140 141 /* initialization */ 142 retval = 0; 143 144 /* two subsequent calls setting WIDTH and HEIGHT will change 145 the video size */ 146 /* the first one will return an error, though. */ 147 148 switch (property_id) { 149 case CV_CAP_PROP_FRAME_WIDTH: 150 width = value; 151 if(width !=0 && height != 0) { 152 retval = icvSetVideoSize( capture, width, height); 153 width = height = 0; 154 } 155 break; 156 case CV_CAP_PROP_FRAME_HEIGHT: 157 height = value; 158 if(width !=0 && height != 0) { 159 retval = icvSetVideoSize( capture, width, height); 160 width = height = 0; 161 } 162 break; 163 case CV_CAP_PROP_FORMAT: 164 165 capture->form.fmt.pix.pixelformat = (unsigned long)value; 166 retval == xioctl (capture->deviceHandle, VIDIOC_S_FMT, &capture->form); 167 if ((unsigned long)value != capture->form.fmt.pix.pixelformat) 168 retval=-1; 169 170 171 PALETTE_YUYV = 0; 172 PALETTE_UYVY= 0; 173 174 175 if ((unsigned long)value== V4L2_PIX_FMT_YUYV) 176 { 177 PALETTE_YUYV = 1; 178 } 179 if ((unsigned long)value== V4L2_PIX_FMT_UYVY) 180 { 181 PALETTE_UYVY = 1; 182 } 183 184 185 break; 186 case CV_CAP_PROP_BRIGHTNESS: 187 case CV_CAP_PROP_CONTRAST: 188 case CV_CAP_PROP_SATURATION: 189 case CV_CAP_PROP_HUE: 190 case CV_CAP_PROP_GAIN: 191 case CV_CAP_PROP_SHARPNESS: 192 case CV_CAP_PROP_EXPOSURE: 193 case CV_CAP_PROP_AUTOGAIN: 194 case CV_CAP_PROP_AWB: 195 retval = icvSetControl(capture, property_id, value); 196 break; 197 default: 198 fprintf(stderr, 199 "HIGHGUI ERROR: V4L: setting property #%d is not supported\n", 200 property_id); 201 } 202 203 /* return the the status */ 204 return retval; 205 } 206 int V4LCamera::icvSetControl (CvCaptureCAM_V4L* capture, 207 int property_id, double value) { 208 209 /* limitation of the input value */ 210 if (value < 0.0) { 211 value = 0.0; 212 } else if (value > 1.0) { 213 value = 1.0; 214 } 215 216 217 /* default value for min and max */ 218 int v4l2_min = 0; 219 int v4l2_max = 255; 220 221 /* initialisations */ 222 CLEAR (capture->control); 223 224 /* set which control we want to set */ 225 switch (property_id) { 226 227 case CV_CAP_PROP_BRIGHTNESS: 228 capture->control.id = V4L2_CID_BRIGHTNESS; 229 break; 230 case CV_CAP_PROP_CONTRAST: 231 capture->control.id = V4L2_CID_CONTRAST; 232 break; 233 case CV_CAP_PROP_SATURATION: 234 capture->control.id = V4L2_CID_SATURATION; 235 break; 236 case CV_CAP_PROP_HUE: 237 capture->control.id = V4L2_CID_HUE; 238 break; 239 case CV_CAP_PROP_GAIN: 240 capture->control.id = V4L2_CID_GAIN; 241 break; 242 case CV_CAP_PROP_SHARPNESS: 243 capture->control.id = V4L2_CID_SHARPNESS; 244 break; 245 case CV_CAP_PROP_EXPOSURE: 246 capture->control.id = V4L2_CID_EXPOSURE; 247 break; 248 case CV_CAP_PROP_AUTOGAIN: 249 capture->control.id = V4L2_CID_AUTOGAIN; 250 break; 251 case CV_CAP_PROP_AWB: 252 capture->control.id = V4L2_CID_AUTO_WHITE_BALANCE; 253 break; 254 default: 255 fprintf(stderr, 256 "HIGHGUI ERROR: V4L2: setting property #%d is not supported\n", 257 property_id); 258 return -1; 259 } 260 261 /* get the min and max values */ 262 if (-1 == xioctl (capture->deviceHandle, 263 VIDIOC_G_CTRL, &capture->control)) { 264 // perror ("VIDIOC_G_CTRL for getting min/max values"); 265 return -1; 266 } 267 268 /* get the min/max values */ 269 switch (property_id) { 270 271 case CV_CAP_PROP_BRIGHTNESS: 272 v4l2_min = capture->v4l2_brightness_min; 273 v4l2_max = capture->v4l2_brightness_max; 274 break; 275 case CV_CAP_PROP_CONTRAST: 276 v4l2_min = capture->v4l2_contrast_min; 277 v4l2_max = capture->v4l2_contrast_max; 278 break; 279 case CV_CAP_PROP_SATURATION: 280 v4l2_min = capture->v4l2_saturation_min; 281 v4l2_max = capture->v4l2_saturation_max; 282 break; 283 case CV_CAP_PROP_HUE: 284 v4l2_min = capture->v4l2_hue_min; 285 v4l2_max = capture->v4l2_hue_max; 286 break; 287 case CV_CAP_PROP_GAIN: 288 v4l2_min = capture->v4l2_gain_min; 289 v4l2_max = capture->v4l2_gain_max; 290 break; 291 case CV_CAP_PROP_SHARPNESS: 292 v4l2_min = capture->v4l2_sharpness_min; 293 v4l2_max = capture->v4l2_sharpness_max; 294 break; 295 case CV_CAP_PROP_EXPOSURE: 296 v4l2_min = capture->v4l2_exposure_min; 297 v4l2_max = capture->v4l2_exposure_max; 298 break; 299 case CV_CAP_PROP_AUTOGAIN: 300 v4l2_min = capture->v4l2_autogain_min; 301 v4l2_max = capture->v4l2_autogain_max; 302 break; 303 case CV_CAP_PROP_AWB: 304 v4l2_min = capture->v4l2_awb_min; 305 v4l2_max = capture->v4l2_awb_max; 306 break; 307 } 308 309 /* initialisations */ 310 CLEAR (capture->control); 311 312 /* set which control we want to set */ 313 switch (property_id) { 314 315 case CV_CAP_PROP_BRIGHTNESS: 316 capture->control.id = V4L2_CID_BRIGHTNESS; 317 break; 318 case CV_CAP_PROP_CONTRAST: 319 capture->control.id = V4L2_CID_CONTRAST; 320 break; 321 case CV_CAP_PROP_SATURATION: 322 capture->control.id = V4L2_CID_SATURATION; 323 break; 324 case CV_CAP_PROP_HUE: 325 capture->control.id = V4L2_CID_HUE; 326 break; 327 case CV_CAP_PROP_GAIN: 328 capture->control.id = V4L2_CID_GAIN; 329 break; 330 case CV_CAP_PROP_SHARPNESS: 331 capture->control.id = V4L2_CID_SHARPNESS; 332 break; 333 case CV_CAP_PROP_EXPOSURE: 334 capture->control.id = V4L2_CID_EXPOSURE; 335 break; 336 case CV_CAP_PROP_AUTOGAIN: 337 capture->control.id = V4L2_CID_AUTOGAIN; 338 break; 339 case CV_CAP_PROP_AWB: 340 capture->control.id = V4L2_CID_AUTO_WHITE_BALANCE; 341 break; 342 default: 343 fprintf(stderr, 344 "HIGHGUI ERROR: V4L2: setting property #%d is not supported\n", 345 property_id); 346 return -1; 347 } 348 349 /* set the value we want to set to the scaled the value */ 350 capture->control.value = (int)(value * (v4l2_max - v4l2_min) + v4l2_min); 351 352 /* The driver may clamp the value or return ERANGE, ignored here */ 353 if (-1 == xioctl (capture->deviceHandle, 354 VIDIOC_S_CTRL, &capture->control) && errno != ERANGE) { 355 perror ("VIDIOC_S_CTRL"); 356 return -1; 357 } 358 359 /* all was OK */ 360 return 0; 361 362 } 363 int V4LCamera::_capture_V4L2 (CvCaptureCAM_V4L *capture) 364 { 365 int detect_v4l2 = 0; 366 367 detect_v4l2 = try_init_v4l2(capture); 368 369 if (detect_v4l2 != 1) { 370 /* init of the v4l2 device is not OK */ 371 return -1; 372 } 373 374 /* Init V4L2 control variables */ 375 capture->v4l2_brightness = 0; 376 capture->v4l2_contrast = 0; 377 capture->v4l2_saturation = 0; 378 capture->v4l2_hue = 0; 379 capture->v4l2_gain = 0; 380 capture->v4l2_sharpness = 0; 381 capture->v4l2_exposure = 0; 382 capture->v4l2_autogain = 0; 383 capture->v4l2_awb = 0; 384 385 capture->v4l2_brightness_min = 0; 386 capture->v4l2_contrast_min = 0; 387 capture->v4l2_saturation_min = 0; 388 capture->v4l2_hue_min = 0; 389 capture->v4l2_gain_min = 0; 390 capture->v4l2_sharpness_min = 0; 391 capture->v4l2_exposure_min = 0; 392 capture->v4l2_autogain_min = 0; 393 capture->v4l2_awb_min = 0; 394 395 capture->v4l2_brightness_max = 0; 396 capture->v4l2_contrast_max = 0; 397 capture->v4l2_saturation_max = 0; 398 capture->v4l2_hue_max = 0; 399 capture->v4l2_gain_max = 0; 400 capture->v4l2_sharpness_max = 0; 401 capture->v4l2_exposure_max = 0; 402 capture->v4l2_autogain_max = 0; 403 capture->v4l2_awb_max = 0; 404 405 406 if ((capture->cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) == 0) { 407 /* Nope. */ 408 fprintf( stderr, "HIGHGUI ERROR: V4L2: device is unable to capture video memory.\n"); 409 410 return -1; 411 } 412 413 414 415 /* Find Window info */ 416 CLEAR (capture->form); 417 capture->form.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 418 419 if (-1 == xioctl (capture->deviceHandle, VIDIOC_G_FMT, &capture->form)) { 420 fprintf( stderr, "HIGHGUI ERROR: V4L2: Could not obtain specifics of capture window.\n\n"); 421 422 return -1; 423 } 424 425 426 427 if (autosetup_capture_mode_v4l2(capture) == -1) 428 return -1; 429 430 icvSetVideoSize(capture, DEFAULT_V4L_WIDTH, DEFAULT_V4L_HEIGHT); 431 432 unsigned int min; 433 434 /* Buggy driver paranoia. */ 435 min = capture->form.fmt.pix.width * 2; 436 437 if (capture->form.fmt.pix.bytesperline < min) 438 capture->form.fmt.pix.bytesperline = min; 439 440 min = capture->form.fmt.pix.bytesperline * capture->form.fmt.pix.height; 441 442 if (capture->form.fmt.pix.sizeimage < min) 443 capture->form.fmt.pix.sizeimage = min; 444 445 return 1; 446 } 447 int V4LCamera::icvSetVideoSize( CvCaptureCAM_V4L* capture, int w, int h) { 448 449 450 451 CLEAR (capture->crop); 452 capture->crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 453 capture->crop.c.left = 0; 454 capture->crop.c.top = 0; 455 capture->crop.c.height = h*24; 456 capture->crop.c.width = w*24; 457 458 /* set the crop area, but don't exit if the device don't support croping */ 459 xioctl (capture->deviceHandle, VIDIOC_S_CROP, &capture->crop); 460 461 CLEAR (capture->form); 462 capture->form.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 463 464 /* read the current setting, mainly to retreive the pixelformat information */ 465 xioctl (capture->deviceHandle, VIDIOC_G_FMT, &capture->form); 466 467 /* set the values we want to change */ 468 capture->form.fmt.pix.width = w; 469 capture->form.fmt.pix.height = h; 470 capture->form.fmt.win.chromakey = 0; 471 capture->form.fmt.win.field = V4L2_FIELD_ANY; 472 capture->form.fmt.win.clips = 0; 473 capture->form.fmt.win.clipcount = 0; 474 capture->form.fmt.pix.field = V4L2_FIELD_ANY; 475 476 /* ask the device to change the size 477 * don't test if the set of the size is ok, because some device 478 * don't allow changing the size, and we will get the real size 479 * later */ 480 xioctl (capture->deviceHandle, VIDIOC_S_FMT, &capture->form); 481 482 /* try to set framerate to 30 fps */ 483 struct v4l2_streamparm setfps; 484 memset (&setfps, 0, sizeof(struct v4l2_streamparm)); 485 setfps.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 486 setfps.parm.capture.timeperframe.numerator = 1; 487 setfps.parm.capture.timeperframe.denominator = 30; 488 xioctl (capture->deviceHandle, VIDIOC_S_PARM, &setfps); 489 490 /* we need to re-initialize some things, like buffers, because the size has 491 * changed */ 492 capture->FirstCapture = 1; 493 494 /* Get window info again, to get the real value */ 495 if (-1 == xioctl (capture->deviceHandle, VIDIOC_G_FMT, &capture->form)) 496 { 497 fprintf(stderr, "HIGHGUI ERROR: V4L/V4L2: Could not obtain specifics of capture window.\n\n"); 498 499 500 501 return 0; 502 } 503 504 return 0; 505 506 507 508 509 return 0; 510 511 } 512 int V4LCamera::try_palette_v4l2(CvCaptureCAM_V4L* capture, unsigned long colorspace) 513 { 514 CLEAR (capture->form); 515 516 capture->form.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 517 capture->form.fmt.pix.pixelformat = colorspace; 518 capture->form.fmt.pix.field = V4L2_FIELD_ANY; 519 capture->form.fmt.pix.width = DEFAULT_V4L_WIDTH; 520 capture->form.fmt.pix.height = DEFAULT_V4L_HEIGHT; 521 522 if (-1 == xioctl (capture->deviceHandle, VIDIOC_S_FMT, &capture->form)) 523 return -1; 524 525 526 if (colorspace != capture->form.fmt.pix.pixelformat) 527 return -1; 528 else 529 return 0; 530 } 531 int V4LCamera::autosetup_capture_mode_v4l2(CvCaptureCAM_V4L* capture) 532 { 533 if (try_palette_v4l2(capture, V4L2_PIX_FMT_YUYV) == 0) { 534 PALETTE_YUYV = 1; 535 } else if (try_palette_v4l2(capture, V4L2_PIX_FMT_UYVY) == 0) { 536 PALETTE_UYVY = 1; 537 } else { 538 fprintf(stderr, "HIGHGUI ERROR: V4L2: Pixel format of incoming image is unsupported by OpenCV\n"); 539 540 return -1; 541 } 542 543 return 0; 544 545 } 546 547 548 549 int V4LCamera::try_init_v4l2(CvCaptureCAM_V4L* capture) 550 { 551 552 // if detect = -1 then unable to open device 553 // if detect = 0 then detected nothing 554 // if detect = 1 then V4L2 device 555 int detect = 0; 556 557 558 if (detect == 0) 559 { 560 CLEAR (capture->cap); 561 if (-1 == xioctl (capture->deviceHandle, VIDIOC_QUERYCAP, &capture->cap)) 562 { 563 detect = 0; 564 565 566 } 567 else 568 { 569 CLEAR (capture->capability); 570 capture->capability.type = capture->cap.capabilities; 571 572 /* Query channels number */ 573 if (-1 != xioctl (capture->deviceHandle, VIDIOC_G_INPUT, &capture->capability.channels)) 574 { 575 detect = 1; 576 } 577 } 578 } 579 580 return detect; 581 582 } 583 /* 42 584 V4LCamera::V4LCamera(string name, 43 585 uint8_t camera_index, uint16_t width, uint16_t height, … … 221 763 close(device); 222 764 } 223 765 */ 766 int V4LCamera::v4l2_alloc_buffers (CvCaptureCAM_V4L *capture, char *deviceName) 767 { 768 CLEAR (capture->req); 769 770 unsigned int buffer_number = DEFAULT_V4L_BUFFERS; 771 772 try_again: 773 774 capture->req.count = buffer_number; 775 capture->req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 776 capture->req.memory = V4L2_MEMORY_MMAP; 777 778 if (-1 == xioctl (capture->deviceHandle, VIDIOC_REQBUFS, &capture->req)) 779 { 780 if (EINVAL == errno) 781 { 782 fprintf (stderr, "%s does not support memory mapping\n", deviceName); 783 } else { 784 perror ("VIDIOC_REQBUFS"); 785 } 786 /* free capture, and returns an error code */ 787 788 return -1; 789 } 790 791 if (capture->req.count < buffer_number) 792 { 793 if (buffer_number == 1) 794 { 795 fprintf (stderr, "Insufficient buffer memory on %s\n", deviceName); 796 797 /* free capture, and returns an error code */ 798 799 return -1; 800 } else { 801 buffer_number--; 802 fprintf (stderr, "Insufficient buffer memory on %s -- decreaseing buffers\n", deviceName); 803 804 goto try_again; 805 } 806 } 807 808 for (n_buffers = 0; n_buffers < capture->req.count; ++n_buffers) 809 { 810 struct v4l2_buffer buf; 811 812 CLEAR (buf); 813 814 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 815 buf.memory = V4L2_MEMORY_MMAP; 816 buf.index = n_buffers; 817 818 if (-1 == xioctl (capture->deviceHandle, VIDIOC_QUERYBUF, &buf)) { 819 perror ("VIDIOC_QUERYBUF"); 820 821 /* free capture, and returns an error code */ 822 823 return -1; 824 } 825 826 capture->buffers[n_buffers].length = buf.length; 827 capture->buffers[n_buffers].start = 828 mmap (NULL /* start anywhere */, 829 buf.length, 830 PROT_READ | PROT_WRITE /* required */, 831 MAP_SHARED /* recommended */, 832 capture->deviceHandle, buf.m.offset); 833 834 if (MAP_FAILED == capture->buffers[n_buffers].start) { 835 perror ("mmap"); 836 837 /* free capture, and returns an error code */ 838 839 return -1; 840 } 841 842 843 } 844 845 /* Set up Image data */ 846 /* 847 cvInitImageHeader( &capture->frame, 848 cvSize( capture->captureWindow.width, 849 capture->captureWindow.height ), 850 IPL_DEPTH_8U, 3, IPL_ORIGIN_TL, 4 );*/ 851 /* Allocate space for RGBA data */ 852 capture->imageSize=capture->form.fmt.pix.width*capture->form.fmt.pix.height*2; 853 capture->imageData = AllocFunction(capture->imageSize); 854 Printf("cmem allocated %i at %x\n",capture->imageSize,capture->imageData); 855 856 return 1; 857 }; 858 int V4LCamera::cvGrabFrame(CvCaptureCAM_V4L* capture) { 859 860 if (capture->FirstCapture) { 861 /* Some general initialization must take place the first time through */ 862 863 /* This is just a technicality, but all buffers must be filled up before any 864 staggered SYNC is applied. SO, filler up. (see V4L HowTo) */ 865 866 867 v4l2_alloc_buffers (capture, NULL); 868 869 for (capture->bufferIndex = 0; 870 capture->bufferIndex < ((int)capture->req.count); 871 ++capture->bufferIndex) 872 { 873 874 struct v4l2_buffer buf; 875 876 CLEAR (buf); 877 878 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 879 buf.memory = V4L2_MEMORY_MMAP; 880 buf.index = (unsigned long)capture->bufferIndex; 881 882 if (-1 == xioctl (capture->deviceHandle, VIDIOC_QBUF, &buf)) { 883 perror ("VIDIOC_QBUF"); 884 return 0; 885 } 886 } 887 888 /* enable the streaming */ 889 capture->type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 890 if (-1 == xioctl (capture->deviceHandle, VIDIOC_STREAMON, 891 &capture->type)) { 892 /* error enabling the stream */ 893 perror ("VIDIOC_STREAMON"); 894 return 0; 895 } 896 897 898 899 900 /* preparation is ok */ 901 capture->FirstCapture = 0; 902 } 903 904 905 906 mainloop_v4l2(capture); 907 908 909 910 return(1); 911 } 912 void V4LCamera::mainloop_v4l2(CvCaptureCAM_V4L* capture) { 913 unsigned int count; 914 915 count = 1; 916 917 while (count-- > 0) { 918 for (;;) { 919 fd_set fds; 920 struct timeval tv; 921 int r; 922 923 FD_ZERO (&fds); 924 FD_SET (capture->deviceHandle, &fds); 925 926 /* Timeout. */ 927 tv.tv_sec = 2; 928 tv.tv_usec = 0; 929 930 r = select (capture->deviceHandle+1, &fds, NULL, NULL, &tv); 931 932 if (-1 == r) { 933 if (EINTR == errno) 934 continue; 935 936 perror ("select"); 937 } 938 939 if (0 == r) { 940 fprintf (stderr, "select timeout\n"); 941 942 /* end the infinite loop */ 943 break; 944 } 945 946 if (read_frame_v4l2 (capture)) 947 break; 948 } 949 } 950 } 951 int V4LCamera::read_frame_v4l2(CvCaptureCAM_V4L* capture) { 952 struct v4l2_buffer buf; 953 954 CLEAR (buf); 955 956 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 957 buf.memory = V4L2_MEMORY_MMAP; 958 959 if (-1 == xioctl (capture->deviceHandle, VIDIOC_DQBUF, &buf)) { 960 switch (errno) { 961 case EAGAIN: 962 return 0; 963 964 case EIO: 965 /* Could ignore EIO, see spec. */ 966 967 /* fall through */ 968 969 default: 970 /* display the error and stop processing */ 971 perror ("VIDIOC_DQBUF"); 972 return 1; 973 } 974 } 975 976 if(buf.index >= capture->req.count) { 977 Thread::Err("buf.index >= capture->req.count\n"); 978 } 979 980 981 capture->bufferIndex = buf.index; 982 983 984 if (-1 == xioctl (capture->deviceHandle, VIDIOC_QBUF, &buf)) 985 perror ("VIDIOC_QBUF"); 986 987 return 1; 988 } 989 void V4LCamera::Run(void) { 990 Time cam_time, new_time, fpsNow, fpsPrev; 991 //IplImage *img; // raw image 992 int fpsCounter = 0; 993 994 // init image old 995 if (!cvGrabFrame(&capture)) { 996 Printf("Could not grab a frame\n"); 997 } 998 cam_time = GetTime(); 999 fpsPrev = cam_time; 1000 1001 while (!ToBeStopped()) { 1002 1003 1004 // fps counter 1005 fpsCounter++; 1006 if (fpsCounter == 100) { 1007 fpsNow = GetTime(); 1008 fps->SetText("fps: %.1f", 1009 fpsCounter / ((float)(fpsNow - fpsPrev) / 1000000000.)); 1010 fpsCounter = 0; 1011 fpsPrev = fpsNow; 1012 } 1013 1014 // cam properties 1015 if (gain->ValueChanged() == true && autogain->Value() == false) 1016 SetGain(gain->Value()); 1017 if (exposure->ValueChanged() == true && autoexposure->Value() == false) 1018 SetExposure(exposure->Value()); 1019 if (bright->ValueChanged() == true) 1020 SetBrightness(bright->Value()); 1021 if (sat->ValueChanged() == true) 1022 SetSaturation(sat->Value()); 1023 if (contrast->ValueChanged() == true) 1024 SetContrast(contrast->Value()); 1025 if (hue->ValueChanged() == true) 1026 SetHue(hue->Value()); 1027 //if (sharpness->ValueChanged() == true) 1028 // cvSetCaptureProperty(capture, CV_CAP_PROP_SHARPNESS, sharpness->Value()); 1029 if (autogain->ValueChanged() == true) { 1030 if (autogain->Value() == true) { 1031 gain->setEnabled(false); 1032 } else { 1033 gain->setEnabled(true); 1034 SetGain(gain->Value()); 1035 } 1036 SetAutoGain(autogain->Value()); 1037 } 1038 if (autoexposure->ValueChanged() == true) { 1039 if (autoexposure->Value() == true) { 1040 exposure->setEnabled(false); 1041 } else { 1042 exposure->setEnabled(true); 1043 SetExposure(exposure->Value()); 1044 } 1045 SetAutoExposure(autoexposure->Value()); 1046 } 1047 //if (awb->ValueChanged() == true) 1048 // cvSetCaptureProperty(capture, CV_CAP_PROP_AWB, awb->Value()); 1049 1050 // cam pictures 1051 cvRetrieveRawFrame(&capture); 1052 if (!cvGrabFrame(&capture)) { 1053 Printf("Could not grab a frame\n"); 1054 } 1055 new_time = GetTime(); 1056 1057 //check for ps3eye deconnection in hds uav 1058 if(new_time-cam_time>100*1000*1000) { 1059 Thread::Warn("delta trop grand\n"); 1060 hasProblems=true; 1061 } 1062 1063 output->GetMutex(); 1064 1065 output->buffer=capture.imageData; 1066 output->ReleaseMutex(); 1067 1068 output->SetDataTime(cam_time); 1069 ProcessUpdate(output); 1070 cam_time = new_time; 1071 } 1072 1073 //cvReleaseCapture(&capture); 1074 } 1075 1076 void V4LCamera::cvRetrieveRawFrame( CvCaptureCAM_V4L* capture) { 1077 1078 1079 1080 1081 /* Now get what has already been captured as a IplImage return */ 1082 1083 /* First, reallocate imageData if the frame size changed */ 1084 1085 1086 /* 1087 if(((unsigned long)capture->frame.width != capture->form.fmt.pix.width) 1088 || ((unsigned long)capture->frame.height != capture->form.fmt.pix.height)) { 1089 if (PALETTE_YUYV == 1 || PALETTE_UYVY == 1) 1090 { 1091 cvFree(&capture->frame.imageData); 1092 cvInitImageHeader( &capture->frame, 1093 cvSize( capture->form.fmt.pix.width, 1094 capture->form.fmt.pix.height ), 1095 IPL_DEPTH_8U,2, IPL_ORIGIN_TL, 4 ); 1096 capture->frame.imageData = (char *)cvAlloc(capture->frame.imageSize); 1097 }else 1098 { 1099 fprintf( stderr, 1100 "HIGHGUI ERROR: V4L: raw output not supported for this palette\n"); 1101 } 1102 1103 } 1104 1105 */ 1106 1107 1108 if (PALETTE_YUYV == 1){ 1109 memcpy((char *)capture->imageData, 1110 (char *)capture->buffers[capture->bufferIndex].start, 1111 capture->imageSize); 1112 } 1113 if (PALETTE_UYVY == 1){ 1114 memcpy((char *)capture->imageData, 1115 (char *)capture->buffers[capture->bufferIndex].start, 1116 capture->imageSize); 1117 } 1118 1119 1120 1121 1122 } 224 1123 int V4LCamera::QueueBuffer(int index) { 225 1124 struct v4l2_buffer buf; -
trunk/lib/FlairSensorActuator/src/V4LCamera.h
r338 r340 163 163 char* frame; 164 164 int nbBuffers; 165 166 typedef struct CvCaptureCAM_V4L 167 { 168 int deviceHandle; 169 int bufferIndex; 170 int FirstCapture; 171 struct video_capability capability; 172 struct video_window captureWindow; 173 struct video_picture imageProperties; 174 struct video_mbuf memoryBuffer; 175 struct video_mmap *mmaps; 176 char *memoryMap; 177 //IplImage frame; 178 char *imageData; 179 int imageSize; 180 181 /* V4L2 variables */ 182 buffer buffers[MAX_V4L_BUFFERS + 1]; 183 struct v4l2_capability cap; 184 struct v4l2_input inp; 185 struct v4l2_format form; 186 struct v4l2_crop crop; 187 struct v4l2_cropcap cropcap; 188 struct v4l2_requestbuffers req; 189 struct v4l2_jpegcompression compr; 190 struct v4l2_control control; 191 enum v4l2_buf_type type; 192 struct v4l2_queryctrl queryctrl; 193 struct v4l2_querymenu querymenu; 194 195 /* V4L2 control variables */ 196 int v4l2_brightness, v4l2_brightness_min, v4l2_brightness_max; 197 int v4l2_contrast, v4l2_contrast_min, v4l2_contrast_max; 198 int v4l2_saturation, v4l2_saturation_min, v4l2_saturation_max; 199 int v4l2_sharpness, v4l2_sharpness_min, v4l2_sharpness_max; 200 int v4l2_exposure, v4l2_exposure_min, v4l2_exposure_max; 201 int v4l2_hue, v4l2_hue_min, v4l2_hue_max; 202 int v4l2_gain, v4l2_gain_min, v4l2_gain_max; 203 int v4l2_autogain, v4l2_autogain_min, v4l2_autogain_max; 204 int v4l2_awb, v4l2_awb_min, v4l2_awb_max; 205 206 207 208 } CvCaptureCAM_V4L; 209 210 CvCaptureCAM_V4L capture; 211 int _capture_V4L2 (CvCaptureCAM_V4L *capture); 212 int try_init_v4l2(CvCaptureCAM_V4L* capture); 213 int autosetup_capture_mode_v4l2(CvCaptureCAM_V4L* capture); 214 int icvSetVideoSize( CvCaptureCAM_V4L* capture, int w, int h); 215 int try_palette_v4l2(CvCaptureCAM_V4L* capture, unsigned long colorspace); 216 int PALETTE_YUYV,PALETTE_UYVY; 217 int icvSetPropertyCAM_V4L( CvCaptureCAM_V4L* capture,int property_id, double value ); 218 int icvSetControl (CvCaptureCAM_V4L* capture,int property_id, double value) ; 219 int cvGrabFrame(CvCaptureCAM_V4L* capture); 220 int v4l2_alloc_buffers (CvCaptureCAM_V4L *capture, char *deviceName); 221 void mainloop_v4l2(CvCaptureCAM_V4L* capture); 222 int read_frame_v4l2(CvCaptureCAM_V4L* capture); 223 void cvRetrieveRawFrame( CvCaptureCAM_V4L* capture); 224 unsigned int n_buffers ; 165 225 }; 166 226 } // end namespace sensor -
trunk/lib/FlairSensorActuator/src/unexported/Bldc_impl.h
r15 r340 40 40 Bldc_impl(flair::actuator::Bldc *self, flair::gui::Layout *layout, 41 41 std::string name, uint8_t motors_count); 42 Bldc_impl(flair::actuator::Bldc *self, uint8_t motors_count); // simulation43 42 ~Bldc_impl(); 44 43 void UpdateFrom(const flair::core::io_data *data);
Note:
See TracChangeset
for help on using the changeset viewer.