Proximity terrain following

I’m starting new development using ArduPilot which basically would allow the drone to fly really close from the ground (~1-2 meters) using a downward facing Lidar or Sonar. The idea is to filter the data provided by the Lidar (and others input data) to define the minimum altitude of flight during mission. The idea of machine learning could be use to provide better results (apply for vineyard in my case).

In order to start the development, I have few questions about the code:

  • If I want to set the altitude that the drone should have, where this altitude needs to be define ? I try to find where the final decision is set but I can’t figure it out. In AC_WPNav.h, the function set_rangefinder_alt update the altitude of the range finder, but AC_PosControl.cpp only use _inva.get_altitude() in pos_to_rate_z() function.

  • To add new functions which take multiple data to provide a final altitude, should I write a library in the same way than Object avoidance ? I don’t get all the interaction between classes and it’s hard to understand the best way to add my code in the project…

Some explanations on this interaction and the way the altitude is taken into account will be greatly appreciated !

I have a working code for that here:

1 Like

Awesome ! That definitely what I was looking for, thank you. I guess what I could do is to create an low virtual fence by taking all the parameters I need and using some algorithms. My problem will be that if the drone flies using PID with LiDAR and others sensors it will just going up and down around the fence every time it reach it ?

I still need to figure it out for the details but thanks a lot. I have a good idea (and a little of your code :slightly_smiling_face:) to modify the code in a good way.

The LW20 and SF11 LiDARs they have a bunch of parameters that can be set independently of the ArduPilot code to filter out rapid changes in distance readings - like fences. This might be an easier approach than trying to code the filtering into ArduPilot directly. In the future we will make all of the features available through the ArduPilot code, we just haven’t got around to it yet ;).


Here you can see my low flight altitude test with sf11 if it usefull for you

Interesting, did you do the flight with “Terrain Following” enabled or with the solution of amilcarlucas and his low altitude fence ?

I need to fly over rows of vineyard, but with Terrain Following the drone just go up and down in a crazy way ! Maybe by working on parameters as proposed by Laser_Developper I can decrease the influence of this bounds between rows.

Otherwise, my idea would be to apply a low pass filter on the measure of the rangefinder and apply the result as a low altitude fence for the drone. Need to test different possibilities…

A low pass filter is not a good solution owing to the fact that results from shorter readings still get included in the altitude calculation. A better approach is to create a rolling buffer containing successive results. The buffer must be large enough to hold results from both the ground and the obstacle. For each new reading, sort the buffer into size order and use the longest readings to establish the position of the ground. Alternatively, the mid point of the buffer (median) gives a more indicative result when you want to speed up the response to real changes in the AGL altitude.
The LW20 provides both the “long” and “median” results.

Oh that’s a shame, I’m working with a SF10 and I didn’t see this option… I need to check about the LW20, but if these “long” or “median” methods could work for my situation, it will definitely be the best solution ! Need to discuss about it with my teacher. Thank you for your advice !

I fly commonly in pos hold but I try terrain and go nice too, how far are the plants? I test over dry corn and go nice too. perhaps as more space between plants you need less laser readings or more fly speed; I didn´t play with sf11 config because default go perfect for dense crops.

The goal is to be as close as possible so around one meter from the plants would be ideal. The rows between each rows are to width and give some problems with pos hold, yet. As proposed by Laser_Developer I need to define where is the ground at each time of the flight, And then flight at fixed distance from that ground.

But I’m surprised that in your video the drone don’t go down when you reach a zone without corn.

Before bad experiences over crops with lidar lite 2 I was surprised too how this laser performs over crops, I hope developers too, now is my standard altitude sensor for long time and never a crazy reaction, looks how is installed as developers recomendation too, oblicue and in ADG port. Perhaps that help. The other advantage is in windy days, altitude is not affected and has a rock solid pos hold and easier to land too.

Good morning… I would like to know if someone could make a Terrain Following sistem or maybe a prototype. Please send me an e-mail at . Cheers

Like I said, the code above should help you getting started. Suggestions and improvements are welcome.
Katawan, sorry had no time to reply to your questions.

@mfep just buy a RTK GPS and do a mission at a constant altitude. it is easier.
If the terrain is not flat enough … you need to use the code above.

No worries. I used your code and it was really helpful as basis to find my way in the ardupilot code.

@mfep I’m working on an improvement of the actual terrain following mode using LiDAR and some filtering for my master project. I would be glad to hear the features you’re looking for to see if I can add it

Good morning… thank you for your reply, in fact, I am looking for a “Terrain Following” system using a LIDAR (as your master project). I am no sure if this system could be work in a DJI drone such as Matrice600