bool Features::setFramerate(const double & fps_suggested, double * fps_returned)
{
const double TRIGGER_PULSE_WIDTH = 100.0 * 1.0e-6; // 100 us
CameraSettingsBlueDevice common_settings(cam_);
double exposure_time = static_cast<double>(common_settings.expose_us.read()) * 1.0e-6;
double pixel_clock = static_cast<double>(common_settings.pixelClock_KHz.read()) * 1.0e3;
double frame_time = computeFrameTime();
double fps_max = 1.0 / (frame_time + exposure_time + 2 * TRIGGER_PULSE_WIDTH);
double fps;
// assuming that the BlueCougar runs in free running mode, this might be conservative
ROS_INFO("Timing info: px_clock=%f MHz, frametime=%f ms, fps_max=%f ", pixel_clock*1.0e-6, frame_time*1000, fps_max);
if (fps_suggested > fps_max)
{
ROS_WARN("FPS (%f) > FPS_max (%f), limiting to FPS_max", fps_suggested, fps_max);
fps = fps_max;
}
else
fps = fps_suggested;
if (fps_returned)
*fps_returned = fps;
// setting the framerate can be quite different for the blueDevice families ...
if (cam_->family.readS() == "mvBlueCOUGAR")
{
CameraSettingsBlueCOUGAR bluecougar_settings(cam_);
double fps_ret;
configure(bluecougar_settings.frameRate_Hz, fps_suggested, &fps_ret);
if (fps_returned)
*fps_returned = (double)fps_ret;
}
else if (cam_->family.readS() == "mvBlueFOX" && cam_->product.readS() != "mvBlueFOX-ML/IGC202dG")
{
IOSubSystemBlueFOX bluefox_IOs(cam_);
CameraSettingsBlueFOX bluefox_settings(cam_);
// define a HRTC program that results in a define image frequency
// the hardware real time controller shall be used to trigger an image
bluefox_settings.triggerSource.write(ctsRTCtrl);
// when the hardware real time controller switches the trigger signal to
// high the exposure of the image shall start
bluefox_settings.triggerMode.write(ctmOnHighLevel); // ctmOnHighLevel
// error checks
if (bluefox_IOs.RTCtrProgramCount() == 0)
{
// no HRTC controllers available (this never happens for the mvBluecougar)
ROS_WARN("NO HRTC Controllers available");
return false;
}
RTCtrProgram* pRTCtrlprogram = bluefox_IOs.getRTCtrProgram(0);
if (!pRTCtrlprogram)
{
// this only should happen if the system is short of memory
ROS_WARN("Short on Memory...");
return false;
}
// start of the program
// we need 5 steps for the program
pRTCtrlprogram->setProgramSize(5);
// wait a certain amount of time to achieve the desired frequency
int progStep = 0;
RTCtrProgramStep* pRTCtrlStep = 0;
pRTCtrlStep = pRTCtrlprogram->programStep(progStep++);
pRTCtrlStep->opCode.write(rtctrlProgWaitClocks);
pRTCtrlStep->clocks_us.write(static_cast<int>((1.0 / fps - TRIGGER_PULSE_WIDTH) * 1.0e6));
// trigger an image
pRTCtrlStep = pRTCtrlprogram->programStep(progStep++);
pRTCtrlStep->opCode.write(rtctrlProgTriggerSet);
// high time for the trigger signal (should not be smaller than 100 us)
pRTCtrlStep = pRTCtrlprogram->programStep(progStep++);
pRTCtrlStep->opCode.write(rtctrlProgWaitClocks);
pRTCtrlStep->clocks_us.write(static_cast<int>(TRIGGER_PULSE_WIDTH * 1.0e6));
// end trigger signal
pRTCtrlStep = pRTCtrlprogram->programStep(progStep++);
pRTCtrlStep->opCode.write(rtctrlProgTriggerReset);
// restart the program
pRTCtrlStep = pRTCtrlprogram->programStep(progStep++);
pRTCtrlStep->opCode.write(rtctrlProgJumpLoc);
pRTCtrlStep->address.write(0);
// start the program
pRTCtrlprogram->mode.write(rtctrlModeRun);
}
else
{
ROS_WARN_STREAM("Setting framerate for "<<cam_->family.read()<<" not implemented");
return false;
}
ROS_INFO("Framerate set to: %f fps", fps);
return true;
}
//-----------------------------------------------------------------------------
bool setupHRTC( Device* pDev, int frameRate_Hz, int exposureTime_us, int sensorHeadCount )
//-----------------------------------------------------------------------------
{
cout << "Trying to capture at " << frameRate_Hz << " frames per second. Please make sure the device can deliver this frame rate." << endl;
int frametime_us = static_cast<int>( 1000000.0 * ( 1.0 / static_cast<double>( frameRate_Hz ) ) );
const int TRIGGER_PULSE_WIDTH_us = 100;
if( frametime_us < 2 * TRIGGER_PULSE_WIDTH_us )
{
cout << "frame rate too high (" << frameRate_Hz << "). Will use 5 Hz." << endl;
frametime_us = 200000;
}
if( exposureTime_us > ( frametime_us - 2 * TRIGGER_PULSE_WIDTH_us ) )
{
cout << "exposure time too high(" << exposureTime_us << "). Will use " << frametime_us - 2 * TRIGGER_PULSE_WIDTH_us << " instead" << endl;
exposureTime_us = frametime_us - 2 * TRIGGER_PULSE_WIDTH_us;
}
{
CameraSettingsBlueCOUGAR csbd( pDev );
csbd.expose_us.write( exposureTime_us );
// define a HRTC program that results in a define image frequency
// the hardware real time controller shall be used to trigger an image
csbd.triggerSource.write( ctsRTCtrl );
csbd.triggerMode.write( ctmOnFallingEdge );
}
IOSubSystemCommon iossc( pDev );
// error checks
if( iossc.RTCtrProgramCount() == 0 )
{
// no HRTC controllers available
cout << "This device (" << pDev->product.read() << ") doesn't support HRTC" << endl;
return false;
}
RTCtrProgram* pRTCtrlprogram = iossc.getRTCtrProgram( 0 );
if( !pRTCtrlprogram )
{
// this only should happen if the system is short of memory
cout << "Error! No valid program. Short of memory?" << endl;
return false;
}
// start of the program
// we need 5 steps for the program
pRTCtrlprogram->setProgramSize( 5 );
// wait a certain amount of time to achieve the desired frequency
int progStep = 0;
int i = 0;
RTCtrProgramStep* pRTCtrlStep = 0;
pRTCtrlStep = pRTCtrlprogram->programStep( progStep++ );
pRTCtrlStep->opCode.write( rtctrlProgWaitClocks );
pRTCtrlStep->clocks_us.write( frametime_us - exposureTime_us );
// trigger both sensor heads
pRTCtrlStep = pRTCtrlprogram->programStep( progStep++ );
pRTCtrlStep->opCode.write( rtctrlProgTriggerSet );
for( i = 0; i < sensorHeadCount; i++ )
{
pRTCtrlStep->sensorHeads.write( digioOn, i );
}
// high time for the trigger signal (should not be smaller than 100 us)
pRTCtrlStep = pRTCtrlprogram->programStep( progStep++ );
pRTCtrlStep->opCode.write( rtctrlProgWaitClocks );
pRTCtrlStep->clocks_us.write( exposureTime_us );
// end trigger signal
pRTCtrlStep = pRTCtrlprogram->programStep( progStep++ );
pRTCtrlStep->opCode.write( rtctrlProgTriggerReset );
for( i = 0; i < sensorHeadCount; i++ )
{
pRTCtrlStep->sensorHeads.write( digioOff, i );
}
// restart the program
pRTCtrlStep = pRTCtrlprogram->programStep( progStep++ );
pRTCtrlStep->opCode.write( rtctrlProgJumpLoc );
pRTCtrlStep->address.write( 0 );
// start the program
pRTCtrlprogram->mode.write( rtctrlModeRun );
// Now this device will deliver synchronous images at exactly the desired frequency
// when it is constantly feed with image requests and the device can deliver
// images at this frequency.
return true;
}
