Autopilot not navigating in 3D?

Hi all!

I am having a problem with how arduplane navigates from WP to WP in Auto mode.


Single motor fixed wing UAV
Hex/ProfiCNC Cube Black
Arduplane 4 firmware

If we look at the actual flight path in latitude longitude plane the autopilot absolutely nails each waypoint. Nothing wrong with that.

But altitude seems weird, almost like the autopilot has no trajectory planning at all in altitude.

Looking at the log files it goes like this. Autotune is not perfect either but you can still see the problem. (white dots mark waypoints with different altitide):

  1. Autopilot navigates to WP1
  2. Once WP1 is reached it looks at the altitude of WP1 and adjusts while heading to WP2

This is probably ok if you fly somewhere where the terrain is flat. I have experienced couple crashes because of this as well.

What i am trying to do is low altitude mapping passes over more or less extreme hills. I have generated my WPs from actual terrain data with external software and each WP has proper offset from ground to clear treetops. The plane would not crash if it followed the CMD.Alt line as well as it follows lat-long path.

Is this expected behaviour or have I done something wrong? My current workaround is to use way more waypoints and manually rise them when the flight path passes over steep hill but the performance is still poor and “wasted” waypoints mean shorter missions.