22 depth(
config,
RS_STREAM_DEPTH, validator),
color(
config,
RS_STREAM_COLOR, validator), infrared(
config,
RS_STREAM_INFRARED, validator), infrared2(
config,
RS_STREAM_INFRARED2, validator), fisheye(
config,
RS_STREAM_FISHEYE, validator),
23 points(
depth), rect_color(
color), color_to_depth(
color,
depth), depth_to_color(
depth,
color), depth_to_rect_color(
depth, rect_color), infrared2_to_depth(infrared2,
depth), depth_to_infrared2(
depth,infrared2),
25 usb_port_id(
""), motion_module_ready(false), keep_fw_logger_alive(false), frames_drops_counter(0)
66 throw std::runtime_error(
"selected camera info is not supported for this camera!");
67 return it->second.c_str();
72 if(
capturing)
throw std::runtime_error(
"streams cannot be reconfigured after having called rs_start_device()");
81 if(
capturing)
throw std::runtime_error(
"streams cannot be reconfigured after having called rs_start_device()");
90 throw std::runtime_error(
"Motion intrinsic is not supported for this device");
95 throw std::runtime_error(
"Motion extrinsics does not supported");
100 if(
capturing)
throw std::runtime_error(
"streams cannot be reconfigured after having called rs_start_device()");
120 if (
data_acquisition_active)
throw std::runtime_error(
"motion-tracking cannot be reconfigured after having called rs_start_device()");
127 if (
data_acquisition_active)
throw std::runtime_error(
"motion-tracking disabled after having called rs_start_device()");
134 if (
data_acquisition_active)
throw std::runtime_error(
"cannot set motion callback when motion data is active");
142 if (
data_acquisition_active)
throw std::runtime_error(
"cannot restart data acquisition without stopping first");
144 auto parser = std::make_shared<motion_module_parser>();
151 [
this, parser](
const unsigned char *
data,
const int size)
mutable 156 auto events = (*parser)(
data,
size);
159 for (
auto & entry : events)
163 for (
int i = 0; i < entry.imu_entries_num; i++)
169 for (
int i = 0; i < entry.non_imu_entries_num; i++)
171 auto tse = entry.non_imu_packets[i];
196 if (
data_acquisition_active)
throw std::runtime_error(
"cannot set motion callback when motion data is active");
204 if (
data_acquisition_active)
throw std::runtime_error(
"cannot set timestamp callback when motion data is active");
222 throw std::runtime_error(
"motion-tracking is not supported by this device");
235 throw std::runtime_error(
"unsupported streaming source!");
250 throw std::runtime_error(
"motion-tracking is not supported by this device");
259 throw std::runtime_error(
"unsupported streaming source");
269 res +=
"0123456789ABCDEF"[n % 16];
273 reverse(res.begin(), res.end());
286 throw std::logic_error(
"FW logger already started");
289 fw_logger = std::make_shared<std::thread>([
this, fw_log_op_code, grab_rate_in_ms, &mutex]() {
290 const int data_size = 500;
295 std::this_thread::sleep_for(std::chrono::milliseconds(grab_rate_in_ms));
297 char data[data_size];
300 std::stringstream sstr;
301 sstr <<
"FW_Log_Data:";
303 sstr <<
hexify(data[i]) <<
" ";
314 throw std::logic_error(
"FW logger not started");
322 bool was_initialized =
false;
323 unsigned long long prev_frame_counter = 0;
328 if(
capturing)
throw std::runtime_error(
"cannot restart device without first stopping device");
330 auto capture_start_time = std::chrono::high_resolution_clock::now();
340 for(
auto mode_selection : selected_modes)
342 assert(static_cast<size_t>(mode_selection.mode.subdevice) <= timestamp_readers.size());
343 auto timestamp_reader = timestamp_readers[mode_selection.mode.subdevice];
346 for(
auto &
stream_mode : mode_selection.get_outputs())
353 std::vector<rs_stream>
streams;
354 for (
auto & output : mode_selection.get_outputs())
356 streams.push_back(output.first);
362 std::shared_ptr<drops_status> frame_drops_status(
new drops_status{});
364 set_subdevice_mode(*
device, mode_selection.mode.subdevice, mode_selection.mode.native_dims.x, mode_selection.mode.native_dims.y, mode_selection.mode.pf.fourcc, mode_selection.mode.fps,
365 [
this, mode_selection,
archive, timestamp_reader, streams, capture_start_time, frame_drops_status, actual_fps_calc, supported_metadata_vector](
const void * frame, std::function<
void()> continuation)
mutable 367 auto now = std::chrono::system_clock::now();
368 auto sys_time = std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()).count();
370 frame_continuation release_and_enqueue(continuation, frame);
373 if (!timestamp_reader->validate_frame(mode_selection.mode, frame)) return;
375 auto actual_fps = actual_fps_calc->calc_fps(now);
378 auto timestamp = timestamp_reader->get_frame_timestamp(mode_selection.mode, frame, actual_fps);
379 auto frame_counter = timestamp_reader->get_frame_counter(mode_selection.mode, frame);
380 auto recieved_time = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - capture_start_time).count();
382 auto requires_processing = mode_selection.requires_processing();
384 double exposure_value[1] = {};
391 auto data =
static_cast<const char*
>(frame);
393 for (
int i = 4, j = 0; i < 12; ++i, ++j)
394 exposure |= ((
data[i] & 0x01) << j);
396 exposure_value[0] = exposure * 0.2 * 10.;
400 auto width = mode_selection.get_width();
401 auto height = mode_selection.get_height();
402 auto fps = mode_selection.get_framerate();
403 std::vector<byte *> dest;
405 auto stride_x = mode_selection.get_stride_x();
406 auto stride_y = mode_selection.get_stride_y();
407 for (
auto & output : mode_selection.get_outputs())
409 LOG_DEBUG(
"FrameAccepted, RecievedAt," << recieved_time <<
", FWTS," << timestamp <<
", DLLTS," << recieved_time <<
", Type," <<
rsimpl::get_string(output.first) <<
",HasPair,0,F#," << frame_counter);
412 frame_drops_status->was_initialized =
true;
415 if (frame_drops_status->was_initialized)
418 frame_drops_status->prev_frame_counter = frame_counter;
421 for (
auto & output : mode_selection.get_outputs())
435 mode_selection.pad_crop,
436 supported_metadata_vector,
441 dest.push_back(
archive->alloc_frame(output.first, additional_data, requires_processing));
446 archive->correct_timestamp(output.first);
450 if (requires_processing)
452 mode_selection.unpack(dest.data(),
reinterpret_cast<const byte *
>(frame));
456 for (
size_t i = 0; i < dest.size(); ++i)
458 if (!requires_processing)
460 archive->attach_continuation(streams[i], std::move(release_and_enqueue));
465 auto frame_ref =
archive->track_frame(streams[i]);
468 frame_ref->update_frame_callback_start_ts(std::chrono::high_resolution_clock::now());
469 frame_ref->log_callback_start(capture_start_time);
477 archive->commit_frame(streams[i]);
492 if(!
capturing)
throw std::runtime_error(
"cannot stop device without first starting device");
510 return archive->poll_for_frames();
521 if (!
result)
throw std::runtime_error(
"Not enough resources to clone frame!");
537 case RS_OPTION_COLOR_EXPOSURE :
return "Controls exposure time of color camera. Setting any value will disable auto exposure";
547 case RS_OPTION_F200_ACCURACY :
return "Set the number of patterns projected per frame. The higher the accuracy value the more patterns projected. Increasing the number of patterns help to achieve better accuracy. Note that this control is affecting the Depth FPS";
548 case RS_OPTION_F200_MOTION_RANGE :
return "Motion vs. Range trade-off, with lower values allowing for better motion sensitivity and higher values allowing for better depth range";
549 case RS_OPTION_F200_FILTER_OPTION :
return "Set the filter to apply to each depth frame. Each one of the filter is optimized per the application requirements";
550 case RS_OPTION_F200_CONFIDENCE_THRESHOLD :
return "The confidence level threshold used by the Depth algorithm pipe to set whether a pixel will get a valid range or will be marked with invalid range";
551 case RS_OPTION_F200_DYNAMIC_FPS :
return "(F200-only) Allows to reduce FPS without restarting streaming. Valid values are {2, 5, 15, 30, 60}";
564 case RS_OPTION_R200_LR_EXPOSURE :
return "This control allows manual adjustment of the exposure time value for the L/R imagers";
566 case RS_OPTION_R200_DEPTH_UNITS :
return "Micrometers per increment in integer depth values, 1000 is default (mm scale). Set before streaming";
567 case RS_OPTION_R200_DEPTH_CLAMP_MIN :
return "Minimum depth in current depth units that will be output. Any values less than 'Min Depth' will be mapped to 0 during the conversion between disparity and depth. Set before streaming";
568 case RS_OPTION_R200_DEPTH_CLAMP_MAX :
return "Maximum depth in current depth units that will be output. Any values greater than 'Max Depth' will be mapped to 0 during the conversion between disparity and depth. Set before streaming";
592 case RS_OPTION_FISHEYE_STROBE :
return "Enables / disables fisheye strobe. When enabled this will align timestamps to common clock-domain with the motion events";
593 case RS_OPTION_FISHEYE_EXTERNAL_TRIGGER :
return "Enables / disables fisheye external trigger mode. When enabled fisheye image will be acquired in-sync with the depth image";
594 case RS_OPTION_FRAMES_QUEUE_SIZE :
return "Number of frames the user is allowed to keep per stream. Trying to hold-on to more frames will cause frame-drops.";
609 auto version_ok =
true;
612 if (supported.capability == capability)
617 if (!firmware.
is_between(supported.from, supported.until))
624 return found && version_ok;
648 if(o.option == option)
658 throw std::logic_error(
"range not specified");
663 for (
size_t i = 0; i <
count; ++i)
675 throw std::logic_error(
"Option unsupported");
683 for (
size_t i = 0; i <
count; ++i)
695 throw std::logic_error(
"Option unsupported");
703 static const int reset_state = 0;
void enable_stream_preset(rs_stream stream, rs_preset preset) override
rsimpl::native_stream infrared2
motion_callback_ptr motion_callback
std::unique_ptr< rs_motion_callback, void(*)(rs_motion_callback *)> motion_callback_ptr
size_t receivedCommandDataLength
int num_libuvc_transfer_buffers
rsimpl::device_config config
static void update_device_info(rsimpl::static_device_info &info)
virtual bool supports(rs_capabilities capability) const override
rs_device_base(std::shared_ptr< rsimpl::uvc::device > device, const rsimpl::static_device_info &info, rsimpl::calibration_validator validator=rsimpl::calibration_validator())
virtual void get_option_range(rs_option option, double &min, double &max, double &step, double &def) override
const std::shared_ptr< rsimpl::uvc::device > device
std::shared_ptr< std::thread > fw_logger
std::vector< rs_frame_metadata > supported_metadata_vector
void get_pu_control_range(const device &device, int subdevice, rs_option option, int *min, int *max, int *step, int *def)
virtual void get_options(const rs_option options[], size_t count, double values[]) override
virtual void stop_motion_tracking()
const rsimpl::uvc::device & get_device() const
frame_callback_ptr callbacks[RS_STREAM_NATIVE_COUNT]
rsimpl::native_stream fisheye
void set_stream_callback(rs_stream stream, void(*on_frame)(rs_device *device, rs_frame_ref *frame, void *user), void *user) override
const int RS_USER_QUEUE_SIZE
void set_subdevice_mode(device &device, int subdevice_index, int width, int height, uint32_t fourcc, int fps, video_channel_callback callback)
const char * rs_camera_info_to_string(rs_camera_info info)
GLint GLint GLsizei GLsizei height
rsimpl::aligned_stream depth_to_infrared2
GLsizei const GLchar *const * string
const int NUMBER_OF_FRAMES_TO_SAMPLE
timestamp_callback_ptr timestamp_callback
const int RS_MAX_EVENT_QUEUE_SIZE
rsimpl::point_stream points
GLint GLint GLsizei GLsizei GLsizei depth
int get_image_bpp(rs_format format)
virtual void stop(rs_source source) override
uint8_t receivedCommandData[HW_MONITOR_BUFFER_SIZE]
rs_option
Defines general configuration controls.
const char * get_name() const override
virtual void on_before_callback(rs_stream, rs_frame_ref *, std::shared_ptr< rsimpl::frame_archive >)
std::string get_usb_port_id(const device &device)
rs_output_buffer_format
Output buffer format: sets how librealsense works with frame memory.
void config(uvc::device &device, uint8_t gyro_bw, uint8_t gyro_range, uint8_t accel_bw, uint8_t accel_range, uint32_t time_seed)
bool is_between(const firmware_version &from, const firmware_version &until)
stream_request presets[RS_STREAM_NATIVE_COUNT][RS_PRESET_COUNT]
const char * get_option_description(rs_option option) const override
void perform_and_send_monitor_command(uvc::device &device, std::timed_mutex &mutex, hwmon_cmd &newCommand)
rsimpl::rectified_stream rect_color
virtual void on_before_start(const std::vector< rsimpl::subdevice_mode_selection > &selected_modes)=0
std::atomic< uint32_t > event_queue_size
void release_frame(rs_frame_ref *ref) override
const char * get_usb_port_id() const override
rs_motion_intrinsics get_motion_intrinsics() const override
rs_extrinsics get_motion_extrinsics_from(rs_stream from) const override
std::vector< supported_capability > capabilities_vector
virtual void stop_video_streaming()
const char * rs_capabilities_to_string(rs_capabilities capability)
int stream_subdevices[RS_STREAM_NATIVE_COUNT]
void set_subdevice_data_channel_handler(device &device, int subdevice_index, data_channel_callback callback)
void start_streaming(device &device, int num_transfer_bufs)
GLfixed GLfixed GLint GLint GLfixed points
GLuint GLuint GLsizei count
void start_data_acquisition(device &device)
rsimpl::native_stream * native_streams[RS_STREAM_NATIVE_COUNT]
Motion data from gyroscope and accelerometer from the microcontroller.
rs_camera_info
Read-only strings that can be queried from the device.
stream_request requests[RS_STREAM_NATIVE_COUNT]
Motion module intrinsics: includes accelerometer and gyroscope intrinsics structs of type rs_motion_d...
rsimpl::aligned_stream depth_to_color
rsimpl::aligned_stream depth_to_rect_color
void enable_stream(rs_stream stream, int width, int height, rs_format format, int fps, rs_output_buffer_format output) override
format
Formats: defines how each stream can be encoded. rs_format specifies how a frame is represented in me...
rsimpl::native_stream color
void disable_stream(rs_stream stream) override
virtual ~rs_device_base()
GLint GLenum GLsizei GLsizei GLsizei GLint GLsizei const void * data
void enable_motion_tracking() override
const char * get_camera_info(rs_camera_info info) const override
std::atomic< bool > keep_fw_logger_alive
std::map< rs_camera_info, std::string > camera_info
rs_format
Formats: defines how each stream can be encoded.
std::vector< supported_option > options
virtual void disable_auto_option(int subdevice, rs_option auto_opt)
std::chrono::high_resolution_clock::time_point capture_started
Timestamp data from the motion microcontroller.
std::shared_ptr< rsimpl::syncronizing_archive > archive
bool poll_all_streams() override
std::atomic< uint32_t > max_publish_list_size
virtual void start_video_streaming()
const static_device_info info
rs_source
Source: allows you to choose between available hardware subdevices.
virtual bool supports_option(rs_option option) const override
virtual std::vector< std::shared_ptr< rsimpl::frame_timestamp_reader > > create_frame_timestamp_readers() const =0
rsimpl::stream_interface * streams[RS_STREAM_COUNT]
int get_pu_control(const device &device, int subdevice, rs_option option)
Cross-stream extrinsics: encode the topology describing how the different devices are connected...
const char * get_string(rs_stream value)
rsimpl::aligned_stream infrared2_to_depth
virtual rs_stream select_key_stream(const std::vector< rsimpl::subdevice_mode_selection > &selected_modes)=0
rs_preset
Presets: general preferences that are translated by librealsense into concrete resolution and FPS...
GLenum GLsizei GLsizei GLint * values
std::unique_ptr< rs_timestamp_callback, void(*)(rs_timestamp_callback *)> timestamp_callback_ptr
rs_stream
Streams are different types of data provided by RealSense devices.
rsimpl::aligned_stream color_to_depth
GLint GLint GLsizei width
const char * rs_option_to_string(rs_option option)
GLsizei GLsizei GLchar * source
void set_pu_control(device &device, int subdevice, rs_option option, int value)
static std::atomic< bool > stop_streaming
rs_capabilities
Specifies various capabilities of a RealSense device.
data_polling_request data_request
void set_motion_callback(rs_motion_callback *callback) override
GLint GLint GLsizei GLsizei GLsizei GLint GLenum format
rsimpl::native_stream depth
bool is_pu_control(rs_option option)
bool data_acquisition_active
virtual void start(rs_source source) override
virtual void set_options(const rs_option options[], size_t count, const double values[]) override
void disable_motion_tracking() override
virtual void stop_fw_logger() override
void wait_all_streams() override
virtual void start_fw_logger(char fw_log_op_code, int grab_rate_in_ms, std::timed_mutex &mutex) override
void stop_data_acquisition(device &device)
std::atomic< uint32_t > events_timeout
virtual void start_motion_tracking()
void set_timestamp_callback(void(*on_event)(rs_device *device, rs_timestamp_data data, void *user), void *user) override
std::atomic< int > frames_drops_counter
std::string hexify(unsigned char n)
const int RS_MAX_EVENT_TIME_OUT
std::vector< subdevice_mode_selection > select_modes(const stream_request(&requests)[RS_STREAM_NATIVE_COUNT]) const
std::mutex usb_port_mutex
rsimpl::native_stream infrared
rs_frame_ref * clone_frame(rs_frame_ref *frame) override
preset
Presets: general preferences that are translated by librealsense into concrete resolution and FPS...