Quad + Lightware SF11/C + Terrain following -> Wobble!

Hi All,

I am working on agricultural surveying and trying to maintain an altitude around 50m.
I picked up an SF11/C and fought through all of the connectivity issues between I2C / Analog connectivity. In the end it turned out I was having EMI issues due to my Lidar being close to my antennas. I added shielding and moved it to solve those problems and finally was able to achieve quite good flight with the drone in auto mode.

However, I am having cases where when flying over a stand of trees (30m height) or sometimes when flying up a small incline the drone will stop/ wobble and try to immediately do hard altitude correction which brings the drone to a complete stop, which then causes a large amount of wobble and ‘confusion’ which can even get open loop and require me to put the drone into another mode like althold so that it can stabalize before continuing the flight.

My question is this: Has anyone else seen anything like this?
My drone is well powered, so I don’t think it is a question of saturating the motors, but rather a case where the altitude holding signal is overpowering the stabilization signal in the case where the drone is initially tilted in forward flight but then the relative altitude quickly changes creating a large error when the drone goes to climb due to the ground obstacle.

Frankly what I wish is that we had PID control over the LIDAR input so that we could choose our own filteringfor the lidar input. All this happens with the LIDAR gain set to 0.01 (lowest setting) and while this works better than the standard setting, it still is vastly too high to get the result that I am looking for (smooth flight which will correct for slanted field cases)

Thoughts / experiences / Questions welcome!