Fix range_link tf bug
Calculate thrust command from z velocity error