Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
Andrew Tridgell 2022-01-29 07:49:00 +11:00
parent 3fe9a55269
commit 33bc067edf
2 changed files with 8 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
ArduPlane/GCS_Mavlink.cpp
View File

@ -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

6
ArduPlane/quadplane.cpp
View File

@ -3436,12 +3436,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;
}