mavlink: set current WP working as expected, report current WP with 0.5 Hz