Guided mode take off with rangefinder

Hello all.
I’m using modified Copter 4.0.7 and I have an issue with rangefinder.

Only in Guided mode, Take off only when I use LiDAR rangefinder for an altimeter, there is a problem in altitude command.
After my copter reached to take off altitude, the altitude decent less then 1 meter.
The altitude command, DAlt, also make about 1 meter decent command.
It looks like an overshoot on altitude controller but the command is generated the looks overshoot response.

It just happen with Guided Takeoff only. The copter flies normally during WP flight in guided mode.
In auto mode there is no issue.
Only the guided takeoff makes me crazy.
And also, there is no problem without rangefinder.

My parameter, rangefinder sets normally according to document.
But I did not set the RNGFND1_GNDCLEAR and RNGFND_GAIN.
The RNGFND1_GNDCLEAR is 10, but exact clearance is about 40cm, and RNGFND_GAIN is 0.8 as a default value.
Can it be a reason of the problem?
I cannot found any relationship with two parameters in code…
And also, I set the TERRAN_ENABLE = 1 and the EKF parameters not to use rangefinder as an altitude source.

In addition, I found something in the code, AC_WPNav.cpp.
A function “advance_wp_target_along_track” seems to engage during the guided takeoff.
And a variable “terr_offset”, in the function, has none-zero value during the guided takeoff only.
It has 0 value during Guided WP flight.
According to the code, the value “terr_offset” means the relative height of the ground from home point, but I have no idea why it only has a value during takeoff and seems to make a problem.

My topic is toooooo long :stuck_out_tongue:
And because of my short English, it seems to make misunderstandings.
If you can not understand my sentences properly, please ask me.
I’ll do my best to explain more correct sentences.
Have a nice day.

Can you re-test with ArduCopter 4.1.0-beta8 version?

Hello, amilcarlucas!
and Thanks for you suggestion.

I also hope to test with latest version.
But, cause of my situation there is a barely chance to do it.

Anyway, I got a suspect of my problem.
If there is a downward rangefinder on drone, AltFrame always set to “ABOVE_TERRAIN”.
It looks only be happened in Guided Takeoff mode, via “do_user_takeoff_start” function.
I think it can be easily solved just bypass the function.

Basically, I also wonder why the “ABOVE_TERRAIN” makes the problem.
I’ll compare with 4.1.0 code.