Skip to content

Commit

Permalink
Merge pull request #2997 from ev-mp/hid_async
Browse files Browse the repository at this point in the history 
Fix IMU Start-up delay on Linux
  • Loading branch information
@ev-mp
ev-mp committed Jan 14, 2019
2 parents 89b8a02 + b6824ae commit 5b3dd73
Show file tree
Hide file tree
Showing 7 changed files with 95 additions and 97 deletions.
4 changes: 0 additions & 4 deletions CMake/global_config.cmake
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,10 +48,6 @@ macro(global_set_flags)
add_definitions(-DRS2_USE_CUDA)
endif()

if (PREVENT_HID_SUSPEND)
add_definitions(-DPREVENT_HID_SUSPEND)
endif()

if(FORCE_LIBUVC)
set(BACKEND RS2_USE_LIBUVC_BACKEND)
message( WARNING "Using libuvc!" )
Expand Down
1 change: 0 additions & 1 deletion CMake/lrs_options.cmake
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,4 +22,3 @@ option(BUILD_CV_EXAMPLES "Build OpenCV examples" OFF)
option(BUILD_PCL_EXAMPLES "Build PCL examples" OFF)
option(BUILD_NODEJS_BINDINGS "Build Node.js bindings" OFF)
option(BUILD_OPENNI2_BINDINGS "Build OpenNI bindings" OFF)
option(PREVENT_HID_SUSPEND "Linux-specific: bypass IMU resume latency by disabling power management" OFF)
6 changes: 4 additions & 2 deletions src/archive.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -283,7 +283,8 @@ namespace librealsense
auto callback_warning_duration = 1000 / (frame->get_stream()->get_framerate() + 1);
auto callback_duration = callback_ended - frame->get_frame_callback_start_time_point();

LOG_DEBUG("CallbackFinished," << librealsense::get_string(frame->get_stream()->get_stream_type()) << "," << frame->get_frame_number()
LOG_DEBUG("CallbackFinished," << librealsense::get_string(frame->get_stream()->get_stream_type()) << ","
<< std::dec << frame->get_frame_number()
<< ",DispatchedAt," << std::fixed << callback_ended);

if (callback_duration > callback_warning_duration)
Expand Down Expand Up @@ -506,7 +507,8 @@ namespace librealsense
auto callback_warning_duration = 1000.f / (get_stream()->get_framerate() + 1);
auto callback_duration = timestamp - get_frame_callback_start_time_point();

LOG_DEBUG("CallbackFinished," << librealsense::get_string(get_stream()->get_stream_type()) << "," << get_frame_number() << ",DispatchedAt," << std::fixed << timestamp);
LOG_DEBUG("CallbackFinished," << librealsense::get_string(get_stream()->get_stream_type()) << ","
<< std::dec << get_frame_number() << ",DispatchedAt," << std::fixed << timestamp);

if (callback_duration > callback_warning_duration)
{
Expand Down
98 changes: 26 additions & 72 deletions src/linux/backend-hid.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -268,9 +268,8 @@ namespace librealsense
_callback = sensor_callback;
_is_capturing = true;
_hid_thread = std::unique_ptr<std::thread>(new std::thread([this, read_device_path_str](){
static const uint32_t buf_len = 128;
const uint32_t channel_size = 24; // TODO: why 24?
std::vector<uint8_t> raw_data(channel_size * buf_len);
std::vector<uint8_t> raw_data(channel_size * hid_buf_len);

do {
fd_set fds;
Expand Down Expand Up @@ -456,13 +455,15 @@ namespace librealsense
}

iio_hid_sensor::iio_hid_sensor(const std::string& device_path, uint32_t frequency)
: _iio_device_path(device_path),
: _stop_pipe_fd{},
_fd(0),
_iio_device_number(0),
_iio_device_path(device_path),
_sensor_name(""),
_sampling_frequency_name(""),
_callback(nullptr),
_is_capturing(false),
_sampling_frequency_name(""),
_fd(0),
_stop_pipe_fd{}
_pm_dispatcher(16) // queue for async power management commands
{
init(frequency);
}
Expand All @@ -471,7 +472,8 @@ namespace librealsense
{
try
{
write_integer_to_param("buffer/enable", 0);
// Ensure PM sync
_pm_dispatcher.flush();
stop_capture();

clear_buffer();
Expand All @@ -488,6 +490,7 @@ namespace librealsense
if (_is_capturing)
return;

set_power(true);
std::ostringstream iio_read_device_path;
iio_read_device_path << "/dev/" << IIO_DEVICE_PREFIX << _iio_device_number;

Expand Down Expand Up @@ -533,7 +536,7 @@ namespace librealsense
_is_capturing = true;
_hid_thread = std::unique_ptr<std::thread>(new std::thread([this](){
const uint32_t channel_size = get_channel_size();
auto raw_data_size = channel_size*buf_len;
auto raw_data_size = channel_size*hid_buf_len;

std::vector<uint8_t> raw_data(raw_data_size);
auto metadata = has_metadata();
Expand Down Expand Up @@ -604,6 +607,7 @@ namespace librealsense
return;

_is_capturing = false;
set_power(false);
signal_stop();
_hid_thread->join();
_callback = NULL;
Expand Down Expand Up @@ -646,7 +650,7 @@ namespace librealsense
create_channel_array();

const uint32_t channel_size = get_channel_size();
auto raw_data_size = channel_size*buf_len;
auto raw_data_size = channel_size*hid_buf_len;

std::vector<uint8_t> raw_data(raw_data_size);

Expand All @@ -673,34 +677,21 @@ namespace librealsense
iio_device_file.close();
}

// Zero -delay will suspend immedeately, Negaive - prevent suspend/resume
bool iio_hid_sensor::set_fs_attribute(std::string path, int input)
// Asynchronous power management
void iio_hid_sensor::set_power(bool on)
{
bool res = false;
auto path = _iio_device_path + "/buffer/enable";

std::fstream sysfs_stream(path);
if (!sysfs_stream.is_open())
// Enqueue power management change
_pm_dispatcher.invoke([path,on](dispatcher::cancellable_timer t)
{
LOG_DEBUG("The specified sysfs entry " << path << " is not valid");
return res;
}
auto st = std::chrono::high_resolution_clock::now();

try
{
// Read/Modify/Confirm
int rval = 0;
sysfs_stream >> std::dec >> rval;

if (rval!=input)
if (!write_fs_arithmetic(path, on))
{
sysfs_stream << input;
sysfs_stream >> std::dec >> rval;
LOG_WARNING("HID set_power " << int(on) << " failed for " << path);
}
res = (rval==input);
}
catch (std::exception&){ /*The sysfs may not respond during internal power-up"*/ }

return res;
},true);
}

void iio_hid_sensor::signal_stop()
Expand Down Expand Up @@ -772,6 +763,8 @@ namespace librealsense
throw linux_backend_exception(to_string() << "IIO device number is incorrect! Failed to open device sensor. " << _iio_device_path);
}

_pm_dispatcher.start();

// read all available input of the iio_device
read_device_inputs();

Expand All @@ -782,29 +775,8 @@ namespace librealsense
input->enable(true);

set_frequency(frequency);
write_integer_to_param("buffer/length", buf_len);
write_integer_to_param("buffer/enable", 1);

#ifdef PREVENT_HID_SUSPEND
// Prevent the power-management to control suspended mode
// HID resume (power up) prodices ~2 sec latency per each sensor.
// During the peridod the sysfs HAL node is lazy initialized, requiring async assignment
// Note that this setting applies for non-HID sensors as well
std::string path = _iio_device_path + "/../power/autosuspend_delay_ms";
_pm_thread = std::unique_ptr<std::thread>(new std::thread([path](){
while (true)
{
try{
if (set_fs_attribute(path,-1))
break;
else
std::this_thread::sleep_for(std::chrono::milliseconds(50));
}
catch(...){ break; } // Device disconnect
}
}));
_pm_thread->detach();
#endif // PREVENT_HID_SUSPEND
write_fs_arithmetic(_iio_device_path + "/buffer/length", hid_buf_len);

}

// calculate the storage size of a scan
Expand Down Expand Up @@ -920,24 +892,6 @@ namespace librealsense
closedir(dir);
}

// configure hid device via fd
void iio_hid_sensor::write_integer_to_param(const std::string& param,int value)
{
std::ostringstream device_path;
device_path << _iio_device_path << "/" << param;

std::ofstream iio_device_file(device_path.str());

if (!iio_device_file.good())
{
throw linux_backend_exception(to_string() << "write_integer_to_param failed! device path: " << _iio_device_path);
}

iio_device_file << value;

iio_device_file.close();
}

v4l_hid_device::v4l_hid_device(const hid_device_info& info)
{
bool found = false;
Expand Down
45 changes: 40 additions & 5 deletions src/linux/backend-hid.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,8 @@ namespace librealsense
{
namespace platform
{
const uint32_t hid_buf_len = 128;

struct hid_input_info
{
std::string input = "";
Expand All @@ -59,6 +61,42 @@ namespace librealsense
// TODO: parse 'offset' and 'scale'
};

template<typename T>
inline bool write_fs_arithmetic(const std::string& path, const T& val)
{
static_assert((std::is_arithmetic<T>::value), "write_arithmetic_fs incompatible type");
bool res = false;
std::fstream fs_handle(path);
if (!fs_handle.good())
{
LOG_WARNING(__FUNCTION__ <<" with " << val << " failed. The specified path " << path << " is not valid");
return res;
}

try // Read/Modify/Confirm
{
T cval = 0;
fs_handle >> cval;

if (cval!=val)
{
fs_handle << val;
fs_handle.flush();
std::ifstream vnv_handle(path);
vnv_handle >> cval;
res = (cval==val);
if (!res)
LOG_WARNING(__FUNCTION__ << " Could not change " << cval << " to " << val << " : path " << path);
}
}
catch (const std::exception& exc)
{
LOG_WARNING(__FUNCTION__ << " with " << path << " failed: " << exc.what());
}

return res;
}

// manage an IIO input. or what is called a scan.
class hid_input
{
Expand Down Expand Up @@ -130,12 +168,12 @@ namespace librealsense
void clear_buffer();

void set_frequency(uint32_t frequency);
void set_power(bool on);

void signal_stop();

bool has_metadata();

static bool set_fs_attribute(std::string path, int val);
static bool sort_hids(hid_input* first, hid_input* second);

void create_channel_array();
Expand All @@ -154,10 +192,6 @@ namespace librealsense
// read the IIO device inputs.
void read_device_inputs();

// configure hid device via fd
void write_integer_to_param(const std::string& param,int value);

static const uint32_t buf_len = 128; // TODO
int _stop_pipe_fd[2]; // write to _stop_pipe_fd[1] and read from _stop_pipe_fd[0]
int _fd;
int _iio_device_number;
Expand All @@ -170,6 +204,7 @@ namespace librealsense
std::atomic<bool> _is_capturing;
std::unique_ptr<std::thread> _hid_thread;
std::unique_ptr<std::thread> _pm_thread; // Delayed initialization due to power-up sequence
dispatcher _pm_dispatcher; // Asynchronous power management
};

class v4l_hid_device : public hid_device
Expand Down
33 changes: 22 additions & 11 deletions src/sensor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -443,11 +443,14 @@ namespace librealsense
auto&& unpacker = *mode.unpacker;
for (auto&& output : unpacker.outputs)
{
LOG_DEBUG("FrameAccepted," << librealsense::get_string(output.stream_desc.type) << "," << std::dec << frame_counter
<< "," << output.stream_desc.index << "," << frame_counter
<< ",Arrived," << std::fixed << f.backend_time << " " << std::fixed << system_time<<" diff - "<< system_time- f.backend_time << " "
LOG_DEBUG("FrameAccepted," << librealsense::get_string(output.stream_desc.type)
<< ",Counter," << std::dec << frame_counter
<< ",Index," << output.stream_desc.index
<< ",BackEndTS," << std::fixed << f.backend_time
<< ",SystemTime," << std::fixed << system_time
<<" ,diff_ts[Sys-BE],"<< system_time- f.backend_time
<< ",TS," << std::fixed << timestamp << ",TS_Domain," << rs2_timestamp_domain_to_string(timestamp_domain)
<<" last_frame_number "<< last_frame_number<<" last_timestamp "<< last_timestamp);
<<",last_frame_number,"<< last_frame_number<<",last_timestamp,"<< last_timestamp);

std::shared_ptr<stream_profile_interface> request = nullptr;
for (auto&& original_prof : mode.original_requests)
Expand Down Expand Up @@ -883,8 +886,10 @@ namespace librealsense

_source.init(_metadata_parsers);
_source.set_sensor(this->shared_from_this());
unsigned long long last_frame_number = 0;
rs2_time_t last_timestamp = 0;
raise_on_before_streaming_changes(true); //Required to be just before actual start allow recording to work
_hid_device->start_capture([this](const platform::sensor_data& sensor_data)
_hid_device->start_capture([this,last_frame_number,last_timestamp](const platform::sensor_data& sensor_data) mutable
{
auto system_time = environment::get_instance().get_time_service()->get_time();
auto timestamp_reader = _hid_iio_timestamp_reader.get();
Expand Down Expand Up @@ -928,18 +933,24 @@ namespace librealsense
// Determine the timestamp for this HID frame
auto timestamp = timestamp_reader->get_frame_timestamp(mode, sensor_data.fo);
auto frame_counter = timestamp_reader->get_frame_counter(mode, sensor_data.fo);
auto ts_domain = timestamp_reader->get_frame_timestamp_domain(mode, sensor_data.fo);

frame_additional_data additional_data{};

additional_data.timestamp = timestamp;
additional_data.frame_number = frame_counter;
additional_data.timestamp_domain = timestamp_reader->get_frame_timestamp_domain(mode, sensor_data.fo);
additional_data.timestamp_domain = ts_domain;
additional_data.system_time = system_time;
LOG_DEBUG("FrameAccepted," << get_string(request->get_stream_type()) << "," << std::dec << frame_counter
<< ",Arrived," << std::fixed << system_time
<< ",TS," << std::fixed << timestamp
<< ",TS_Domain," << rs2_timestamp_domain_to_string(additional_data.timestamp_domain));

LOG_DEBUG("FrameAccepted," << get_string(request->get_stream_type())
<< ",Counter," << std::dec << frame_counter << ",Index,0"
<< ",BackEndTS," << std::fixed << sensor_data.fo.backend_time
<< ",SystemTime," << std::fixed << system_time
<<" ,diff_ts[Sys-BE],"<< system_time- sensor_data.fo.backend_time
<< ",TS," << std::fixed << timestamp << ",TS_Domain," << rs2_timestamp_domain_to_string(ts_domain)
<<",last_frame_number,"<< last_frame_number<<",last_timestamp,"<< last_timestamp);

last_frame_number = frame_counter;
last_timestamp = timestamp;
auto frame = _source.alloc_frame(RS2_EXTENSION_MOTION_FRAME, data_size, additional_data, true);
if (!frame)
{
Expand Down
5 changes: 3 additions & 2 deletions unit-tests/unit-tests-live.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4857,7 +4857,8 @@ TEST_CASE("Syncer try wait for frames", "[live][software-device]") {
}
}

TEST_CASE("Projection from recording", "[software-device][using_pipeline][projection]") {
// Marked as MayFail due to DSO-11753. TODO -revisit once resolved
TEST_CASE("Projection from recording", "[software-device][using_pipeline][projection][!mayfail]") {
rs2::context ctx;
if (!make_context(SECTION_FROM_TEST_NAME, &ctx))
return;
Expand All @@ -4882,7 +4883,7 @@ TEST_CASE("Projection from recording", "[software-device][using_pipeline][projec

while (!depth_profile || !color_profile)
{
frameset frames = sync.wait_for_frames(200);
frameset frames = sync.wait_for_frames();
REQUIRE(frames.size() > 0);
if (frames.size() == 1)
{
Expand Down

0 comments on commit 5b3dd73

Please sign in to comment.