Wrong altitude (sonar enabled)

Hi,

I’ve made my diy HC-SR04 sonar for APM 2.8 with V3.2.1 firmware. Sonar data is correct (from 2 cm to 150 cm), but in modes like alt hold copter cant maintain it’s altitude. Relative altitude is wrong too. How to enable sonar in flight?

Best regards