f200.cpp
Go to the documentation of this file.
1 // License: Apache 2.0. See LICENSE file in root directory.
2 // Copyright(c) 2015 Intel Corporation. All Rights Reserved.
3 
4 #define _USE_MATH_DEFINES
5 #include <math.h>
6 #include <algorithm>
7 
8 #include "image.h"
9 #include "ivcam-private.h"
10 #include "f200.h"
11 
12 using namespace rsimpl::f200;
13 
14 namespace rsimpl
15 {
16  const std::vector <std::pair<rs_option, char>> eu_F200_depth_controls = {{rs_option::RS_OPTION_F200_LASER_POWER, 0x01},
22 
23  static const cam_mode f200_color_modes[] = {
24  {{1920, 1080}, {2,5,15,30}},
25  {{1280, 720}, {2,5,15,30}},
26  {{ 960, 540}, {2,5,15,30,60}},
27  {{ 848, 480}, {2,5,15,30,60}},
28  {{ 640, 480}, {2,5,15,30,60}},
29  {{ 640, 360}, {2,5,15,30,60}},
30  {{ 424, 240}, {2,5,15,30,60}},
31  {{ 320, 240}, {2,5,15,30,60}},
32  {{ 320, 180}, {2,5,15,30,60}}
33  };
34  static const cam_mode f200_depth_modes[] = {
35  {{640, 480}, {2,5,15,30,60}},
36  {{640, 240}, {2,5,15,30,60}} // 110 FPS is excluded due to a bug in timestamp field
37  };
38  static const cam_mode f200_ir_only_modes[] = {
39  {{640, 480}, {30,60,120,240,300}},
40  {{640, 240}, {30,60,120,240,300}}
41  };
42 
43  static static_device_info get_f200_info(std::shared_ptr<uvc::device> /*device*/, const ivcam::camera_calib_params & c)
44  {
45  LOG_INFO("Connecting to Intel RealSense F200");
46  static_device_info info;
47 
48  info.name = "Intel RealSense F200";
49 
50  // Color modes on subdevice 0
52  for(auto & m : f200_color_modes)
53  {
54  for(auto fps : m.fps)
55  {
56  info.subdevice_modes.push_back({0, m.dims, pf_yuy2, fps, MakeColorIntrinsics(c, m.dims), {}, {0}});
57  }
58  }
59 
60  // Depth and IR modes on subdevice 1
63  for(auto & m : f200_ir_only_modes)
64  {
65  for(auto fps : m.fps)
66  {
67  info.subdevice_modes.push_back({1, m.dims, pf_f200_invi, fps, MakeDepthIntrinsics(c, m.dims), {}, {0}});
68  }
69  }
70  for(auto & m : f200_depth_modes)
71  {
72  for(auto fps : m.fps)
73  {
74  info.subdevice_modes.push_back({1, m.dims, pf_invz, fps, MakeDepthIntrinsics(c, m.dims), {}, {0}});
75  info.subdevice_modes.push_back({1, m.dims, pf_f200_inzi, fps, MakeDepthIntrinsics(c, m.dims), {}, {0}});
76  }
77  }
78 
80  info.presets[RS_STREAM_DEPTH ][RS_PRESET_BEST_QUALITY] = {true, 640, 480, RS_FORMAT_Z16, 60, RS_OUTPUT_BUFFER_FORMAT_CONTINUOUS};
81  info.presets[RS_STREAM_COLOR ][RS_PRESET_BEST_QUALITY] = {true, 640, 480, RS_FORMAT_RGB8, 60, RS_OUTPUT_BUFFER_FORMAT_CONTINUOUS};
82 
83  info.presets[RS_STREAM_INFRARED][RS_PRESET_LARGEST_IMAGE] = {true, 640, 480, RS_FORMAT_Y8, 60, RS_OUTPUT_BUFFER_FORMAT_CONTINUOUS};
84  info.presets[RS_STREAM_DEPTH ][RS_PRESET_LARGEST_IMAGE] = {true, 640, 480, RS_FORMAT_Z16, 60, RS_OUTPUT_BUFFER_FORMAT_CONTINUOUS};
85  info.presets[RS_STREAM_COLOR ][RS_PRESET_LARGEST_IMAGE] = {true, 1920, 1080, RS_FORMAT_RGB8, 60, RS_OUTPUT_BUFFER_FORMAT_CONTINUOUS};
86 
87  info.presets[RS_STREAM_INFRARED][RS_PRESET_HIGHEST_FRAMERATE] = {true, 640, 480, RS_FORMAT_Y8, 60, RS_OUTPUT_BUFFER_FORMAT_CONTINUOUS};
88  info.presets[RS_STREAM_DEPTH ][RS_PRESET_HIGHEST_FRAMERATE] = {true, 640, 480, RS_FORMAT_Z16, 60, RS_OUTPUT_BUFFER_FORMAT_CONTINUOUS};
89  info.presets[RS_STREAM_COLOR ][RS_PRESET_HIGHEST_FRAMERATE] = {true, 640, 480, RS_FORMAT_RGB8, 60, RS_OUTPUT_BUFFER_FORMAT_CONTINUOUS};
90 
91  // Hardcoded extension controls
92  // option min max step def
93  // ------ --- --- ---- ---
94  info.options.push_back({ RS_OPTION_F200_LASER_POWER, 0, 16, 1, 16 });
95  info.options.push_back({ RS_OPTION_F200_ACCURACY, 1, 3, 1, 2 });
96  info.options.push_back({ RS_OPTION_F200_MOTION_RANGE, 0, 100, 1, 0 });
97  info.options.push_back({ RS_OPTION_F200_FILTER_OPTION, 0, 7, 1, 5 });
98  info.options.push_back({ RS_OPTION_F200_CONFIDENCE_THRESHOLD, 0, 15, 1, 6 });
99  info.options.push_back({ RS_OPTION_F200_DYNAMIC_FPS, 0, 1000, 1, 60 });
100 
101  rsimpl::pose depth_to_color = {transpose((const float3x3 &)c.Rt), (const float3 &)c.Tt * 0.001f}; // convert mm to m
102  info.stream_poses[RS_STREAM_DEPTH] = info.stream_poses[RS_STREAM_INFRARED] = inverse(depth_to_color);
103  info.stream_poses[RS_STREAM_COLOR] = {{{1,0,0},{0,1,0},{0,0,1}}, {0,0,0}};
104 
105  info.nominal_depth_scale = (c.Rmax / 0xFFFF) * 0.001f; // convert mm to m
108  return info;
109  }
110 
111  f200_camera::f200_camera(std::shared_ptr<uvc::device> device, const static_device_info & info, const ivcam::camera_calib_params & calib, const f200::cam_temperature_data & temp, const f200::thermal_loop_params & params) :
112  iv_camera(device, info, calib),
113  base_temperature_data(temp),
114  thermal_loop_params(params),
115  last_temperature_delta(std::numeric_limits<float>::infinity())
116  {
117  // If thermal control loop requested, start up thread to handle it
119  {
120  runTemperatureThread = true;
122  }
123  }
124 
126  {
127  // Shut down thermal control loop thread
128  runTemperatureThread = false;
129  temperatureCv.notify_one();
130  if (temperatureThread.joinable())
131  temperatureThread.join();
132  }
133 
135  {
138  const float tempFromHFOV = (tan(thermal_loop_params.HFOVsensitivity*(float)M_PI/360)*(1 + base_calibration.Kc[0][0]*base_calibration.Kc[0][0]))/(FcxSlope * (1 + base_calibration.Kc[0][0] * tan(thermal_loop_params.HFOVsensitivity * (float)M_PI/360)));
139  float TempThreshold = thermal_loop_params.TempThreshold; //celcius degrees, the temperatures delta that above should be fixed;
140  if (TempThreshold <= 0) TempThreshold = tempFromHFOV;
141  if (TempThreshold > tempFromHFOV) TempThreshold = tempFromHFOV;
142 
143  std::unique_lock<std::mutex> lock(temperatureMutex);
144  while (runTemperatureThread)
145  {
146  temperatureCv.wait_for(lock, std::chrono::seconds(10));
147 
148  // todo - this will throw if bad, but might periodically fail anyway. try/catch
149  try
150  {
151  float IRTemp = (float)f200::read_ir_temp(get_device(), usbMutex);
152  float LiguriaTemp = f200::read_mems_temp(get_device(), usbMutex);
153 
154  double IrBaseTemperature = base_temperature_data.IRTemp; //should be taken from the parameters
155  double liguriaBaseTemperature = base_temperature_data.LiguriaTemp; //should be taken from the parameters
156 
157  // calculate deltas from the calibration and last fix
158  double IrTempDelta = IRTemp - IrBaseTemperature;
159  double liguriaTempDelta = LiguriaTemp - liguriaBaseTemperature;
160  double weightedTempDelta = liguriaTempDelta * thermal_loop_params.LiguriaTempWeight + IrTempDelta * thermal_loop_params.IrTempWeight;
161  double tempDeltaFromLastFix = fabs(weightedTempDelta - last_temperature_delta);
162 
163  // read intrinsic from the calibration working point
164  double Kc11 = base_calibration.Kc[0][0];
165  double Kc13 = base_calibration.Kc[0][2];
166 
167  // Apply model
168  if (tempDeltaFromLastFix >= TempThreshold)
169  {
170  // if we are during a transition, fix for after the transition
171  double tempDeltaToUse = weightedTempDelta;
172  if (tempDeltaToUse > 0 && tempDeltaToUse < thermal_loop_params.TransitionTemp)
173  {
174  tempDeltaToUse = thermal_loop_params.TransitionTemp;
175  }
176 
177  // calculate fixed values
178  double fixed_Kc11 = Kc11 + (FcxSlope * tempDeltaToUse) + thermal_loop_params.FcxOffset;
179  double fixed_Kc13 = Kc13 + (UxSlope * tempDeltaToUse) + thermal_loop_params.UxOffset;
180 
181  // write back to intrinsic hfov and vfov
182  auto compensated_calibration = base_calibration;
183  compensated_calibration.Kc[0][0] = (float) fixed_Kc11;
184  compensated_calibration.Kc[1][1] = base_calibration.Kc[1][1] * (float)(fixed_Kc11/Kc11);
185  compensated_calibration.Kc[0][2] = (float) fixed_Kc13;
186 
187  // todo - Pass the current resolution into update_asic_coefficients
188  LOG_INFO("updating asic with new temperature calibration coefficients");
189  update_asic_coefficients(get_device(), usbMutex, compensated_calibration);
190  last_temperature_delta = (float)weightedTempDelta;
191  }
192  }
193  catch(const std::exception & e) { LOG_ERROR("TemperatureControlLoop: " << e.what()); }
194  }
195  }
196 
197  void f200_camera::start_fw_logger(char /*fw_log_op_code*/, int /*grab_rate_in_ms*/, std::timed_mutex& /*mutex*/)
198  {
199  throw std::logic_error("Not implemented");
200  }
202  {
203  throw std::logic_error("Not implemented");
204  }
205 
206  void f200_camera::set_options(const rs_option options[], size_t count, const double values[])
207  {
208  std::vector<rs_option> base_opt;
209  std::vector<double> base_opt_val;
210 
211 
212  for(size_t i=0; i<count; ++i)
213  {
214  switch(options[i])
215  {
216  case RS_OPTION_F200_DYNAMIC_FPS: f200::set_dynamic_fps(get_device(), static_cast<uint8_t>(values[i])); break; // IVCAM 1.0 Only
217  // Default will be handled by parent implementation
218  default: base_opt.push_back(options[i]); base_opt_val.push_back(values[i]); break;
219  }
220  }
221 
222  //Set common options
223  if (!base_opt.empty())
224  iv_camera::set_options(base_opt.data(), base_opt.size(), base_opt_val.data());
225  }
226 
227  void f200_camera::get_options(const rs_option options[], size_t count, double values[])
228  {
229  std::vector<rs_option> base_opt;
230  std::vector<size_t> base_opt_index;
231  std::vector<double> base_opt_val;
232 
233  for (size_t i = 0; i<count; ++i)
234  {
235  LOG_INFO("Reading option " << options[i]);
236 
237  if(uvc::is_pu_control(options[i]))
238  {
239  values[i] = uvc::get_pu_control_with_retry(get_device(), 0, options[i]);
240  continue;
241  }
242 
243  uint8_t val=0;
244  switch(options[i])
245  {
246  case RS_OPTION_F200_DYNAMIC_FPS: f200::get_dynamic_fps(get_device(), val); values[i] = val; break;
247 
248  // Default will be handled by parent implementation
249  default: base_opt.push_back(options[i]); base_opt_index.push_back(i); break;
250  }
251  }
252 
253  //Retrieve the common options
254  if (base_opt.size())
255  {
256  base_opt_val.resize(base_opt.size());
257  iv_camera::get_options(base_opt.data(), base_opt.size(), base_opt_val.data());
258  }
259 
260  // Merge the local data with values obtained by base class
261  for (auto i : base_opt_index)
262  values[i] = base_opt_val[i];
263  }
264 
265  std::shared_ptr<rs_device> make_f200_device(std::shared_ptr<uvc::device> device)
266  {
267  std::timed_mutex mutex;
269  auto calib = f200::read_f200_calibration(*device, mutex);
270  ivcam::enable_timestamp(*device, mutex, true, true);
271 
272  auto info = get_f200_info(device, std::get<0>(calib));
273  ivcam::get_module_serial_string(*device, mutex, info.serial, 96);
274  ivcam::get_firmware_version_string(*device, mutex, info.firmware_version);
275 
276  info.camera_info[RS_CAMERA_INFO_CAMERA_FIRMWARE_VERSION] = info.firmware_version;
277  info.camera_info[RS_CAMERA_INFO_DEVICE_SERIAL_NUMBER] = info.serial;
278  info.camera_info[RS_CAMERA_INFO_DEVICE_NAME] = info.name;
279 
280  info.capabilities_vector.push_back(RS_CAPABILITIES_ENUMERATION);
281  info.capabilities_vector.push_back(RS_CAPABILITIES_COLOR);
282  info.capabilities_vector.push_back(RS_CAPABILITIES_DEPTH);
283  info.capabilities_vector.push_back(RS_CAPABILITIES_INFRARED);
284 
285  return std::make_shared<f200_camera>(device, info, std::get<0>(calib), std::get<1>(calib), std::get<2>(calib));
286  }
287 } // namespace rsimpl::f200
virtual void stop_fw_logger() override
Definition: f200.cpp:201
const std::vector< std::pair< rs_option, char > > eu_F200_depth_controls
Definition: f200.cpp:16
const GLfloat * params
Definition: glext.h:371
void update_asic_coefficients(uvc::device &device, std::timed_mutex &mutex, const ivcam::camera_calib_params &compensated_params)
static void update_device_info(rsimpl::static_device_info &info)
Definition: device.cpp:525
const std::shared_ptr< rsimpl::uvc::device > device
Definition: device.h:77
const rsimpl::uvc::device & get_device() const
Definition: device.h:102
std::mutex temperatureMutex
Definition: f200.h:28
rs_error * e
std::thread temperatureThread
Definition: f200.h:26
pose stream_poses[RS_STREAM_NATIVE_COUNT]
Definition: types.h:277
const GLfloat * m
Definition: glext.h:6461
void set_dynamic_fps(uvc::device &device, uint8_t dynamic_fps)
Definition: archive.h:12
int read_ir_temp(uvc::device &device, std::timed_mutex &mutex)
f200::cam_temperature_data base_temperature_data
Definition: f200.h:21
rs_option
Defines general configuration controls.
Definition: rs.h:128
ivcam::camera_calib_params base_calibration
Definition: ivcam-device.h:27
std::vector< subdevice_mode > subdevice_modes
Definition: types.h:273
std::condition_variable temperatureCv
Definition: f200.h:29
stream_request presets[RS_STREAM_NATIVE_COUNT][RS_PRESET_COUNT]
Definition: types.h:275
virtual void start_fw_logger(char fw_log_op_code, int grab_rate_in_ms, std::timed_mutex &mutex) override
Definition: f200.cpp:197
float last_temperature_delta
Definition: f200.h:24
int stream_subdevices[RS_STREAM_NATIVE_COUNT]
Definition: types.h:271
void set_options(const rs_option options[], size_t count, const double values[]) override
Definition: f200.cpp:206
void get_dynamic_fps(const uvc::device &device, uint8_t &dynamic_fps)
f200_camera(std::shared_ptr< uvc::device > device, const static_device_info &info, const ivcam::camera_calib_params &calib, const f200::cam_temperature_data &temp, const f200::thermal_loop_params &params)
Definition: f200.cpp:111
rs_intrinsics MakeDepthIntrinsics(const ivcam::camera_calib_params &c, const int2 &dims)
const GLubyte * c
Definition: glext.h:11542
GLuint GLuint GLsizei count
Definition: glext.h:111
std::timed_mutex usbMutex
Definition: ivcam-device.h:25
GLfloat f
Definition: glext.h:1868
void get_module_serial_string(uvc::device &device, std::timed_mutex &mutex, std::string &serial, int offset)
static const cam_mode f200_ir_only_modes[]
Definition: f200.cpp:38
static static_device_info get_f200_info(std::shared_ptr< uvc::device >, const ivcam::camera_calib_params &c)
Definition: f200.cpp:43
std::vector< supported_option > options
Definition: types.h:276
rs_intrinsics MakeColorIntrinsics(const ivcam::camera_calib_params &c, const int2 &dims)
std::shared_ptr< rs_device > make_f200_device(std::shared_ptr< uvc::device > device)
Definition: f200.cpp:265
void get_options(const rs_option options[], size_t count, double values[]) override
float read_mems_temp(uvc::device &device, std::timed_mutex &mutex)
const native_pixel_format pf_f200_inzi
Definition: image.cpp:476
GLfloat seconds
Definition: wglext.h:657
#define LOG_INFO(...)
Definition: types.h:78
void set_options(const rs_option options[], size_t count, const double values[]) override
void get_firmware_version_string(uvc::device &device, std::timed_mutex &mutex, std::string &version, int gvd_cmd, int offset)
GLenum GLsizei GLsizei GLint * values
Definition: glext.h:1484
pose inverse(const pose &a)
Definition: types.h:123
static const cam_mode f200_color_modes[]
Definition: f200.cpp:23
std::tuple< ivcam::camera_calib_params, cam_temperature_data, thermal_loop_params > read_f200_calibration(uvc::device &device, std::timed_mutex &mutex)
int get_pu_control_with_retry(const device &device, int subdevice, rs_option option)
Definition: uvc.h:83
void enable_timestamp(uvc::device &device, std::timed_mutex &mutex, bool colorEnable, bool depthEnable)
void temperature_control_loop()
Definition: f200.cpp:134
static const cam_mode f200_depth_modes[]
Definition: f200.cpp:34
float3x3 transpose(const float3x3 &a)
Definition: types.h:119
std::atomic< bool > runTemperatureThread
Definition: f200.h:27
const native_pixel_format pf_yuy2
Definition: image.cpp:462
bool is_pu_control(rs_option option)
Definition: uvc.h:46
void claim_ivcam_interface(uvc::device &device)
const native_pixel_format pf_f200_invi
Definition: image.cpp:474
void get_options(const rs_option options[], size_t count, double values[]) override
Definition: f200.cpp:227
const native_pixel_format pf_invz
Definition: image.cpp:473
GLuint GLfloat * val
Definition: glext.h:1490
#define LOG_ERROR(...)
Definition: types.h:80


librealsense
Author(s): Sergey Dorodnicov , Mark Horn , Reagan Lopez
autogenerated on Fri Mar 13 2020 03:16:17