NaoCamera::NaoCamera(const char* device, CameraInfo::Camera camera, int width, int height, bool flip,
const CameraSettings::CameraSettingsCollection& settings, const Matrix5uc& autoExposureWeightTable) :
camera(camera),
WIDTH(width),
HEIGHT(height)
{
VERIFY((fd = open(device, O_RDWR | O_NONBLOCK)) != -1);
mapBuffers();
queueBuffers();
setImageFormat();
setFrameRate(1, 30);
checkSettingsAvailability();
specialSettings.horizontalFlip.value = flip ? 1 : 0;
setControlSetting(specialSettings.horizontalFlip);
specialSettings.verticalFlip.value = flip ? 1 : 0;
setControlSetting(specialSettings.verticalFlip);
setSettings(settings, autoExposureWeightTable);
writeCameraSettings();
readCameraSettings();
startCapturing();
}
void NaoCamera::setCameraParams() {
std::cout << "NaoCamera: Setting Camera Parameters (on the fly)" << std::endl;
// if(!setControlSetting(V4L2_CID_AUTO_WHITE_BALANCE, camera_params_.kCameraAutoWhiteBalance)) std::cerr << "Error setting auto white balance: " << errno << "\n";
// if(!setControlSetting(V4L2_CID_EXPOSURE_AUTO, camera_params_.kCameraExposureAuto)) std::cerr << "Error setting exposure auto: " << errno << "\n";
// if(!setControlSetting(V4L2_CID_BACKLIGHT_COMPENSATION, camera_params_.kCameraBacklightCompensation)) std::cerr << "Error setting backlight compensation: " << errno << "\n";
if(!setControlSetting(V4L2_CID_BRIGHTNESS, camera_params_.kCameraBrightness)) std::cerr << "Error setting brightness: " << errno << "\n"; //ONLY VALUE
if(!setControlSetting(V4L2_CID_CONTRAST, camera_params_.kCameraContrast)) std::cerr << "Error setting contrast: " << errno << "\n"; //0-53
if(!setControlSetting(V4L2_CID_SATURATION, camera_params_.kCameraSaturation)) std::cerr << "Error setting saturation: " << errno << "\n"; //0-255
if(!setControlSetting(V4L2_CID_HUE, camera_params_.kCameraHue)) std::cerr << "Error setting hue: " << errno << "\n"; // ONLY VALUE
//if(!setControlSetting(V4L2_CID_EXPOSURE, camera_params_.kCameraExposure)) std::cerr << "Error setting exposure: " << errno << "\n"; // ONLY VALUE
//if(!setControlSetting(V4L2_CID_GAIN, camera_params_.kCameraGain)) std::cerr << "Error setting gain: " << errno << "\n"; // ONLY VALUE
if(!setControlSetting(V4L2_CID_SHARPNESS, camera_params_.kCameraSharpness)) std::cerr << "Error setting sharpness: " << errno << "\n"; //0-4
}
bool NaoCamera::setControlSettings(std::list<CameraSettingsBH::V4L2Setting> controlsettings,
std::list<CameraSettingsBH::V4L2Setting> appliedControlSettings)
{
std::list<CameraSettingsBH::V4L2Setting>::iterator ait = appliedControlSettings.begin();
std::list<CameraSettingsBH::V4L2Setting>::const_iterator it = controlsettings.begin();
std::list<CameraSettingsBH::V4L2Setting>::const_iterator end = controlsettings.end();
bool success = true;
int counter = 0;
for(; it != end && timeStamp - lastCameraSettingTimestamp >= cameraSettingApplicationRate; ++it, ++ait, counter++)
{
if(it->value == ait->value)
{
continue; // This setting has successfully been applied, so we don't have to deal with it.
}
if(!setControlSetting(it->command, it->value))
{
OUTPUT_WARNING("NaoCamera: Setting camera control value failed: " << it->command);
success = false; // The settings should have been set but wasn't => no success.
}
else
{
appliedSettings.setSetting(*it);
lastCameraSettingTimestamp = timeStamp;
}
}
return success;
}
unsigned char NaoCamera::switchCamera ( unsigned char camera)
{
unsigned char cmd[2] = {camera, 0};
int flip = camera == NAO_UPPER_CAMERA ? 1 : 0;
int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
// disable streaming
assert (ioctl (fd, VIDIOC_STREAMOFF, &type) != -1);
// switch camera
int i2cfd = openI2CAdapter();
assert (i2c_smbus_write_block_data (i2cfd, 220, 1, cmd) != -1);
closeI2CAdapter (i2cfd);
setControlSetting (V4L2_CID_VFLIP, flip);
setControlSetting (V4L2_CID_HFLIP, flip);
// enable streaming
assert (ioctl (fd, VIDIOC_STREAMON, &type) != -1);
currentCamera = camera;
return camera;
}
void NaoCamera::initDefaultControlSettings()
{
// make sure automatic stuff is off
int flip = currentCamera == NAO_UPPER_CAMERA ? 1 : 0;
setControlSetting (V4L2_CID_HFLIP, flip);
setControlSetting (V4L2_CID_VFLIP, flip);
//std::list<CameraSettings::V4L2Setting> v4l2settings = settings.getInitSettings();
//assert(setControlSettings(v4l2settings));
setControlSetting (V4L2_CID_AUTOEXPOSURE , 0);
setControlSetting (V4L2_CID_AUTO_WHITE_BALANCE, 0);
setControlSetting (V4L2_CID_AUTOGAIN, 0);
setControlSetting (V4L2_CID_EXPOSURE_AUTO, V4L2_EXPOSURE_MANUAL);
setControlSetting (V4L2_CID_GAIN, 0xF0);
setControlSetting (V4L2_CID_BACKLIGHT_COMPENSATION, 0);
setControlSetting (V4L2_CID_SAT_AUTO, 0);
setControlSetting (V4L2_CID_HUE_AUTO, 0);
setControlSetting (V4L2_CID_AUTO_CONTRAST_CENTER, 0);
setControlSetting (V4L2_CID_POWER_LINE_FREQUENCY, V4L2_CID_POWER_LINE_FREQUENCY_DISABLED);
setControlSetting (V4L2_CID_HUE, 0);
setControlSetting (V4L2_CID_SATURATION, 255);
setControlSetting (V4L2_CID_BRIGHTNESS, 128);
setControlSetting (V4L2_CID_CONTRAST, 96);
setControlSetting (V4L2_CID_AWB_G_CHANNEL_GAIN, 64);
setControlSetting (V4L2_CID_RED_BALANCE, 128 ); //128
setControlSetting (V4L2_CID_BLUE_BALANCE, 128);
usleep (300 * 1000);
}
void NaoCamera::setDefaultSettings() {
std::cout << "NaoCamera: Initializing Camera Parameters to default" << std::endl;
if(!setControlSetting(V4L2_CID_HFLIP, hflip_)) std::cerr << "Error setting hflip: " << errno << "\n";
if(!setControlSetting(V4L2_CID_VFLIP, vflip_)) std::cerr << "Error setting vflip: " << errno << "\n";
if(!setControlSetting(V4L2_CID_AUTO_WHITE_BALANCE, 0)) std::cerr << "Error setting auto white balance: " << errno << "\n";
if(!setControlSetting(V4L2_CID_EXPOSURE_AUTO, 1)) std::cerr << "Error setting exposure auto: " << errno << "\n";
if(!setControlSetting(V4L2_CID_BACKLIGHT_COMPENSATION, 0x00)) std::cerr << "Error backlight compensation brightness: " << errno << "\n";
if(!setControlSetting(V4L2_CID_BRIGHTNESS, 220)) std::cerr << "Error setting brightness: " << errno << "\n"; //ONLY VALUE
if(!setControlSetting(V4L2_CID_CONTRAST, 53)) std::cerr << "Error setting contrast: " << errno << "\n"; //20-53
if(!setControlSetting(V4L2_CID_SATURATION, 128)) std::cerr << "Error setting saturation: " << errno << "\n"; //0-255
if(!setControlSetting(V4L2_CID_HUE, 0)) std::cerr << "Error setting hue: " << errno << "\n"; // ONLY VALUE
if(!setControlSetting(V4L2_CID_EXPOSURE, 0)) std::cerr << "Error setting exposure: " << errno << "\n"; // SETS IT TO ARBITRARY DEFAULT
if(!setControlSetting(V4L2_CID_GAIN, 32)) std::cerr << "Error setting gain: " << errno << "\n"; // 0-255
if(!setControlSetting(V4L2_CID_SHARPNESS, 0)) std::cerr << "Error setting sharpness: " << errno << "\n"; //0-7
}
void NaoCamera::initDefaultControlSettings(bool flip)
{
setControlSetting(V4L2_CID_HFLIP, flip ? 1 : 0);
setControlSetting(V4L2_CID_VFLIP, flip ? 1 : 0);
}
void ImageTranscriber::initSettings()
{
// DO NOT SCREW UP THE ORDER BELOW
setControlSetting(V4L2_CID_HFLIP, settings.hflip);
setControlSetting(V4L2_CID_VFLIP, settings.vflip);
// Auto exposure on (buggy driver, blah)
setControlSetting(V4L2_CID_EXPOSURE_AUTO, 1);
// Set most settings with auto exposure off
setControlSetting(V4L2_CID_BRIGHTNESS, settings.brightness);
setControlSetting(V4L2_CID_CONTRAST, settings.contrast);
setControlSetting(V4L2_CID_SATURATION, settings.saturation);
setControlSetting(V4L2_CID_HUE, settings.hue);
setControlSetting(V4L2_CID_SHARPNESS, settings.sharpness);
// Auto white balance and backlight comp off!
setControlSetting(V4L2_CID_AUTO_WHITE_BALANCE,
settings.auto_whitebalance);
setControlSetting(V4L2_CID_BACKLIGHT_COMPENSATION,
settings.backlight_compensation);
// Auto exposure back off
setControlSetting(V4L2_CID_EXPOSURE_AUTO, settings.auto_exposure);
setControlSetting(V4L2_CID_EXPOSURE, settings.exposure);
setControlSetting(V4L2_CID_GAIN, settings.gain);
// This is actually just the white balance setting!
setControlSetting(V4L2_CID_DO_WHITE_BALANCE, settings.white_balance);
}
void ImageTranscriber::initSettings()
{
// DO NOT SCREW UP THE ORDER BELOW
setControlSetting(V4L2_CID_HFLIP, settings.hflip);
setControlSetting(V4L2_CID_VFLIP, settings.vflip);
// Still need to turn this on to change brightness, grumble grumble
setControlSetting(V4L2_CID_EXPOSURE_AUTO, 1);
setControlSetting(V4L2_CID_BRIGHTNESS, settings.brightness);
setControlSetting(V4L2_CID_CONTRAST, settings.contrast);
setControlSetting(V4L2_CID_SATURATION, settings.saturation);
setControlSetting(V4L2_CID_HUE, settings.hue);
setControlSetting(V4L2_CID_SHARPNESS, settings.sharpness);
// Auto white balance, exposure, and backlight comp off!
setControlSetting(V4L2_CID_AUTO_WHITE_BALANCE,
settings.auto_whitebalance);
setControlSetting(V4L2_CID_BACKLIGHT_COMPENSATION,
settings.backlight_compensation);
setControlSetting(V4L2_CID_EXPOSURE_AUTO, settings.auto_exposure);
setControlSetting(V4L2_CID_EXPOSURE, settings.exposure);
setControlSetting(V4L2_CID_GAIN, settings.gain);
// This is actually just the white balance setting!
setControlSetting(V4L2_CID_DO_WHITE_BALANCE, settings.white_balance);
setControlSetting(V4L2_MT9M114_FADE_TO_BLACK, settings.fade_to_black);
}
