I found the solution if anyone is interested. It's this little line in
_hgt_dem_adj = 0.1f * _hgt_dem + 0.9f * _hgt_dem_adj_last;
Adjusting more weight to
_hgt_dem_adj_last will make transitions in altitude smoother and decreasing will make it more discrete.
There are a few tweaks to think about when changing the value, for example, do we want it smooth on both ends? But I will just make a parameter for it for now and see how it goes.
With this setting smoother you can also set your TECS time constant a bit lower.
Is this pull request worthy? @dagar?