00001
00002
00003
00004 #pragma once
00005 #ifndef LIBREALSENSE_DEVICE_H
00006 #define LIBREALSENSE_DEVICE_H
00007
00008 #include "uvc.h"
00009 #include "stream.h"
00010 #include <chrono>
00011 #include <memory>
00012 #include <vector>
00013
00014 namespace rsimpl
00015 {
00016
00017
00018 template<class T, class R, class W> struct struct_interface
00019 {
00020 T struct_;
00021 R reader;
00022 W writer;
00023 bool active;
00024
00025 struct_interface(R r, W w) : reader(r), writer(w), active(false) {}
00026
00027 void activate() { if (!active) { struct_ = reader(); active = true; } }
00028 template<class U> double get(U T::* field) { activate(); return static_cast<double>(struct_.*field); }
00029 template<class U, class V> void set(U T::* field, V value) { activate(); struct_.*field = static_cast<U>(value); }
00030 void commit() { if (active) writer(struct_); }
00031 };
00032
00033 template<class T, class R, class W> struct_interface<T, R, W> make_struct_interface(R r, W w) { return{ r,w }; }
00034
00035 template <typename T>
00036 class wraparound_mechanism
00037 {
00038 public:
00039 wraparound_mechanism(T min_value, T max_value)
00040 : max_number(max_value - min_value + 1), last_number(min_value), num_of_wraparounds(0)
00041 {}
00042
00043 T fix(T number)
00044 {
00045 if ((number + (num_of_wraparounds*max_number)) < last_number)
00046 ++num_of_wraparounds;
00047
00048
00049 number += (num_of_wraparounds*max_number);
00050 last_number = number;
00051 return number;
00052 }
00053
00054 private:
00055 T max_number;
00056 T last_number;
00057 unsigned long long num_of_wraparounds;
00058 };
00059
00060 struct frame_timestamp_reader
00061 {
00062 virtual bool validate_frame(const subdevice_mode & mode, const void * frame) = 0;
00063 virtual double get_frame_timestamp(const subdevice_mode & mode, const void * frame, double actual_fps) = 0;
00064 virtual unsigned long long get_frame_counter(const subdevice_mode & mode, const void * frame) = 0;
00065 };
00066
00067
00068 namespace motion_module
00069 {
00070 struct motion_module_parser;
00071 }
00072 }
00073
00074 struct rs_device_base : rs_device
00075 {
00076 private:
00077 const std::shared_ptr<rsimpl::uvc::device> device;
00078 protected:
00079 rsimpl::device_config config;
00080 private:
00081 rsimpl::native_stream depth, color, infrared, infrared2, fisheye;
00082 rsimpl::point_stream points;
00083 rsimpl::rectified_stream rect_color;
00084 rsimpl::aligned_stream color_to_depth, depth_to_color, depth_to_rect_color, infrared2_to_depth, depth_to_infrared2;
00085 rsimpl::native_stream * native_streams[RS_STREAM_NATIVE_COUNT];
00086 rsimpl::stream_interface * streams[RS_STREAM_COUNT];
00087
00088 bool capturing;
00089 bool data_acquisition_active;
00090 std::chrono::high_resolution_clock::time_point capture_started;
00091 std::atomic<uint32_t> max_publish_list_size;
00092 std::atomic<uint32_t> event_queue_size;
00093 std::atomic<uint32_t> events_timeout;
00094 std::shared_ptr<rsimpl::syncronizing_archive> archive;
00095
00096 mutable std::string usb_port_id;
00097 mutable std::mutex usb_port_mutex;
00098
00099 std::shared_ptr<std::thread> fw_logger;
00100
00101 protected:
00102 const rsimpl::uvc::device & get_device() const { return *device; }
00103 rsimpl::uvc::device & get_device() { return *device; }
00104
00105 virtual void start_video_streaming();
00106 virtual void stop_video_streaming();
00107 virtual void start_motion_tracking();
00108 virtual void stop_motion_tracking();
00109
00110 virtual void disable_auto_option(int subdevice, rs_option auto_opt);
00111 virtual void on_before_callback(rs_stream, rs_frame_ref *, std::shared_ptr<rsimpl::frame_archive>) { }
00112
00113 bool motion_module_ready;
00114 std::atomic<bool> keep_fw_logger_alive;
00115
00116 std::atomic<int> frames_drops_counter;
00117
00118 public:
00119 rs_device_base(std::shared_ptr<rsimpl::uvc::device> device, const rsimpl::static_device_info & info, rsimpl::calibration_validator validator = rsimpl::calibration_validator());
00120 virtual ~rs_device_base();
00121
00122 const rsimpl::stream_interface & get_stream_interface(rs_stream stream) const override { return *streams[stream]; }
00123
00124 const char * get_name() const override { return config.info.name.c_str(); }
00125 const char * get_serial() const override { return config.info.serial.c_str(); }
00126 const char * get_firmware_version() const override { return config.info.firmware_version.c_str(); }
00127 float get_depth_scale() const override { return config.depth_scale; }
00128
00129 const char* get_camera_info(rs_camera_info info) const override;
00130
00131 void enable_stream(rs_stream stream, int width, int height, rs_format format, int fps, rs_output_buffer_format output) override;
00132 void enable_stream_preset(rs_stream stream, rs_preset preset) override;
00133 void disable_stream(rs_stream stream) override;
00134
00135 rs_motion_intrinsics get_motion_intrinsics() const override;
00136 rs_extrinsics get_motion_extrinsics_from(rs_stream from) const override;
00137 void enable_motion_tracking() override;
00138 void set_stream_callback(rs_stream stream, void(*on_frame)(rs_device * device, rs_frame_ref * frame, void * user), void * user) override;
00139 void set_stream_callback(rs_stream stream, rs_frame_callback * callback) override;
00140 void disable_motion_tracking() override;
00141
00142 void set_motion_callback(rs_motion_callback * callback) override;
00143 void set_motion_callback(void(*on_event)(rs_device * device, rs_motion_data data, void * user), void * user) override;
00144 void set_timestamp_callback(void(*on_event)(rs_device * device, rs_timestamp_data data, void * user), void * user) override;
00145 void set_timestamp_callback(rs_timestamp_callback * callback) override;
00146
00147 virtual void start(rs_source source) override;
00148 virtual void stop(rs_source source) override;
00149
00150 virtual void start_fw_logger(char fw_log_op_code, int grab_rate_in_ms, std::timed_mutex& mutex) override;
00151 virtual void stop_fw_logger() override;
00152
00153 bool is_capturing() const override { return capturing; }
00154 int is_motion_tracking_active() const override { return data_acquisition_active; }
00155
00156 void wait_all_streams() override;
00157 bool poll_all_streams() override;
00158
00159 virtual bool supports(rs_capabilities capability) const override;
00160 virtual bool supports(rs_camera_info info_param) const override;
00161
00162 virtual bool supports_option(rs_option option) const override;
00163 virtual void get_option_range(rs_option option, double & min, double & max, double & step, double & def) override;
00164 virtual void set_options(const rs_option options[], size_t count, const double values[]) override;
00165 virtual void get_options(const rs_option options[], size_t count, double values[])override;
00166 virtual void on_before_start(const std::vector<rsimpl::subdevice_mode_selection> & selected_modes) = 0;
00167 virtual rs_stream select_key_stream(const std::vector<rsimpl::subdevice_mode_selection> & selected_modes) = 0;
00168 virtual std::vector<std::shared_ptr<rsimpl::frame_timestamp_reader>>
00169 create_frame_timestamp_readers() const = 0;
00170 void release_frame(rs_frame_ref * ref) override;
00171 const char * get_usb_port_id() const override;
00172 rs_frame_ref * clone_frame(rs_frame_ref * frame) override;
00173
00174 virtual void send_blob_to_device(rs_blob_type , void * , int ) { throw std::runtime_error("not supported!"); }
00175 static void update_device_info(rsimpl::static_device_info& info);
00176
00177 const char * get_option_description(rs_option option) const override;
00178 };
00179
00180 #endif