It appears that "sonarrange" data is the raw data measured by Lidar Lite and used to set altitude below RNGFND_MAX_CM . However 45 degrees pitch causes 12m drop on 40m intended altitude and 0.9 m drop on 3 m. Calculating with 20 degree pitch the variance on 40 meter is 2.75m and on 3 m it is 21 cm.
Furthermore, with moving copter Lidar will always measure the distance well behind the copter's location (on 40m altitude and 45 degree pitch the raw data from lidar will result in 28m real altitude and measuring the ground distance 28 meter behind the copter's position ) .. so Lidar would need a downward facing gimbal to be fully useful on a moving copter.
Talking about software - two things would be useful to be configurable: (1) correction of raw data by Pixhawk based on pitch/roll (2) disable range-finder and use barometer above a given degree of pitch/roll
Replies
..or you could use gimbal function to pitch/roll stabilize the sensor by two simple servos... :)