00001
00002
00003
00004 #define _USE_MATH_DEFINES
00005 #include <math.h>
00006 #include <algorithm>
00007
00008 #include "image.h"
00009 #include "ivcam-private.h"
00010 #include "f200.h"
00011
00012 using namespace rsimpl::f200;
00013
00014 namespace rsimpl
00015 {
00016 const std::vector <std::pair<rs_option, char>> eu_F200_depth_controls = {{rs_option::RS_OPTION_F200_LASER_POWER, 0x01},
00017 {rs_option::RS_OPTION_F200_ACCURACY, 0x02},
00018 {rs_option::RS_OPTION_F200_MOTION_RANGE, 0x03},
00019 {rs_option::RS_OPTION_F200_FILTER_OPTION, 0x05},
00020 {rs_option::RS_OPTION_F200_CONFIDENCE_THRESHOLD, 0x06},
00021 {rs_option::RS_OPTION_F200_DYNAMIC_FPS, 0x07}};
00022
00023 static const cam_mode f200_color_modes[] = {
00024 {{1920, 1080}, {2,5,15,30}},
00025 {{1280, 720}, {2,5,15,30}},
00026 {{ 960, 540}, {2,5,15,30,60}},
00027 {{ 848, 480}, {2,5,15,30,60}},
00028 {{ 640, 480}, {2,5,15,30,60}},
00029 {{ 640, 360}, {2,5,15,30,60}},
00030 {{ 424, 240}, {2,5,15,30,60}},
00031 {{ 320, 240}, {2,5,15,30,60}},
00032 {{ 320, 180}, {2,5,15,30,60}}
00033 };
00034 static const cam_mode f200_depth_modes[] = {
00035 {{640, 480}, {2,5,15,30,60}},
00036 {{640, 240}, {2,5,15,30,60}}
00037 };
00038 static const cam_mode f200_ir_only_modes[] = {
00039 {{640, 480}, {30,60,120,240,300}},
00040 {{640, 240}, {30,60,120,240,300}}
00041 };
00042
00043 static static_device_info get_f200_info(std::shared_ptr<uvc::device> , const ivcam::camera_calib_params & c)
00044 {
00045 LOG_INFO("Connecting to Intel RealSense F200");
00046 static_device_info info;
00047
00048 info.name = "Intel RealSense F200";
00049
00050
00051 info.stream_subdevices[RS_STREAM_COLOR] = 0;
00052 for(auto & m : f200_color_modes)
00053 {
00054 for(auto fps : m.fps)
00055 {
00056 info.subdevice_modes.push_back({0, m.dims, pf_yuy2, fps, MakeColorIntrinsics(c, m.dims), {}, {0}});
00057 }
00058 }
00059
00060
00061 info.stream_subdevices[RS_STREAM_DEPTH] = 1;
00062 info.stream_subdevices[RS_STREAM_INFRARED] = 1;
00063 for(auto & m : f200_ir_only_modes)
00064 {
00065 for(auto fps : m.fps)
00066 {
00067 info.subdevice_modes.push_back({1, m.dims, pf_f200_invi, fps, MakeDepthIntrinsics(c, m.dims), {}, {0}});
00068 }
00069 }
00070 for(auto & m : f200_depth_modes)
00071 {
00072 for(auto fps : m.fps)
00073 {
00074 info.subdevice_modes.push_back({1, m.dims, pf_invz, fps, MakeDepthIntrinsics(c, m.dims), {}, {0}});
00075 info.subdevice_modes.push_back({1, m.dims, pf_f200_inzi, fps, MakeDepthIntrinsics(c, m.dims), {}, {0}});
00076 }
00077 }
00078
00079 info.presets[RS_STREAM_INFRARED][RS_PRESET_BEST_QUALITY] = {true, 640, 480, RS_FORMAT_Y8, 60, RS_OUTPUT_BUFFER_FORMAT_CONTINUOUS};
00080 info.presets[RS_STREAM_DEPTH ][RS_PRESET_BEST_QUALITY] = {true, 640, 480, RS_FORMAT_Z16, 60, RS_OUTPUT_BUFFER_FORMAT_CONTINUOUS};
00081 info.presets[RS_STREAM_COLOR ][RS_PRESET_BEST_QUALITY] = {true, 640, 480, RS_FORMAT_RGB8, 60, RS_OUTPUT_BUFFER_FORMAT_CONTINUOUS};
00082
00083 info.presets[RS_STREAM_INFRARED][RS_PRESET_LARGEST_IMAGE] = {true, 640, 480, RS_FORMAT_Y8, 60, RS_OUTPUT_BUFFER_FORMAT_CONTINUOUS};
00084 info.presets[RS_STREAM_DEPTH ][RS_PRESET_LARGEST_IMAGE] = {true, 640, 480, RS_FORMAT_Z16, 60, RS_OUTPUT_BUFFER_FORMAT_CONTINUOUS};
00085 info.presets[RS_STREAM_COLOR ][RS_PRESET_LARGEST_IMAGE] = {true, 1920, 1080, RS_FORMAT_RGB8, 60, RS_OUTPUT_BUFFER_FORMAT_CONTINUOUS};
00086
00087 info.presets[RS_STREAM_INFRARED][RS_PRESET_HIGHEST_FRAMERATE] = {true, 640, 480, RS_FORMAT_Y8, 60, RS_OUTPUT_BUFFER_FORMAT_CONTINUOUS};
00088 info.presets[RS_STREAM_DEPTH ][RS_PRESET_HIGHEST_FRAMERATE] = {true, 640, 480, RS_FORMAT_Z16, 60, RS_OUTPUT_BUFFER_FORMAT_CONTINUOUS};
00089 info.presets[RS_STREAM_COLOR ][RS_PRESET_HIGHEST_FRAMERATE] = {true, 640, 480, RS_FORMAT_RGB8, 60, RS_OUTPUT_BUFFER_FORMAT_CONTINUOUS};
00090
00091
00092
00093
00094 info.options.push_back({ RS_OPTION_F200_LASER_POWER, 0, 16, 1, 16 });
00095 info.options.push_back({ RS_OPTION_F200_ACCURACY, 1, 3, 1, 2 });
00096 info.options.push_back({ RS_OPTION_F200_MOTION_RANGE, 0, 100, 1, 0 });
00097 info.options.push_back({ RS_OPTION_F200_FILTER_OPTION, 0, 7, 1, 5 });
00098 info.options.push_back({ RS_OPTION_F200_CONFIDENCE_THRESHOLD, 0, 15, 1, 6 });
00099 info.options.push_back({ RS_OPTION_F200_DYNAMIC_FPS, 0, 1000, 1, 60 });
00100
00101 rsimpl::pose depth_to_color = {transpose((const float3x3 &)c.Rt), (const float3 &)c.Tt * 0.001f};
00102 info.stream_poses[RS_STREAM_DEPTH] = info.stream_poses[RS_STREAM_INFRARED] = inverse(depth_to_color);
00103 info.stream_poses[RS_STREAM_COLOR] = {{{1,0,0},{0,1,0},{0,0,1}}, {0,0,0}};
00104
00105 info.nominal_depth_scale = (c.Rmax / 0xFFFF) * 0.001f;
00106 info.num_libuvc_transfer_buffers = 1;
00107 rs_device_base::update_device_info(info);
00108 return info;
00109 }
00110
00111 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) :
00112 iv_camera(device, info, calib),
00113 base_temperature_data(temp),
00114 thermal_loop_params(params),
00115 last_temperature_delta(std::numeric_limits<float>::infinity())
00116 {
00117
00118 if(thermal_loop_params.IRThermalLoopEnable)
00119 {
00120 runTemperatureThread = true;
00121 temperatureThread = std::thread(&f200_camera::temperature_control_loop, this);
00122 }
00123 }
00124
00125 f200_camera::~f200_camera()
00126 {
00127
00128 runTemperatureThread = false;
00129 temperatureCv.notify_one();
00130 if (temperatureThread.joinable())
00131 temperatureThread.join();
00132 }
00133
00134 void f200_camera::temperature_control_loop()
00135 {
00136 const float FcxSlope = base_calibration.Kc[0][0] * thermal_loop_params.FcxSlopeA + thermal_loop_params.FcxSlopeB;
00137 const float UxSlope = base_calibration.Kc[0][2] * thermal_loop_params.UxSlopeA + base_calibration.Kc[0][0] * thermal_loop_params.UxSlopeB + thermal_loop_params.UxSlopeC;
00138 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)));
00139 float TempThreshold = thermal_loop_params.TempThreshold;
00140 if (TempThreshold <= 0) TempThreshold = tempFromHFOV;
00141 if (TempThreshold > tempFromHFOV) TempThreshold = tempFromHFOV;
00142
00143 std::unique_lock<std::mutex> lock(temperatureMutex);
00144 while (runTemperatureThread)
00145 {
00146 temperatureCv.wait_for(lock, std::chrono::seconds(10));
00147
00148
00149 try
00150 {
00151 float IRTemp = (float)f200::read_ir_temp(get_device(), usbMutex);
00152 float LiguriaTemp = f200::read_mems_temp(get_device(), usbMutex);
00153
00154 double IrBaseTemperature = base_temperature_data.IRTemp;
00155 double liguriaBaseTemperature = base_temperature_data.LiguriaTemp;
00156
00157
00158 double IrTempDelta = IRTemp - IrBaseTemperature;
00159 double liguriaTempDelta = LiguriaTemp - liguriaBaseTemperature;
00160 double weightedTempDelta = liguriaTempDelta * thermal_loop_params.LiguriaTempWeight + IrTempDelta * thermal_loop_params.IrTempWeight;
00161 double tempDeltaFromLastFix = fabs(weightedTempDelta - last_temperature_delta);
00162
00163
00164 double Kc11 = base_calibration.Kc[0][0];
00165 double Kc13 = base_calibration.Kc[0][2];
00166
00167
00168 if (tempDeltaFromLastFix >= TempThreshold)
00169 {
00170
00171 double tempDeltaToUse = weightedTempDelta;
00172 if (tempDeltaToUse > 0 && tempDeltaToUse < thermal_loop_params.TransitionTemp)
00173 {
00174 tempDeltaToUse = thermal_loop_params.TransitionTemp;
00175 }
00176
00177
00178 double fixed_Kc11 = Kc11 + (FcxSlope * tempDeltaToUse) + thermal_loop_params.FcxOffset;
00179 double fixed_Kc13 = Kc13 + (UxSlope * tempDeltaToUse) + thermal_loop_params.UxOffset;
00180
00181
00182 auto compensated_calibration = base_calibration;
00183 compensated_calibration.Kc[0][0] = (float) fixed_Kc11;
00184 compensated_calibration.Kc[1][1] = base_calibration.Kc[1][1] * (float)(fixed_Kc11/Kc11);
00185 compensated_calibration.Kc[0][2] = (float) fixed_Kc13;
00186
00187
00188 LOG_INFO("updating asic with new temperature calibration coefficients");
00189 update_asic_coefficients(get_device(), usbMutex, compensated_calibration);
00190 last_temperature_delta = (float)weightedTempDelta;
00191 }
00192 }
00193 catch(const std::exception & e) { LOG_ERROR("TemperatureControlLoop: " << e.what()); }
00194 }
00195 }
00196
00197 void f200_camera::start_fw_logger(char , int , std::timed_mutex& )
00198 {
00199 throw std::logic_error("Not implemented");
00200 }
00201 void f200_camera::stop_fw_logger()
00202 {
00203 throw std::logic_error("Not implemented");
00204 }
00205
00206 void f200_camera::set_options(const rs_option options[], size_t count, const double values[])
00207 {
00208 std::vector<rs_option> base_opt;
00209 std::vector<double> base_opt_val;
00210
00211
00212 for(size_t i=0; i<count; ++i)
00213 {
00214 switch(options[i])
00215 {
00216 case RS_OPTION_F200_DYNAMIC_FPS: f200::set_dynamic_fps(get_device(), static_cast<uint8_t>(values[i])); break;
00217
00218 default: base_opt.push_back(options[i]); base_opt_val.push_back(values[i]); break;
00219 }
00220 }
00221
00222
00223 if (!base_opt.empty())
00224 iv_camera::set_options(base_opt.data(), base_opt.size(), base_opt_val.data());
00225 }
00226
00227 void f200_camera::get_options(const rs_option options[], size_t count, double values[])
00228 {
00229 std::vector<rs_option> base_opt;
00230 std::vector<size_t> base_opt_index;
00231 std::vector<double> base_opt_val;
00232
00233 for (size_t i = 0; i<count; ++i)
00234 {
00235 LOG_INFO("Reading option " << options[i]);
00236
00237 if(uvc::is_pu_control(options[i]))
00238 {
00239 values[i] = uvc::get_pu_control_with_retry(get_device(), 0, options[i]);
00240 continue;
00241 }
00242
00243 uint8_t val=0;
00244 switch(options[i])
00245 {
00246 case RS_OPTION_F200_DYNAMIC_FPS: f200::get_dynamic_fps(get_device(), val); values[i] = val; break;
00247
00248
00249 default: base_opt.push_back(options[i]); base_opt_index.push_back(i); break;
00250 }
00251 }
00252
00253
00254 if (base_opt.size())
00255 {
00256 base_opt_val.resize(base_opt.size());
00257 iv_camera::get_options(base_opt.data(), base_opt.size(), base_opt_val.data());
00258 }
00259
00260
00261 for (auto i : base_opt_index)
00262 values[i] = base_opt_val[i];
00263 }
00264
00265 std::shared_ptr<rs_device> make_f200_device(std::shared_ptr<uvc::device> device)
00266 {
00267 std::timed_mutex mutex;
00268 ivcam::claim_ivcam_interface(*device);
00269 auto calib = f200::read_f200_calibration(*device, mutex);
00270 ivcam::enable_timestamp(*device, mutex, true, true);
00271
00272 auto info = get_f200_info(device, std::get<0>(calib));
00273 ivcam::get_module_serial_string(*device, mutex, info.serial, 96);
00274 ivcam::get_firmware_version_string(*device, mutex, info.firmware_version);
00275
00276 info.camera_info[RS_CAMERA_INFO_CAMERA_FIRMWARE_VERSION] = info.firmware_version;
00277 info.camera_info[RS_CAMERA_INFO_DEVICE_SERIAL_NUMBER] = info.serial;
00278 info.camera_info[RS_CAMERA_INFO_DEVICE_NAME] = info.name;
00279
00280 info.capabilities_vector.push_back(RS_CAPABILITIES_ENUMERATION);
00281 info.capabilities_vector.push_back(RS_CAPABILITIES_COLOR);
00282 info.capabilities_vector.push_back(RS_CAPABILITIES_DEPTH);
00283 info.capabilities_vector.push_back(RS_CAPABILITIES_INFRARED);
00284
00285 return std::make_shared<f200_camera>(device, info, std::get<0>(calib), std::get<1>(calib), std::get<2>(calib));
00286 }
00287 }