void Camera::printStatus() {
float val;
std::cout << "CAMERA STATUS: " << std::endl;
dc1394_feature_get_absolute_value(camera, DC1394_FEATURE_FRAME_RATE, &val);
std::cout << " framerate : " << val << " fps" << std::endl;
uint32_t uval32;
dc1394_feature_get_value(camera, DC1394_FEATURE_SHUTTER, &uval32);
std::cout << " shutter : " << uval32 << "\n" << std::endl;
uint64_t pio_dir_addr = PIO_DIRECTION;
pio_dir_reg32 pio_dir_reg = readRegisterContent(pio_dir_addr);
std::cout << " gpio 0 as output configured : " << pio_dir_reg.io0_mode << std::endl;
std::cout << " gpio 1 as output configured : " << pio_dir_reg.io1_mode << std::endl;
std::cout << " gpio 2 as output configured : " << pio_dir_reg.io2_mode << std::endl;
std::cout << " gpio 3 as output configured : " << pio_dir_reg.io3_mode << "\n" << std::endl;
uint64_t strobe_ctrl_inq_addr = STROBE_CTRL_INQ;
strobe_ctrl_inq_reg32 strobe_ctrl_ing_reg = readRegisterContent(strobe_ctrl_inq_addr);
std::cout << " strobe 0 present : " << strobe_ctrl_ing_reg.strobe_0_inq << std::endl;
std::cout << " strobe 1 present : " << strobe_ctrl_ing_reg.strobe_1_inq << std::endl;
std::cout << " strobe 2 present : " << strobe_ctrl_ing_reg.strobe_2_inq << std::endl;
std::cout << " strobe 3 present : " << strobe_ctrl_ing_reg.strobe_3_inq << "\n" << std::endl;
uint64_t strobe_0_inq_addr = STROBE_0_INQ;
strobe_inq_reg32 strobe_0_inq_reg = readRegisterContent(strobe_0_inq_addr);
std::cout << " strobe_0_inq presence_inq : " << strobe_0_inq_reg.presence_inq << std::endl;
std::cout << " strobe_0_inq readout_inq : " << strobe_0_inq_reg.readout_inq << std::endl;
std::cout << " strobe_0_inq on_off_inq : " << strobe_0_inq_reg.on_off_inq << std::endl;
std::cout << " strobe_0_inq polarity_inq : " << strobe_0_inq_reg.polarity_inq << std::endl;
std::cout << " strobe_0_inq min_value : " << strobe_0_inq_reg.min_value << std::endl;
std::cout << " strobe_0_inq max_value : " << strobe_0_inq_reg.max_value << "\n" << std::endl;
uint64_t strobe_1_inq_addr = STROBE_1_INQ;
strobe_inq_reg32 strobe_1_inq_reg = readRegisterContent(strobe_1_inq_addr);
std::cout << " strobe_1_inq presence_inq : " << strobe_1_inq_reg.presence_inq << std::endl;
std::cout << " strobe_1_inq readout_inq : " << strobe_1_inq_reg.readout_inq << std::endl;
std::cout << " strobe_1_inq on_off_inq : " << strobe_1_inq_reg.on_off_inq << std::endl;
std::cout << " strobe_1_inq polarity_inq : " << strobe_1_inq_reg.polarity_inq << std::endl;
std::cout << " strobe_1_inq min_value : " << strobe_1_inq_reg.min_value << std::endl;
std::cout << " strobe_1_inq max_value : " << strobe_1_inq_reg.max_value << "\n" << std::endl;
uint64_t strobe_0_cnt_addr = STROBE_0_CNT;
strobe_cnt_reg32 strobe_0_cnt_reg = readRegisterContent(strobe_0_cnt_addr);
std::cout << " strobe_0_cnt presence_inq : " << strobe_0_cnt_reg.presence_inq << std::endl;
std::cout << " strobe_0_cnt on_off : " << strobe_0_cnt_reg.on_off << std::endl;
std::cout << " strobe_0_cnt signal_polarity : " << strobe_0_cnt_reg.signal_polarity << std::endl;
std::cout << " strobe_0_cnt delay_value : " << strobe_0_cnt_reg.delay_value << std::endl;
std::cout << " strobe_0_cnt duration_value : " << strobe_0_cnt_reg.duration_value << "\n" << std::endl;
uint64_t strobe_1_cnt_addr = STROBE_1_CNT;
strobe_cnt_reg32 strobe_1_cnt_reg = readRegisterContent(strobe_1_cnt_addr);
std::cout << " strobe_1_cnt presence_inq : " << strobe_1_cnt_reg.presence_inq << std::endl;
std::cout << " strobe_1_cnt on_off : " << strobe_1_cnt_reg.on_off << std::endl;
std::cout << " strobe_1_cnt signal_polarity : " << strobe_1_cnt_reg.signal_polarity << std::endl;
std::cout << " strobe_1_cnt delay_value : " << strobe_1_cnt_reg.delay_value << std::endl;
std::cout << " strobe_1_cnt duration_value : " << strobe_1_cnt_reg.duration_value << std::endl;
}
float FirewireVideo::GetGamma() const
{
float gamma;
err = dc1394_feature_get_absolute_value(camera,DC1394_FEATURE_GAMMA,&gamma);
if( err != DC1394_SUCCESS )
throw VideoException("Failed to read gamma");
return gamma;
}
float FirewireVideo::GetShutterTime() const
{
float shutter;
err = dc1394_feature_get_absolute_value(camera,DC1394_FEATURE_SHUTTER,&shutter);
if( err != DC1394_SUCCESS )
throw VideoException("Failed to read shutter");
return shutter;
}
int ar2VideoGetAbsValue1394(AR2VideoParam1394T *vid, int paramName, ARdouble *value)
{
float v;
switch (paramName)
{
case AR_VIDEO_1394_GAMMA:
if (dc1394_feature_get_absolute_value(vid->camera, DC1394_FEATURE_BRIGHTNESS, &v) != DC1394_SUCCESS)
{
ARLOGe("unable to get brightness.\n");
return -1;
}
*value = (float)v;
return 0;
}
return -1;
}
float of1394VideoGrabber::getShutter(){
float exp;
dc1394_feature_get_absolute_value(camera, DC1394_FEATURE_SHUTTER, &exp);
return exp;
}
float of1394VideoGrabber::getExposure(){
float exp;
dc1394_feature_get_absolute_value(camera, DC1394_FEATURE_EXPOSURE, &exp);
return exp;
}
/**************************************************************************************
** Client is asking us to start an exposure
***************************************************************************************/
bool FFMVCCD::StartExposure(float duration)
{
FILE *imagefile;
dc1394error_t err;
dc1394video_frame_t *frame;
int i;
int ms;
unsigned int val;
float gain = 1.0;
uint32_t uwidth, uheight;
float sub_length;
float fval;
ms = duration* 1000;
//IDMessage(getDeviceName(), "Doing %d sub exposures at %f %s each", sub_count, absShutter, prop_info.pUnits);
ExposureRequest=duration;
// Since we have only have one CCD with one chip, we set the exposure duration of the primary CCD
PrimaryCCD.setBPP(16);
PrimaryCCD.setExposureDuration(duration);
gettimeofday(&ExpStart,NULL);
InExposure=true;
IDMessage(getDeviceName(), "Exposure has begun.");
// Let's get a pointer to the frame buffer
uint8_t * image = PrimaryCCD.getFrameBuffer();
// Get width and height
int width = PrimaryCCD.getSubW() / PrimaryCCD.getBinX();
int height = PrimaryCCD.getSubH() / PrimaryCCD.getBinY();
memset(image, 0, PrimaryCCD.getFrameBufferSize());
if (duration != last_exposure_length) {
/* Calculate the number of exposures needed */
sub_count = duration / max_exposure;
if (ms % ((int) (max_exposure * 1000))) {
++sub_count;
}
sub_length = duration / sub_count;
IDMessage(getDeviceName(), "Triggering a %f second exposure using %d subs of %f seconds",
duration, sub_count, sub_length);
/* Set sub length */
#if 0
err = dc1394_feature_set_absolute_control(dcam, DC1394_FEATURE_SHUTTER, DC1394_ON);
if (err != DC1394_SUCCESS) {
IDMessage(getDeviceName(), "Failed to enable ansolute shutter control.");
}
#endif
err = dc1394_feature_set_absolute_value(dcam, DC1394_FEATURE_SHUTTER, sub_length);
if (err != DC1394_SUCCESS) {
IDMessage(getDeviceName(), "Unable to set shutter value.");
}
err = dc1394_feature_get_absolute_value(dcam, DC1394_FEATURE_SHUTTER, &fval);
if (err != DC1394_SUCCESS) {
IDMessage(getDeviceName(), "Unable to get shutter value.");
}
IDMessage(getDeviceName(), "Shutter value is %f.", fval);
}
/* Flush the DMA buffer */
while (1) {
err=dc1394_capture_dequeue(dcam, DC1394_CAPTURE_POLICY_POLL, &frame);
if (err != DC1394_SUCCESS) {
IDMessage(getDeviceName(), "Flushing DMA buffer failed!");
break;
}
if (!frame) {
break;
}
dc1394_capture_enqueue(dcam, frame);
}
/*-----------------------------------------------------------------------
* have the camera start sending us data
*-----------------------------------------------------------------------*/
IDMessage(getDeviceName(), "start transmission");
err=dc1394_video_set_transmission(dcam, DC1394_ON);
if (err != DC1394_SUCCESS) {
IDMessage(getDeviceName(), "Unable to start transmission");
return false;
}
// We're done
return true;
}
float ofxLibdc::getFeatureAbs(dc1394feature_t feature) const {
float value;
dc1394_feature_get_absolute_value(camera, feature, &value);
return value;
}
/** Get feature values.
*
* @pre feature_set_ initialized for this camera
*
* @param finfo pointer to information for this feature
* @param value [out] pointer where parameter value stored
* @param value2 [out] optional pointer for second parameter value
* for white balance. Otherwise NULL.
*/
void Features::getValues(dc1394feature_info_t *finfo,
double *value, double *value2)
{
dc1394feature_t feature = finfo->id;
dc1394error_t rc;
if (!finfo->readout_capable)
{
ROS_INFO_STREAM("feature " << featureName(feature)
<< " value not available from device");
return;
}
if (feature == DC1394_FEATURE_WHITE_BALANCE)
{
// handle White Balance separately, it has two components
if (finfo->absolute_capable && finfo->abs_control)
{
// supports reading and setting float value
// @todo get absolute White Balance values
rc = DC1394_FUNCTION_NOT_SUPPORTED;
}
else
{
// get integer White Balance values
uint32_t bu_val;
uint32_t rv_val;
rc = dc1394_feature_whitebalance_get_value(camera_, &bu_val, &rv_val);
if (DC1394_SUCCESS == rc)
{
// convert to double
*value = bu_val;
*value2 = rv_val;
}
}
if (DC1394_SUCCESS == rc)
{
ROS_DEBUG_STREAM("feature " << featureName(feature)
<< " Blue/U: " << *value
<< " Red/V: " << *value2);
}
else
{
ROS_WARN_STREAM("failed to get values for feature "
<< featureName(feature));
}
}
else
{
// other features only have one component
if (finfo->absolute_capable && finfo->abs_control)
{
// supports reading and setting float value
float fval;
rc = dc1394_feature_get_absolute_value(camera_, feature, &fval);
if (DC1394_SUCCESS == rc)
{
*value = fval; // convert to double
}
}
else // no float representation
{
uint32_t ival;
rc = dc1394_feature_get_value(camera_, feature, &ival);
if (DC1394_SUCCESS == rc)
{
*value = ival; // convert to double
}
}
if (DC1394_SUCCESS == rc)
{
ROS_DEBUG_STREAM("feature " << featureName(feature)
<< " has value " << *value);
}
else
{
ROS_WARN_STREAM("failed to get value of feature "
<< featureName(feature));
}
}
}
// Note:
// the following was tested on a IIDC camera over USB therefore might not work as
// well on a camera over proper firewire transport
void FirewireVideo::init_format7_camera(
uint64_t guid, int dma_frames,
dc1394speed_t iso_speed,
dc1394video_mode_t video_mode,
float framerate,
uint32_t width, uint32_t height,
uint32_t left, uint32_t top, bool reset_at_boot
) {
if(video_mode< DC1394_VIDEO_MODE_FORMAT7_0)
throw VideoException("roi can be specified only for format7 modes");
camera = dc1394_camera_new (d, guid);
if (!camera)
throw VideoException("Failed to initialize camera");
// Attempt to stop camera if it is already running
dc1394switch_t is_iso_on = DC1394_OFF;
dc1394_video_get_transmission(camera, &is_iso_on);
if (is_iso_on==DC1394_ON) {
dc1394_video_set_transmission(camera, DC1394_OFF);
}
cout << "Using camera with GUID " << camera->guid << endl;
if(reset_at_boot){
dc1394_camera_reset(camera);
}
//-----------------------------------------------------------------------
// setup mode and roi
//-----------------------------------------------------------------------
if(iso_speed >= DC1394_ISO_SPEED_800)
{
err=dc1394_video_set_operation_mode(camera, DC1394_OPERATION_MODE_1394B);
if( err != DC1394_SUCCESS )
throw VideoException("Could not set DC1394_OPERATION_MODE_1394B");
}
err=dc1394_video_set_iso_speed(camera, iso_speed);
if( err != DC1394_SUCCESS )
throw VideoException("Could not set iso speed");
// check that the required mode is actually supported
dc1394format7mode_t format7_info;
err = dc1394_format7_get_mode_info(camera, video_mode, &format7_info);
if( err != DC1394_SUCCESS )
throw VideoException("Could not get format7 mode info");
// safely set the video mode
err=dc1394_video_set_mode(camera, video_mode);
if( err != DC1394_SUCCESS )
throw VideoException("Could not set format7 video mode");
// set position to 0,0 so that setting any size within min and max is a valid command
err = dc1394_format7_set_image_position(camera, video_mode,0,0);
if( err != DC1394_SUCCESS )
throw VideoException("Could not set format7 image position");
// work out the desired image size
width = nearest_value(width, format7_info.unit_pos_x, 0, format7_info.max_size_x - left);
height = nearest_value(height, format7_info.unit_pos_y, 0, format7_info.max_size_y - top);
// set size
err = dc1394_format7_set_image_size(camera,video_mode,width,height);
if( err != DC1394_SUCCESS )
throw VideoException("Could not set format7 size");
// get the info again since many parameters depend on image size
err = dc1394_format7_get_mode_info(camera, video_mode, &format7_info);
if( err != DC1394_SUCCESS )
throw VideoException("Could not get format7 mode info");
// work out position of roi
left = nearest_value(left, format7_info.unit_size_x, format7_info.unit_size_x, format7_info.max_size_x - width);
top = nearest_value(top, format7_info.unit_size_y, format7_info.unit_size_y, format7_info.max_size_y - height);
// set roi position
err = dc1394_format7_set_image_position(camera,video_mode,left,top);
if( err != DC1394_SUCCESS )
throw VideoException("Could not set format7 size");
this->width = width;
this->height = height;
this->top = top;
this->left = left;
cout<<"roi: "<<left<<" "<<top<<" "<<width<<" "<<height<<" ";
//-----------------------------------------------------------------------
// setup frame rate
//-----------------------------------------------------------------------
err=dc1394_format7_set_packet_size(camera,video_mode, format7_info.max_packet_size);
if( err != DC1394_SUCCESS )
throw VideoException("Could not set format7 packet size");
if((framerate != MAX_FR) && (framerate != EXT_TRIG)){
//set the framerate by using the absolute feature as suggested by the
//folks at PointGrey
err = dc1394_feature_set_absolute_control(camera,DC1394_FEATURE_FRAME_RATE,DC1394_ON);
if( err != DC1394_SUCCESS )
throw VideoException("Could not turn on absolute frame rate control");
err = dc1394_feature_set_mode(camera,DC1394_FEATURE_FRAME_RATE,DC1394_FEATURE_MODE_MANUAL);
if( err != DC1394_SUCCESS )
throw VideoException("Could not make frame rate manual ");
err=dc1394_feature_set_absolute_value(camera,DC1394_FEATURE_FRAME_RATE,framerate);
if( err != DC1394_SUCCESS )
throw VideoException("Could not set format7 framerate ");
}
// ask the camera what is the resulting framerate (this assume that such a rate is actually
// allowed by the shutter time)
float value;
err=dc1394_feature_get_absolute_value(camera,DC1394_FEATURE_FRAME_RATE,&value);
if( err != DC1394_SUCCESS )
throw VideoException("Could not get framerate");
cout<<" framerate(shutter permitting):"<<value<<endl;
//-----------------------------------------------------------------------
// setup capture
//-----------------------------------------------------------------------
err=dc1394_capture_setup(camera,dma_frames, DC1394_CAPTURE_FLAGS_DEFAULT);
if( err != DC1394_SUCCESS )
throw VideoException("Could not setup camera - check settings");
Start();
}
// Note:
// the following was tested on a IIDC camera over USB therefore might not work as
// well on a camera over proper firewire transport
void FirewireVideo::init_format7_camera(
uint64_t guid, int dma_frames,
dc1394speed_t iso_speed,
dc1394video_mode_t video_mode,
int framerate,
uint32_t width, uint32_t height,
uint32_t left, uint32_t top, bool reset_at_boot
) {
if(video_mode< DC1394_VIDEO_MODE_FORMAT7_0)
throw VideoException("roi can be specified only for format7 modes");
camera = dc1394_camera_new (d, guid);
if (!camera)
throw VideoException("Failed to initialize camera");
// Attempt to stop camera if it is already running
dc1394switch_t is_iso_on = DC1394_OFF;
dc1394_video_get_transmission(camera, &is_iso_on);
if (is_iso_on==DC1394_ON) {
dc1394_video_set_transmission(camera, DC1394_OFF);
}
cout << "Using camera with GUID " << camera->guid << endl;
if(reset_at_boot){
dc1394_camera_reset(camera);
}
//-----------------------------------------------------------------------
// setup mode and roi
//-----------------------------------------------------------------------
if(iso_speed >= DC1394_ISO_SPEED_800)
{
err=dc1394_video_set_operation_mode(camera, DC1394_OPERATION_MODE_1394B);
if( err != DC1394_SUCCESS )
throw VideoException("Could not set DC1394_OPERATION_MODE_1394B");
}
err=dc1394_video_set_iso_speed(camera, iso_speed);
if( err != DC1394_SUCCESS )
throw VideoException("Could not set iso speed");
// check that the required mode is actually supported
dc1394format7mode_t format7_info;
err = dc1394_format7_get_mode_info(camera, video_mode, &format7_info);
if( err != DC1394_SUCCESS )
throw VideoException("Could not get format7 mode info");
// safely set the video mode
err=dc1394_video_set_mode(camera, video_mode);
if( err != DC1394_SUCCESS )
throw VideoException("Could not set format7 video mode");
// set position to 0,0 so that setting any size within min and max is a valid command
err = dc1394_format7_set_image_position(camera, video_mode,0,0);
if( err != DC1394_SUCCESS )
throw VideoException("Could not set format7 image position");
// work out the desired image size
width = nearest_value(width, format7_info.unit_pos_x, 0, format7_info.max_size_x - left);
height = nearest_value(height, format7_info.unit_pos_y, 0, format7_info.max_size_y - top);
// set size
err = dc1394_format7_set_image_size(camera,video_mode,width,height);
if( err != DC1394_SUCCESS )
throw VideoException("Could not set format7 size");
// get the info again since many parameters depend on image size
err = dc1394_format7_get_mode_info(camera, video_mode, &format7_info);
if( err != DC1394_SUCCESS )
throw VideoException("Could not get format7 mode info");
// work out position of roi
left = nearest_value(left, format7_info.unit_size_x, format7_info.unit_size_x, format7_info.max_size_x - width);
top = nearest_value(top, format7_info.unit_size_y, format7_info.unit_size_y, format7_info.max_size_y - height);
// set roi position
err = dc1394_format7_set_image_position(camera,video_mode,left,top);
if( err != DC1394_SUCCESS )
throw VideoException("Could not set format7 size");
this->width = width;
this->height = height;
this->top = top;
this->left = left;
cout<<"roi: "<<left<<" "<<top<<" "<<width<<" "<<height<<" ";
//-----------------------------------------------------------------------
// setup frame rate
//-----------------------------------------------------------------------
if((framerate == MAX_FR)||(framerate == EXT_TRIG)){
err=dc1394_format7_set_packet_size(camera,video_mode, format7_info.max_packet_size);
if( err != DC1394_SUCCESS )
throw VideoException("Could not set format7 packet size");
} else {
// setting packet size to get the desired frame rate according to the libdc docs
// does not do the trick, so for now we support only max frame rate
throw VideoException("In format 7 only max frame rate is currently supported");
// uint32_t depth;
// err = dc1394_format7_get_data_depth(camera, video_mode, &depth);
// if( err != DC1394_SUCCESS )
// throw VideoException("Could not get format7 depth");
//
// // the following is straight from the libdc docs
// double bus_period = bus_period_from_iso_speed(iso_speed);
//
// // work out the max number of packets that the bus can deliver
// int num_packets = (int) (1.0/(bus_period*framerate) + 0.5);
//
// if((num_packets > 4095)||(num_packets < 0))
// throw VideoException("number of format7 packets out of range");
//
// // work out what the packet size should be for the requested size and framerate
// uint32_t packet_size = (width*964*depth + (num_packets*8) - 1)/(num_packets*8);
// packet_size = nearest_value(packet_size,format7_info.unit_packet_size,format7_info.unit_packet_size,format7_info.max_packet_size);
//
// if(packet_size > format7_info.max_packet_size){
// throw VideoException("format7 requested frame rate and size exceed bus bandwidth");
// }
//
// err=dc1394_format7_set_packet_size(camera,video_mode, packet_size);
// if( err != DC1394_SUCCESS ){
// throw VideoException("Could not set format7 packet size");
// }
}
// ask the camera what is the resulting framerate (this assume that such a rate is actually
// allowed by the shutter time)
err = dc1394_feature_set_power(camera,DC1394_FEATURE_FRAME_RATE,DC1394_OFF);
if( err != DC1394_SUCCESS )
throw VideoException("Could not turn off frame rate");
float value;
err=dc1394_feature_get_absolute_value(camera,DC1394_FEATURE_FRAME_RATE,&value);
if( err != DC1394_SUCCESS )
throw VideoException("Could not get framerate");
cout<<" framerate(shutter permitting):"<<value<<endl;
//-----------------------------------------------------------------------
// setup capture
//-----------------------------------------------------------------------
err=dc1394_capture_setup(camera,dma_frames, DC1394_CAPTURE_FLAGS_DEFAULT);
if( err != DC1394_SUCCESS )
throw VideoException("Could not setup camera - check settings");
Start();
}