char Accelerometer_init() {
int16_t c = readRegister(WHO_AM_I_ADDRESS); // Read WHO_AM_I register
if (c == WHO_AM_I_VALUE) //c != ERROR ||
{
DBPRINT("Accelerometer is online...\n");
}
else
{
DBPRINT("Accelerometer: Failed to connect (0x%X).\n",(int16_t)c);
return FAILURE;
}
setStandbyMode(); // Must be in standby to change registers
// Set up the full scale range to 2, 4, or 8g.
char fsr = GSCALE;
if(fsr > 8) fsr = 8; // Limit G-Scale
fsr >>= 2; // 00 = 2G, 01 = 4A, 10 = 8G (pg. 20)
writeRegister(XYZ_DATA_CFG_ADDRESS, fsr);
setActiveMode();
Timer_new(TIMER_ACCELEROMETER, UPDATE_DELAY);
#ifdef USE_ACCUMULATOR
resetAccumulator();
#endif
haveReading = FALSE;
return SUCCESS;
}
INT UEyeCamDriver::setExtTriggerMode() {
if (!isConnected())
return IS_INVALID_CAMERA_HANDLE;
INT is_err = IS_SUCCESS;
if (!extTriggerModeActive()) {
setStandbyMode(); // No need to check for success
if ((is_err = is_EnableEvent(cam_handle_, IS_SET_EVENT_FRAME)) != IS_SUCCESS) {
std::cerr << "Could not enable frame event for UEye camera '"
<< cam_name_ << "' (" << err2str(is_err) << ")" << std::endl;
return is_err;
}
if ((is_err = is_SetExternalTrigger(cam_handle_, IS_SET_TRIGGER_HI_LO)) != IS_SUCCESS) {
std::cerr << "Could not enable falling-edge external trigger mode on UEye camera '"
<< cam_name_ << "' (" << err2str(is_err) << ")" << std::endl;
return is_err;
}
if ((is_err = is_CaptureVideo(cam_handle_, IS_DONT_WAIT)) != IS_SUCCESS) {
std::cerr << "Could not start external trigger live video mode on UEye camera '"
<< cam_name_ << "' (" << err2str(is_err) << ")" << std::endl;
return is_err;
}
std::cout << "Started falling-edge external trigger live video mode on UEye camera '" + cam_name_ + "'"
<< std::endl;
}
return is_err;
}
INT UEyeCamDriver::setSensorScaling(double& rate, bool reallocate_buffer) {
if (!isConnected())
return IS_INVALID_CAMERA_HANDLE;
INT is_err = IS_SUCCESS;
// Stop capture to prevent access to memory buffer
setStandbyMode();
SENSORSCALERINFO sensorScalerInfo;
is_err = is_GetSensorScalerInfo(cam_handle_, &sensorScalerInfo,
sizeof(sensorScalerInfo));
if (is_err == IS_NOT_SUPPORTED) {
std::cerr << "Internal image scaling is not supported by camera" << std::endl;
rate = 1.0;
cam_sensor_scaling_rate_ = 1.0;
return IS_SUCCESS;
} else if (is_err != IS_SUCCESS) {
std::cerr << "Could not obtain supported internal image scaling information ("
<< err2str(is_err) << ")" << std::endl;
rate = 1.0;
cam_sensor_scaling_rate_ = 1.0;
return is_err;
} else {
if (rate < sensorScalerInfo.dblMinFactor || rate
> sensorScalerInfo.dblMaxFactor) {
std::cerr << "Requested internal image scaling rate of " << rate
<< " is not within supported bounds of ["
<< sensorScalerInfo.dblMinFactor << ", "
<< sensorScalerInfo.dblMaxFactor
<< "]; not updating current rate of "
<< sensorScalerInfo.dblCurrFactor << std::endl;
rate = sensorScalerInfo.dblCurrFactor;
return IS_SUCCESS;
}
}
if ((is_err = is_SetSensorScaler(cam_handle_, IS_ENABLE_SENSOR_SCALER, rate))
!= IS_SUCCESS) {
std::cerr << "Could not set internal image scaling rate to " << rate
<< "X (" << err2str(is_err) << "); resetting to 1X"
<< std::endl;
rate = 1.0;
if ((is_err = is_SetSensorScaler(cam_handle_, IS_ENABLE_SENSOR_SCALER, rate))
!= IS_SUCCESS) {
std::cerr << "Could not set internal image scaling rate to 1X ("
<< err2str(is_err) << ")" << std::endl;
return is_err;
}
}
std::cout << "Updated internal image scaling rate to " << rate << "X" << std::endl;
cam_sensor_scaling_rate_ = rate;
return (reallocate_buffer ? reallocateCamBuffer() : IS_SUCCESS);
}
INT UEyeCamDriver::connectCam(int new_cam_ID) {
INT is_err = IS_SUCCESS;
int numCameras;
// Terminate any existing opened cameras
setStandbyMode();
// Updates camera ID if specified.
if (new_cam_ID >= 0) {
cam_id_ = new_cam_ID;
}
// Query for number of connected cameras
if ((is_err = is_GetNumberOfCameras(&numCameras)) != IS_SUCCESS) {
std::cerr << "Failed query for number of connected UEye cameras ("
<< err2str(is_err) << ")" << std::endl;
return is_err;
} else if (numCameras < 1) {
std::cerr << "No UEye cameras are connected" << std::endl;
return IS_NO_SUCCESS;
} // NOTE: previously checked if ID < numCameras, but turns out that ID can be arbitrary
// Attempt to open camera handle, and handle case where camera requires a
// mandatory firmware upload
cam_handle_ = (HIDS) cam_id_;
if ((is_err = is_InitCamera(&cam_handle_, NULL)) == IS_STARTER_FW_UPLOAD_NEEDED) {
INT uploadTimeMSEC = 25000;
is_GetDuration(cam_handle_, IS_STARTER_FW_UPLOAD, &uploadTimeMSEC);
std::cout << "Uploading new firmware to UEye camera '" << cam_name_
<< "'; please wait for about " << uploadTimeMSEC / 1000.0
<< " seconds" << std::endl;
// Attempt to re-open camera handle while triggering automatic firmware upload
cam_handle_ = (HIDS) (((INT) cam_handle_) | IS_ALLOW_STARTER_FW_UPLOAD);
is_err = is_InitCamera(&cam_handle_, NULL); // Will block for N seconds
}
if (is_err != IS_SUCCESS) {
std::cerr << "Could not open UEye camera ID " << cam_id_ << " ("
<< err2str(is_err) << ")" << std::endl;
return is_err;
}
// Set display mode to Device Independent Bitmap (DIB)
is_err = is_SetDisplayMode(cam_handle_, IS_SET_DM_DIB);
// Fetch sensor parameters
is_err = is_GetSensorInfo(cam_handle_, &cam_sensor_info_);
// Initialize local camera frame buffer
reallocateCamBuffer();
return is_err;
}
INT UEyeCamDriver::disconnectCam() {
INT is_err = IS_SUCCESS;
if (isConnected()) {
setStandbyMode();
// Release existing camera buffers
if (cam_buffer_ != NULL) {
is_err = is_FreeImageMem(cam_handle_, cam_buffer_, cam_buffer_id_);
}
cam_buffer_ = NULL;
// Release camera handle
is_err = is_ExitCamera(cam_handle_);
cam_handle_ = (HIDS) 0;
std::cout << "Disconnected UEye camera '" + cam_name_ + "'" << std::endl;
}
return is_err;
}
INT UEyeCamDriver::reallocateCamBuffer() {
INT is_err = IS_SUCCESS;
// Stop capture to prevent access to memory buffer
setStandbyMode();
if (cam_buffer_ != NULL) {
is_err = is_FreeImageMem(cam_handle_, cam_buffer_, cam_buffer_id_);
cam_buffer_ = NULL;
}
INT width = cam_aoi_.s32Width /
(cam_sensor_scaling_rate_ * cam_subsampling_rate_ * cam_binning_rate_);
INT height = cam_aoi_.s32Height /
(cam_sensor_scaling_rate_ * cam_subsampling_rate_ * cam_binning_rate_);
if ((is_err = is_AllocImageMem(cam_handle_, width, height, bits_per_pixel_,
&cam_buffer_, &cam_buffer_id_))
!= IS_SUCCESS) {
std::cerr << "Failed to allocate " << width << " x " << height
<< " image buffer" << std::endl;
return is_err;
}
if ((is_err = is_SetImageMem(cam_handle_, cam_buffer_, cam_buffer_id_)) != IS_SUCCESS) {
std::cerr << "Failed to associate an image buffer to the UEye camera driver" << std::endl;
return is_err;
}
if ((is_err = is_GetImageMemPitch(cam_handle_, &cam_buffer_pitch_)) != IS_SUCCESS) {
std::cerr << "Failed to query UEye camera buffer's pitch (a.k.a. stride)" << std::endl;
return is_err;
}
cam_buffer_size_ = cam_buffer_pitch_ * height;
std::cout << "Allocate internal memory - width: " << width << "; height: "
<< height << "; fetched pitch: " << cam_buffer_pitch_
<< "; expected bpp: " << bits_per_pixel_ << "; total size: "
<< cam_buffer_size_ << std::endl;
return is_err;
}
INT UEyeCamDriver::setFreeRunMode() {
if (!isConnected())
return IS_INVALID_CAMERA_HANDLE;
INT is_err = IS_SUCCESS;
if (!freeRunModeActive()) {
setStandbyMode(); // No need to check for success
// Set the flash to a high active pulse for each image in the trigger mode
INT flash_delay = 0;
UINT flash_duration = 1000;
setFlashParams(flash_delay, flash_duration);
UINT nMode = IO_FLASH_MODE_FREERUN_HI_ACTIVE;
if ((is_err = is_IO(cam_handle_, IS_IO_CMD_FLASH_SET_MODE,
(void*) &nMode, sizeof(nMode)))
!= IS_SUCCESS) {
std::cerr << "Could not set free-run active-low flash output for UEye camera '"
<< cam_name_ << "' (" << err2str(is_err) << ")" << std::endl;
return is_err;
}
if ((is_err = is_EnableEvent(cam_handle_, IS_SET_EVENT_FRAME)) != IS_SUCCESS) {
std::cerr << "Could not enable frame event for UEye camera '"
<< cam_name_ << "' (" << err2str(is_err) << ")" << std::endl;
return is_err;
}
if ((is_err = is_CaptureVideo(cam_handle_, IS_WAIT)) != IS_SUCCESS) {
std::cerr << "Could not start free-run live video mode on UEye camera '"
<< cam_name_ << "' (" << err2str(is_err) << ")" << std::endl;
return is_err;
}
std::cout << "Started live video mode on UEye camera '" + cam_name_ + "'" << std::endl;
}
return is_err;
}
INT UEyeCamDriver::setColorMode(string mode, bool reallocate_buffer) {
if (!isConnected())
return IS_INVALID_CAMERA_HANDLE;
INT is_err = IS_SUCCESS;
// Stop capture to prevent access to memory buffer
setStandbyMode();
// Set to specified color mode
if (mode == "rgb8") {
if ((is_err = is_SetColorMode(cam_handle_, IS_CM_RGB8_PACKED)) != IS_SUCCESS) {
std::cerr << "Could not set color mode to RGB8 (" << err2str(is_err)
<< ")" << std::endl;
return is_err;
}
bits_per_pixel_ = 24;
} else if (mode == "bayer_rggb8") {
if ((is_err = is_SetColorMode(cam_handle_, IS_CM_SENSOR_RAW8)) != IS_SUCCESS) {
std::cerr << "Could not set color mode to BAYER_RGGB8 ("
<< err2str(is_err) << ")" << std::endl;
return is_err;
}
bits_per_pixel_ = 8;
} else { // Default to MONO8
if ((is_err = is_SetColorMode(cam_handle_, IS_CM_MONO8)) != IS_SUCCESS) {
std::cerr << "Could not set color mode to MONO8 ("
<< err2str(is_err) << ")" << std::endl;
return is_err;
}
bits_per_pixel_ = 8;
}
std::cout << "Updated color mode to " << mode << std::endl;
return (reallocate_buffer ? reallocateCamBuffer() : IS_SUCCESS);
}
INT UEyeCamDriver::setBinning(int& rate, bool reallocate_buffer) {
if (!isConnected())
return IS_INVALID_CAMERA_HANDLE;
INT is_err = IS_SUCCESS;
// Stop capture to prevent access to memory buffer
setStandbyMode();
INT rate_flag;
INT supportedRates;
supportedRates = is_SetBinning(cam_handle_, IS_GET_SUPPORTED_BINNING);
switch (rate) {
case 1:
rate_flag = IS_BINNING_DISABLE;
break;
case 2:
rate_flag = IS_BINNING_2X;
break;
case 4:
rate_flag = IS_BINNING_4X;
break;
case 8:
rate_flag = IS_BINNING_8X;
break;
case 16:
rate_flag = IS_BINNING_16X;
break;
default:
std::cerr << "Invalid or unsupported binning rate: " << rate
<< ", resetting to 1X" << std::endl;
rate = 1;
rate_flag = IS_BINNING_DISABLE;
break;
}
if ((supportedRates & rate_flag) == rate_flag) {
if ((is_err = is_SetBinning(cam_handle_, rate_flag)) != IS_SUCCESS) {
std::cerr << "Could not set binning rate to " << rate << "X ("
<< err2str(is_err) << ")" << std::endl;
return is_err;
}
} else {
std::cerr << "Camera does not support requested binning rate of "
<< rate << std::endl;
// Query current rate
INT currRate = is_SetBinning(cam_handle_, IS_GET_BINNING);
if (currRate == IS_BINNING_DISABLE) {
rate = 1;
} else if (currRate == IS_BINNING_2X) {
rate = 2;
} else if (currRate == IS_BINNING_4X) {
rate = 4;
} else if (currRate == IS_BINNING_8X) {
rate = 8;
} else if (currRate == IS_BINNING_16X) {
rate = 16;
} else {
std::cerr << "ColorCamera.has unsupported binning rate (" << currRate
<< "), resetting to 1X" << std::endl;
if ((is_err = is_SetBinning(cam_handle_, IS_BINNING_DISABLE)) != IS_SUCCESS) {
std::cerr << "Could not set binning rate to 1X ("
<< err2str(is_err) << ")" << std::endl;
return is_err;
}
}
return IS_SUCCESS;
}
std::cout << "Updated binning rate to " << rate << "X" << std::endl;
cam_binning_rate_ = rate;
return (reallocate_buffer ? reallocateCamBuffer() : IS_SUCCESS);
}
void ADXL345::init(unsigned int address)
{
_dev_address = address;
setStandbyMode();
setMeasureMode();
}
