control_position(curr_pos, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current)