Plane: fix NAV_CONTROLLER_OUTPUT in Q modes

Q modes don't always use wp_nav, but do use pos_control

AUTO and QRTL setup the plane navigation as well, so can use the L1
data

ArduPlane/GCS_Mavlink.cpp

@@ -172,20 +172,23 @@ void GCS_MAVLINK_Plane::send_nav_controller_output() const
     }
 #if HAL_QUADPLANE_ENABLED
     const QuadPlane &quadplane = plane.quadplane;
-    if (quadplane.show_vtol_view()) {
+    if (quadplane.show_vtol_view() && quadplane.using_wp_nav()) {
         const Vector3f &targets = quadplane.attitude_control->get_att_target_euler_cd();
-        bool wp_nav_valid = quadplane.using_wp_nav();
+
+        const Vector2f& curr_pos = quadplane.inertial_nav.get_position_xy_cm();
+        const Vector2f& target_pos = quadplane.pos_control->get_pos_target_cm().xy().tofloat();
+        const Vector2f error = (target_pos - curr_pos) * 0.01;
 
         mavlink_msg_nav_controller_output_send(
             chan,
             targets.x * 0.01,
             targets.y * 0.01,
             targets.z * 0.01,
-            wp_nav_valid ? quadplane.wp_nav->get_wp_bearing_to_destination() : 0,
-            wp_nav_valid ? MIN(quadplane.wp_nav->get_wp_distance_to_destination() * 0.01, UINT16_MAX) : 0,
+            degrees(error.angle()),
+            error.length(),
             (plane.control_mode != &plane.mode_qstabilize) ? quadplane.pos_control->get_pos_error_z_cm() * 0.01 : 0,
             plane.airspeed_error * 100,  // incorrect units; see PR#7933
-            wp_nav_valid ? quadplane.wp_nav->crosstrack_error() : 0);
+            quadplane.wp_nav->crosstrack_error());
         return;
     }
 #endif

ArduPlane/quadplane.cpp

@@ -3432,12 +3432,6 @@ bool QuadPlane::using_wp_nav(void) const
     if (plane.control_mode == &plane.mode_qloiter || plane.control_mode == &plane.mode_qland) {
         return true;
     }
-    if (plane.control_mode == &plane.mode_qrtl && poscontrol.get_state() >= QPOS_POSITION2) {
-        return true;
-    }
-    if (plane.control_mode == &plane.mode_auto && poscontrol.get_state() > QPOS_APPROACH) {
-        return true;
-    }
     return false;
 }