Lidar as the first source of height

Hi everyone.
I have connected to pixhawk lidar benewakeTf mini. It works very well. I would like the copter to calculate the height from this sensor considering the tilt angles. In short, it needs a height from the lidar to multiply by the cosine angle pich and roll to have a real distance from the ground.(distance= distance*cos(roll)*cos(pitch) How to modify the code and which variables to use?
Thanks

@MarcK

I guess that you are using it as a Serial Lightware ?
Generally when you look at the signal (reading sinar range) you will see the ‘‘non compensated’’ signal which make sense because its coming directly from the RangeFinder.

If you look at the code, you can see that it is using the IMU to compensate (up to 45 deg):
https://github.com/ArduPilot/ardupilot/blob/master/ArduCopter/sensors.cpp#L57

Yes.I conected lidar as Serial Lightware.So I suspected that the data from the lidara in the log are raw. Thanks a lot for the precise answer