Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 00:58:37 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: fix NAV_CONTROLLER_OUTPUT in Q modes

Browse Source 
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 committed by Randy Mackay
parent 2b257663dc
commit d7913104cb
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 #if HAL_QUADPLANE_ENABLED
const QuadPlane &quadplane = plane.quadplane; 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(); 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( mavlink_msg_nav_controller_output_send(
chan, chan,
targets.x * 0.01, targets.x * 0.01,
targets.y * 0.01, targets.y * 0.01,
targets.z * 0.01, targets.z * 0.01,
wp_nav_valid ? quadplane.wp_nav->get_wp_bearing_to_destination() : 0, degrees(error.angle()),
wp_nav_valid ? MIN(quadplane.wp_nav->get_wp_distance_to_destination() * 0.01, UINT16_MAX) : 0, error.length(),
(plane.control_mode != &plane.mode_qstabilize) ? quadplane.pos_control->get_pos_error_z_cm() * 0.01 : 0, (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 plane.airspeed_error * 100, // incorrect units; see PR#7933
wp_nav_valid ? quadplane.wp_nav->crosstrack_error() : 0); quadplane.wp_nav->crosstrack_error());
return; return;
} }
#endif #endif

6
ArduPlane/quadplane.cpp
View File

@ -3432,12 +3432,6 @@ bool QuadPlane::using_wp_nav(void) const
if (plane.control_mode == &plane.mode_qloiter || plane.control_mode == &plane.mode_qland) { if (plane.control_mode == &plane.mode_qloiter || plane.control_mode == &plane.mode_qland) {
return true; 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; return false;
} }