Properly setting sampling time for altitude PID with LiDAR. Fixed HW impl tests for lidar_t changes.
This reverts commit 822cbd7c