tuned control loops, added ultrasound height, hacked the height in ins_int.h