Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Fix IMU handling for D400 series #4587

Merged
merged 7 commits into from
Aug 13, 2019
Merged

Fix IMU handling for D400 series #4587

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
417ff24
Refactor IMU output data rate assignments
ev-mp Aug 5, 2019
853e7d0
Fix Linux build. Resolve minor GCC warnings
ev-mp Aug 6, 2019
bf94375
Disable IMU Temperature sensor for D465
ev-mp Aug 6, 2019
c9434da
Adjust RGB controls according to specific PIDs
ev-mp Aug 6, 2019
db32335
Adjust default IMU rates for D400 SKUs
ev-mp Aug 7, 2019
6739a69
Add IMU extrinsic handling according to the SKU type for D400 series
ev-mp Aug 7, 2019
ca988e1
ED VErsion 2.25.11 label
ev-mp Aug 11, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion include/librealsense2/rs.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@ extern "C" {

#define RS2_API_MAJOR_VERSION 2
#define RS2_API_MINOR_VERSION 25
#define RS2_API_PATCH_VERSION 0
#define RS2_API_PATCH_VERSION 11
#define RS2_API_BUILD_VERSION 0

#ifndef STRINGIFY
Expand Down
2 changes: 1 addition & 1 deletion src/ds5/advanced_mode/advanced_mode.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -69,13 +69,13 @@ namespace librealsense
{
case ds::RS410_PID:
case ds::RS415_PID:
case ds::RS465_PID:
default_410(p);
break;
case ds::RS430_PID:
case ds::RS430I_PID:
case ds::RS435_RGB_PID:
case ds::RS435I_PID:
case ds::RS465_PID:
default_430(p);
break;
case ds::RS405_PID:
Expand Down
14 changes: 9 additions & 5 deletions src/ds5/ds5-color.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -60,15 +60,21 @@ namespace librealsense
color_ep->register_pixel_format(pf_bayer16);
color_ep->register_pixel_format(pf_uyvyc);

color_ep->register_pu(RS2_OPTION_BACKLIGHT_COMPENSATION);
color_ep->register_pu(RS2_OPTION_BRIGHTNESS);
color_ep->register_pu(RS2_OPTION_CONTRAST);
color_ep->register_pu(RS2_OPTION_SATURATION);
color_ep->register_pu(RS2_OPTION_GAIN);
color_ep->register_pu(RS2_OPTION_GAMMA);
color_ep->register_pu(RS2_OPTION_HUE);
color_ep->register_pu(RS2_OPTION_SATURATION);
color_ep->register_pu(RS2_OPTION_SHARPNESS);

// Currently disabled for certain sensors
if (!val_in_range(color_devices_info.front().pid, { ds::RS465_PID }))
{
color_ep->register_pu(RS2_OPTION_HUE);
color_ep->register_pu(RS2_OPTION_BACKLIGHT_COMPENSATION);
color_ep->register_pu(RS2_OPTION_AUTO_EXPOSURE_PRIORITY);
}

auto white_balance_option = std::make_shared<uvc_pu_option>(*color_ep, RS2_OPTION_WHITE_BALANCE);
auto auto_white_balance_option = std::make_shared<uvc_pu_option>(*color_ep, RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE);
color_ep->register_option(RS2_OPTION_WHITE_BALANCE, white_balance_option);
Expand All @@ -94,8 +100,6 @@ namespace librealsense
{ 2.f, "60Hz" },
{ 3.f, "Auto" }, }));

color_ep->register_pu(RS2_OPTION_AUTO_EXPOSURE_PRIORITY);

color_ep->register_metadata(RS2_FRAME_METADATA_FRAME_TIMESTAMP, make_uvc_header_parser(&platform::uvc_header::timestamp));
color_ep->register_metadata(RS2_FRAME_METADATA_ACTUAL_FPS, std::make_shared<ds5_md_attribute_actual_fps> (false, [](const rs2_metadata_type& param)
{return param*100;})); //the units of the exposure of the RGB sensor are 100 microseconds so the md_attribute_actual_fps need the lambda to convert it to microseconds
Expand Down
19 changes: 15 additions & 4 deletions src/ds5/ds5-device.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -478,7 +478,7 @@ namespace librealsense
return {};
}

ds::d400_caps ds5_device::parse_device_capabilities() const
ds::d400_caps ds5_device::parse_device_capabilities(const uint16_t pid) const
{
using namespace ds;
std::array<unsigned char,HW_MONITOR_BUFFER_SIZE> gvd_buf;
Expand All @@ -491,13 +491,24 @@ namespace librealsense
if (gvd_buf[rgb_sensor]) // WithRGB
val |= d400_caps::CAP_RGB_SENSOR;
if (gvd_buf[imu_sensor])
{
val |= d400_caps::CAP_IMU_SENSOR;
if (hid_bmi_055_pid.end() != hid_bmi_055_pid.find(pid))
val |= d400_caps::CAP_BMI_055;
else
{
if (hid_bmi_085_pid.end() != hid_bmi_085_pid.find(pid))
val |= d400_caps::CAP_BMI_085;
else
LOG_WARNING("The IMU sensor is undefined for PID " << std::hex << pid << std::dec);
}
}
if (0xFF != (gvd_buf[fisheye_sensor_lb] & gvd_buf[fisheye_sensor_hb]))
val |= d400_caps::CAP_FISHEYE_SENSOR;
if (0x1 == gvd_buf[depth_sensor_type])
val |= d400_caps::CAP_ROLLING_SHUTTER; // Standard depth
val |= d400_caps::CAP_ROLLING_SHUTTER; // e.g. ASRC
if (0x2 == gvd_buf[depth_sensor_type])
val |= d400_caps::CAP_GLOBAL_SHUTTER; // Wide depth
val |= d400_caps::CAP_GLOBAL_SHUTTER; // e.g. AWGC

return val;
}
Expand Down Expand Up @@ -598,7 +609,7 @@ namespace librealsense

_recommended_fw_version = firmware_version(D4XX_RECOMMENDED_FIRMWARE_VERSION);
if (_fw_version >= firmware_version("5.10.4.0"))
_device_capabilities = parse_device_capabilities();
_device_capabilities = parse_device_capabilities(pid);

auto& depth_ep = get_depth_sensor();
auto advanced_mode = is_camera_in_advanced_mode();
Expand Down
2 changes: 1 addition & 1 deletion src/ds5/ds5-device.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -62,7 +62,7 @@ namespace librealsense

float get_stereo_baseline_mm() const;

ds::d400_caps parse_device_capabilities() const;
ds::d400_caps parse_device_capabilities(const uint16_t pid) const;

void init(std::shared_ptr<context> ctx,
const platform::backend_device_group& group);
Expand Down
24 changes: 12 additions & 12 deletions src/ds5/ds5-factory.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -257,8 +257,8 @@ namespace librealsense
if (_fw_version >= firmware_version("5.10.4.0"))
{
tags.push_back({ RS2_STREAM_FISHEYE, -1, 640, 480, RS2_FORMAT_RAW8, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT});
tags.push_back({RS2_STREAM_GYRO, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, 200, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({RS2_STREAM_ACCEL, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, 125, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({RS2_STREAM_GYRO, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, (int)odr::IMU_FPS_200, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({RS2_STREAM_ACCEL, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, (int)odr::IMU_FPS_63, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
}

return tags;
Expand Down Expand Up @@ -361,8 +361,8 @@ namespace librealsense
tags.push_back({ RS2_STREAM_INFRARED, 1, 640, 480, RS2_FORMAT_Y8, 15, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_INFRARED, 2, 640, 480, RS2_FORMAT_Y8, 15, profile_tag::PROFILE_TAG_SUPERSET });
}
tags.push_back({ RS2_STREAM_GYRO, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, 200, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_ACCEL, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, 63, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_GYRO, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, (int)odr::IMU_FPS_200, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_ACCEL, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, (int)odr::IMU_FPS_63, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });

return tags;
};
Expand Down Expand Up @@ -407,8 +407,8 @@ namespace librealsense
tags.push_back({ RS2_STREAM_FISHEYE, -1, 640, 480, RS2_FORMAT_RAW8, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT});
if (_fw_version >= firmware_version("5.10.4.0")) // Back-compat with records is required
{
tags.push_back({RS2_STREAM_GYRO, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, 200, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({RS2_STREAM_ACCEL, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, 125, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({RS2_STREAM_GYRO, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, (int)odr::IMU_FPS_200, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({RS2_STREAM_ACCEL, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, (int)odr::IMU_FPS_63, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
}

return tags;
Expand Down Expand Up @@ -526,8 +526,8 @@ namespace librealsense
tags.push_back({ RS2_STREAM_COLOR, -1, width, height, RS2_FORMAT_RGB8, fps, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_DEPTH, -1, width, height, RS2_FORMAT_Z16, fps, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_INFRARED, -1, width, height, RS2_FORMAT_Y8, fps, profile_tag::PROFILE_TAG_SUPERSET });
tags.push_back({RS2_STREAM_GYRO, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, 200, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({RS2_STREAM_ACCEL, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, 63, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({RS2_STREAM_GYRO, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, (int)odr::IMU_FPS_200, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({RS2_STREAM_ACCEL, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, (int)odr::IMU_FPS_63, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });

return tags;
};
Expand Down Expand Up @@ -735,8 +735,8 @@ namespace librealsense
tags.push_back({ RS2_STREAM_COLOR, -1, width, height, RS2_FORMAT_RGB8, fps, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_DEPTH, -1, width, height, RS2_FORMAT_Z16, fps, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_INFRARED, -1, width, height, RS2_FORMAT_Y8, fps, profile_tag::PROFILE_TAG_SUPERSET });
tags.push_back({ RS2_STREAM_GYRO, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, 200, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_ACCEL, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, 63, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_GYRO, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, (int)odr::IMU_FPS_200, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_ACCEL, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, (int)odr::IMU_FPS_100, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });

return tags;
};
Expand All @@ -759,8 +759,8 @@ namespace librealsense
std::vector<tagged_profile> get_profiles_tags() const override
{
std::vector<tagged_profile> tags;
tags.push_back({RS2_STREAM_GYRO, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, 200, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({RS2_STREAM_ACCEL, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, 63, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({RS2_STREAM_GYRO, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, (int)odr::IMU_FPS_200, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({RS2_STREAM_ACCEL, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, (int)odr::IMU_FPS_63, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });

return tags;
};
Expand Down
27 changes: 22 additions & 5 deletions src/ds5/ds5-motion.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -167,6 +167,22 @@ namespace librealsense
std::unique_ptr<frame_timestamp_reader> iio_hid_ts_reader(new iio_hid_timestamp_reader());
std::unique_ptr<frame_timestamp_reader> custom_hid_ts_reader(new ds5_custom_hid_timestamp_reader());
auto enable_global_time_option = std::shared_ptr<global_time_option>(new global_time_option());

// Dynamically populate the supported HID profiles according to the selected IMU module
std::vector<odr> accel_fps_rates;
std::map<unsigned, unsigned> fps_and_frequency_map;
if (ds::d400_caps::CAP_BMI_085 && _device_capabilities)
accel_fps_rates = { odr::IMU_FPS_100,odr::IMU_FPS_200 };
else // Applies to BMI_055 and unrecognized sensors
accel_fps_rates = { odr::IMU_FPS_63,odr::IMU_FPS_250 };

for (auto&& elem : accel_fps_rates)
{
sensor_name_and_hid_profiles.push_back({ accel_sensor_name, { RS2_STREAM_ACCEL, 0, 1, 1, static_cast<uint16_t>(elem), RS2_FORMAT_MOTION_XYZ32F } });
fps_and_frequency_map.emplace(unsigned(elem), hid_fps_translation.at(elem));
}
fps_and_sampling_frequency_per_rs2_stream[RS2_STREAM_ACCEL] = fps_and_frequency_map;

auto hid_ep = std::make_shared<ds5_hid_sensor>(this, ctx->get_backend().create_hid_device(all_hid_infos.front()),
std::unique_ptr<frame_timestamp_reader>(new global_timestamp_reader(std::move(iio_hid_ts_reader), _tf_keeper, enable_global_time_option)),
std::unique_ptr<frame_timestamp_reader>(new global_timestamp_reader(std::move(custom_hid_ts_reader), _tf_keeper, enable_global_time_option)),
Expand All @@ -179,7 +195,7 @@ namespace librealsense

uint16_t pid = static_cast<uint16_t>(strtoul(all_hid_infos.front().pid.data(), nullptr, 16));

if ((camera_fw_version >= firmware_version(custom_sensor_fw_ver)) && (!val_in_range(pid, { ds::RS400_IMU_PID, ds::RS435I_PID, ds::RS430I_PID })))
if ((camera_fw_version >= firmware_version(custom_sensor_fw_ver)) && (!val_in_range(pid, { ds::RS400_IMU_PID, ds::RS435I_PID, ds::RS430I_PID, ds::RS465_PID })))
{
hid_ep->register_option(RS2_OPTION_MOTION_MODULE_TEMPERATURE,
std::make_shared<motion_module_temperature_option>(*hid_ep));
Expand Down Expand Up @@ -247,7 +263,7 @@ namespace librealsense
{
using namespace ds;

_mm_calib = std::make_shared<mm_calib_handler>(_hw_monitor);
_mm_calib = std::make_shared<mm_calib_handler>(_hw_monitor,_device_capabilities);

_accel_intrinsic = [this]() { return _mm_calib->get_intrinsic(RS2_STREAM_ACCEL); };
_gyro_intrinsic = [this]() { return _mm_calib->get_intrinsic(RS2_STREAM_GYRO); };
Expand Down Expand Up @@ -443,7 +459,8 @@ namespace librealsense
//});
}

mm_calib_handler::mm_calib_handler(std::shared_ptr<hw_monitor> hw_monitor) : _hw_monitor(hw_monitor)
mm_calib_handler::mm_calib_handler(std::shared_ptr<hw_monitor> hw_monitor, ds::d400_caps dev_cap) :
_hw_monitor(hw_monitor), _dev_cap(dev_cap)
{
_imu_eeprom_raw = [this]() { return get_imu_eeprom_raw(); };

Expand All @@ -468,8 +485,8 @@ namespace librealsense
switch (calib_id)
{
case ds::dm_v2_eeprom_id: // DM V2 id
prs = std::make_shared<dm_v2_imu_calib_parser>(raw,valid); break;
case ds::tm1_eeprom_id:// TM1 id
prs = std::make_shared<dm_v2_imu_calib_parser>(raw, _dev_cap, valid); break;
case ds::tm1_eeprom_id: // TM1 id
prs = std::make_shared<tm1_imu_calib_parser>(raw); break;
default:
throw recoverable_exception(to_string() << "Motion Intrinsics unresolved - "
Expand Down
Loading