Confidence level is reported incorrectly
There are two bugs causing confidence to be reported incorrectly.
https://gitlab.com/voxl-public/tof_cam_ros/blob/master/src/TOFCameraManager.cpp#L461 should be sizeof(uint32_t)
, current code causes intensity field to overlap with confidence field.
https://gitlab.com/voxl-public/tof_cam_ros/blob/master/src/TOFCameraManager.cpp#L633 source is only uint8_t
, copying sizeof(uint32_t)
bytes results in most significant bits filled with junk. Need to assign the confidence to temporary uint32_t first or use a different method for filling 3 most significant bytes with 0.
Edited by Kārlis Seņko