heading and thrust PIDs work