<commit_before>#include "V4lCameraHandlerV6.h"

#include "Tools/Debug/NaoTHAssert.h"
//#include "Tools/Debug/Stopwatch.h"
#include "Tools/NaoTime.h"

#include "Representations/Infrastructure/Image.h"

#include <string.h>
#include <iostream>
#include <poll.h>
#include <linux/uvcvideo.h>
#include <linux/usb/video.h>

//Custom V4L control variables
#define V4L2_MT9M114_BRIGHTNESS_DARK (V4L2_CID_PRIVATE_BASE+1)
#define V4L2_MT9M114_AE_TARGET_GAIN (V4L2_CID_PRIVATE_BASE+2)
#define V4L2_MT9M114_AE_MIN_VIRT_AGAIN (V4L2_CID_PRIVATE_BASE+3)
#define V4L2_MT9M114_AE_MAX_VIRT_AGAIN (V4L2_CID_PRIVATE_BASE+4)
#define V4L2_MT9M114_AE_MIN_VIRT_DGAIN (V4L2_CID_PRIVATE_BASE+5)
#define V4L2_MT9M114_AE_MAX_VIRT_DGAIN (V4L2_CID_PRIVATE_BASE+6)

#define LOG "[CameraHandler " << currentCamera << "] "

using namespace naoth;
using namespace std;


V4lCameraHandlerV6::V4lCameraHandlerV6()
  :
  fd(-1), 
  buffers(NULL),
  n_buffers(0),
  currentImage(NULL),
  atLeastOneImageRetrieved(false),
  initialParamsSet(false),
  wasQueried(false),
  isCapturing(false),
  bufferSwitched(false),
  blockingCaptureModeEnabled(false),
  lastCameraSettingTimestamp(0)
{

  // NOTE: width, height and fps are not included here
  settingsOrder.push_back(CameraSettings::AutoExposition);
  settingsOrder.push_back(CameraSettings::AutoExpositionAlgorithm);
  settingsOrder.push_back(CameraSettings::Brightness);
  settingsOrder.push_back(CameraSettings::MinAnalogGain);
  settingsOrder.push_back(CameraSettings::MaxAnalogGain);
  settingsOrder.push_back(CameraSettings::TargetGain);


  settingsOrder.push_back(CameraSettings::AutoWhiteBalancing);

  settingsOrder.push_back(CameraSettings::Contrast);
  settingsOrder.push_back(CameraSettings::Saturation);
  settingsOrder.push_back(CameraSettings::Hue);
  settingsOrder.push_back(CameraSettings::Sharpness);
  settingsOrder.push_back(CameraSettings::Exposure);
  settingsOrder.push_back(CameraSettings::Gain);
  settingsOrder.push_back(CameraSettings::GammaCorrection);

  settingsOrder.push_back(CameraSettings::WhiteBalance);
  // this throws errors sometimes and slows down the robot, check whats wrong before activating it
//  settingsOrder.push_back(CameraSettings::BacklightCompensation);
  

  uvcSettingsOrder.push_back(CameraSettings::VerticalFlip);
  uvcSettingsOrder.push_back(CameraSettings::HorizontalFlip);

  for(int i = 0; i < CameraSettings::numOfCameraSetting; i++)  {
    currentSettings.data[i] = -1;
  }

  // set our IDs
  initIDMapping();
}

V4lCameraHandlerV6::~V4lCameraHandlerV6()
{
  std::cout << "[V4lCameraHandlerV6] stop wait" << std::endl;
  shutdown();
  std::cout << "[V4lCameraHandlerV6] stop done" << std::endl;
}

void V4lCameraHandlerV6::init(std::string camDevice, CameraInfo::CameraID camID, bool blockingMode)
{
  if(isCapturing) {
    shutdown();
  }

  currentCamera = camID;
  cameraName = camDevice;

  // open the device
  openDevice(blockingMode);
  initDevice();
  setFPS(30);

  // HACK (preserved settings): load the current settings from the driver,
  //                            so we don't have to set all of them again
  internalUpdateCameraSettings();

  setSingleCameraParameter(csConst[CameraSettings::Exposure], currentSettings.data[CameraSettings::Exposure], "Exposure");

  // print the retrieved settings
  for (int i = 0; i < CameraSettings::numOfCameraSetting; i++) {
    if (csConst[i] > -1) {
      cout << LOG << CameraSettings::getCameraSettingsName((CameraSettings::CameraSettingID)i)
           << " = " << currentSettings.data[i] << std::endl;
    }
  }

  startCapturing();
}

void V4lCameraHandlerV6::initIDMapping()
{
  // initialize with an invalid value in order not to be used when updating
  // the params in V4L
  for (int i = 0; i < CameraSettings::numOfCameraSetting; i++) {
    csConst[i] = -1;
    uvcExtensionSelector[i] = -1;
  }


  // map the existing parameters that can be used safely
  csConst[CameraSettings::Brightness] = V4L2_CID_BRIGHTNESS;
  csConst[CameraSettings::BrightnessDark] = V4L2_MT9M114_BRIGHTNESS_DARK;
  csConst[CameraSettings::Contrast] = V4L2_CID_CONTRAST;
  csConst[CameraSettings::Saturation] = V4L2_CID_SATURATION;
  csConst[CameraSettings::Hue] = V4L2_CID_HUE;

  uvcExtensionSelector[CameraSettings::VerticalFlip] = 13;
  uvcExtensionSelector[CameraSettings::HorizontalFlip] = 12;
  
  csConst[CameraSettings::Sharpness] = V4L2_CID_SHARPNESS;
  csConst[CameraSettings::AutoExposition] = V4L2_CID_EXPOSURE_AUTO;
  csConst[CameraSettings::AutoWhiteBalancing] = V4L2_CID_AUTO_WHITE_BALANCE;
  csConst[CameraSettings::Gain] = V4L2_CID_GAIN;
  csConst[CameraSettings::MinAnalogGain] = V4L2_MT9M114_AE_MIN_VIRT_AGAIN;
  csConst[CameraSettings::MaxAnalogGain] = V4L2_MT9M114_AE_MAX_VIRT_AGAIN;
  csConst[CameraSettings::TargetGain] = V4L2_MT9M114_AE_TARGET_GAIN;
  csConst[CameraSettings::GammaCorrection] = V4L2_CID_GAMMA;
  
  csConst[CameraSettings::Exposure] = V4L2_CID_EXPOSURE;
  //csConst[CameraSettings::WhiteBalance] = V4L2_CID_DO_WHITE_BALANCE;
  csConst[CameraSettings::WhiteBalance] = V4L2_CID_WHITE_BALANCE_TEMPERATURE;
  csConst[CameraSettings::BacklightCompensation] = V4L2_CID_BACKLIGHT_COMPENSATION;
  csConst[CameraSettings::PowerlineFrequency] = V4L2_CID_POWER_LINE_FREQUENCY;

//---------------------------------------------------------------------
// copied from the driver for information:
//---------------------------------------------------------------------
/*
V4L2_CID_BRIGHTNESS:
Range: [0 .. 255]
Default: 55
Description: Set brightness in auto exposure mode.

V4L2_CID_CONTRAST:
Range: [16 .. 64] (Fixed point number: 16 = 0.5, 64 = 2.0)
Default: 32
Description: Controls contrast enhancement and noise reduction values.

V4L2_CID_SATURATION:
Range: [0 .. 255]
Default: 128
Description: Zero means gray-scale, values > 128 result in a boosted saturation.

V4L2_CID_HUE:
Range: [-22° .. 22°]
Default: 0<commit_msg>Remove some more non existing parameters from config (don't attempt to read the values)<commit_after>#include "V4lCameraHandlerV6.h"

#include "Tools/Debug/NaoTHAssert.h"
//#include "Tools/Debug/Stopwatch.h"
#include "Tools/NaoTime.h"

#include "Representations/Infrastructure/Image.h"

#include <string.h>
#include <iostream>
#include <poll.h>
#include <linux/uvcvideo.h>
#include <linux/usb/video.h>

//Custom V4L control variables
#define V4L2_MT9M114_AE_TARGET_GAIN (V4L2_CID_PRIVATE_BASE+2)
#define V4L2_MT9M114_AE_MIN_VIRT_AGAIN (V4L2_CID_PRIVATE_BASE+3)
#define V4L2_MT9M114_AE_MAX_VIRT_AGAIN (V4L2_CID_PRIVATE_BASE+4)
#define V4L2_MT9M114_AE_MIN_VIRT_DGAIN (V4L2_CID_PRIVATE_BASE+5)
#define V4L2_MT9M114_AE_MAX_VIRT_DGAIN (V4L2_CID_PRIVATE_BASE+6)

#define LOG "[CameraHandler " << currentCamera << "] "

using namespace naoth;
using namespace std;


V4lCameraHandlerV6::V4lCameraHandlerV6()
  :
  fd(-1), 
  buffers(NULL),
  n_buffers(0),
  currentImage(NULL),
  atLeastOneImageRetrieved(false),
  initialParamsSet(false),
  wasQueried(false),
  isCapturing(false),
  bufferSwitched(false),
  blockingCaptureModeEnabled(false),
  lastCameraSettingTimestamp(0)
{

  // NOTE: width, height and fps are not included here
  settingsOrder.push_back(CameraSettings::AutoExposition);
  settingsOrder.push_back(CameraSettings::AutoExpositionAlgorithm);
  settingsOrder.push_back(CameraSettings::Brightness);
  settingsOrder.push_back(CameraSettings::MinAnalogGain);
  settingsOrder.push_back(CameraSettings::MaxAnalogGain);
  settingsOrder.push_back(CameraSettings::TargetGain);


  settingsOrder.push_back(CameraSettings::AutoWhiteBalancing);

  settingsOrder.push_back(CameraSettings::Contrast);
  settingsOrder.push_back(CameraSettings::Saturation);
  settingsOrder.push_back(CameraSettings::Hue);
  settingsOrder.push_back(CameraSettings::Sharpness);
  settingsOrder.push_back(CameraSettings::Exposure);
  settingsOrder.push_back(CameraSettings::Gain);
  settingsOrder.push_back(CameraSettings::GammaCorrection);

  settingsOrder.push_back(CameraSettings::WhiteBalance);  

  uvcSettingsOrder.push_back(CameraSettings::VerticalFlip);
  uvcSettingsOrder.push_back(CameraSettings::HorizontalFlip);

  for(int i = 0; i < CameraSettings::numOfCameraSetting; i++)  {
    currentSettings.data[i] = -1;
  }

  // set our IDs
  initIDMapping();
}

V4lCameraHandlerV6::~V4lCameraHandlerV6()
{
  std::cout << "[V4lCameraHandlerV6] stop wait" << std::endl;
  shutdown();
  std::cout << "[V4lCameraHandlerV6] stop done" << std::endl;
}

void V4lCameraHandlerV6::init(std::string camDevice, CameraInfo::CameraID camID, bool blockingMode)
{
  if(isCapturing) {
    shutdown();
  }

  currentCamera = camID;
  cameraName = camDevice;

  // open the device
  openDevice(blockingMode);
  initDevice();
  setFPS(30);

  // HACK (preserved settings): load the current settings from the driver,
  //                            so we don't have to set all of them again
  internalUpdateCameraSettings();

  setSingleCameraParameter(csConst[CameraSettings::Exposure], currentSettings.data[CameraSettings::Exposure], "Exposure");

  // print the retrieved settings
  for (int i = 0; i < CameraSettings::numOfCameraSetting; i++) {
    if (csConst[i] > -1) {
      cout << LOG << CameraSettings::getCameraSettingsName((CameraSettings::CameraSettingID)i)
           << " = " << currentSettings.data[i] << std::endl;
    }
  }

  startCapturing();
}

void V4lCameraHandlerV6::initIDMapping()
{
  // initialize with an invalid value in order not to be used when updating
  // the params in V4L
  for (int i = 0; i < CameraSettings::numOfCameraSetting; i++) {
    csConst[i] = -1;
    uvcExtensionSelector[i] = -1;
  }


  // map the existing parameters that can be used safely
  csConst[CameraSettings::Brightness] = V4L2_CID_BRIGHTNESS;
  csConst[CameraSettings::Contrast] = V4L2_CID_CONTRAST;
  csConst[CameraSettings::Saturation] = V4L2_CID_SATURATION;
  csConst[CameraSettings::Hue] = V4L2_CID_HUE;

  uvcExtensionSelector[CameraSettings::VerticalFlip] = 13;
  uvcExtensionSelector[CameraSettings::HorizontalFlip] = 12;
  
  csConst[CameraSettings::Sharpness] = V4L2_CID_SHARPNESS;
  csConst[CameraSettings::AutoExposition] = V4L2_CID_EXPOSURE_AUTO;
  csConst[CameraSettings::AutoWhiteBalancing] = V4L2_CID_AUTO_WHITE_BALANCE;
  csConst[CameraSettings::Gain] = V4L2_CID_GAIN;
  csConst[CameraSettings::MinAnalogGain] = V4L2_MT9M114_AE_MIN_VIRT_AGAIN;
  csConst[CameraSettings::MaxAnalogGain] = V4L2_MT9M114_AE_MAX_VIRT_AGAIN;
  csConst[CameraSettings::TargetGain] = V4L2_MT9M114_AE_TARGET_GAIN;
  csConst[CameraSettings::GammaCorrection] = V4L2_CID_GAMMA;
  
  csConst[CameraSettings::Exposure] = V4L2_CID_EXPOSURE;
  //csConst[CameraSettings::WhiteBalance] = V4L2_CID_DO_WHITE_BALANCE;
  csConst[CameraSettings::WhiteBalance] = V4L2_CID_WHITE_BALANCE_TEMPERATURE;

//---------------------------------------------------------------------
// copied from the driver for information:
//---------------------------------------------------------------------
/*
V4L2_CID_BRIGHTNESS:
Range: [0 .. 255]
Default: 55
Description: Set brightness in auto exposure mode.

V4L2_CID_CONTRAST:
Range: [16 .. 64] (Fixed point number: 16 = 0.5, 64 = 2.0)
Default: 32
Description: Controls contrast enhancement and noise reduction values.

V4L2_CID_SATURATION:
Range: [0 .. 255]
Default: 128
Description: Zero means gray-scale, values > 128 result in a boosted saturation.

V4L2_CID_HUE:
Range: [-22° .. 22°]
Default: 0
{'fraction_non_alphanumeric': 0.13349771046143008, 'fraction_numerical': 0.030908770693906306, 'mean_word_length': 5.236683141131246, 'pattern_counts': {'":': 0, '<': 63, '<?xml version=': 0, '>': 17, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '6270905', 'n_tokens_mistral': 4155, 'n_tokens_neox': 3945, 'n_words': 657}