@Leonardthall Your modified firmware looks much better 
I will do some further tests in hilly terrain and look how it performs there.
Logs: https://1drv.ms/u/s!AunrEY0aDm-h7j1IWknjvT6NvXTs?e=iGAHdI
PSC_TC_Z 0.25:
PSC_TC_Z 0.50:
1 Like