There are likely 2 issues here. One was a floating point math fix that is in newer versions of PX4. Can you update to latest px4: http://voxl-packages.modalai.com/dists/qrb5165/dev/binary-arm64/voxl-px4_1.14.0-2.0.146-202607161052_arm64.deb and try again? The other issue is that sending in trajectory setpoints from your external application, even when in altitude mode, will go straight into the position controller and affect flight. So, I would only send those in when in offboard mode.