Moin!
Ich habe keine Ahnung von C, aber nur aus Interesse: Was hast du mit dem US Sensor vor? Höhe halten oder navigieren?