From 773d261c6a67bb2ad0986e2d0a28a639c9b19715 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 29 Jan 2022 07:49:00 +1100 Subject: [PATCH] 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 | 13 ++++++++----- ArduPlane/quadplane.cpp | 6 ------ 2 files changed, 8 insertions(+), 11 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 39f2a06e4e..b55338556c 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/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 diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 865522a292..6f174a23b5 100644 --- a/ArduPlane/quadplane.cpp +++ b/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; }