Also this call, given that you managed now to get the WPs out of MAVLink, should run in the navigator.