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

Plane: QRTL: ensure close to home change to pos1 is not overwitten

This commit is contained in:
Iampete1 2024-03-15 21:02:42 +00:00 committed by Andrew Tridgell
parent 439c85e793
commit 0a23576fe9
1 changed files with 15 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
ArduPlane/mode_qrtl.cpp
View File

@ -14,6 +14,7 @@ bool ModeQRTL::_enter()
submode = SubMode::RTL;
plane.prev_WP_loc = plane.current_loc;
bool jump_to_pos1 = false;
int32_t RTL_alt_abs_cm = plane.home.alt + quadplane.qrtl_alt*100UL;
if (quadplane.motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED) {
// VTOL motors are active, either in VTOL flight or assisted flight
@ -57,7 +58,7 @@ bool ModeQRTL::_enter()
// we're close to destination and already running VTOL motors, don't transition and don't climb
gcs().send_text(MAV_SEVERITY_INFO,"VTOL position1 d=%.1f r=%.1f", dist, radius);
poscontrol.set_state(QuadPlane::QPOS_POSITION1);
jump_to_pos1 = true;
}
}
@ -65,6 +66,10 @@ bool ModeQRTL::_enter()
plane.do_RTL(RTL_alt_abs_cm);
quadplane.poscontrol_init_approach();
if (jump_to_pos1) {
poscontrol.set_state(QuadPlane::QPOS_POSITION1);
}
int32_t from_alt;
int32_t to_alt;
if (plane.current_loc.get_alt_cm(Location::AltFrame::ABSOLUTE,from_alt) && plane.next_WP_loc.get_alt_cm(Location::AltFrame::ABSOLUTE,to_alt)) {
@ -134,18 +139,24 @@ void ModeQRTL::run()
Location destination = plane.calc_best_rally_or_home_location(plane.current_loc, RTL_alt_abs_cm);
const float dist = plane.current_loc.get_distance(destination);
const float radius = get_VTOL_return_radius();
if (dist < radius) {
// if close to home return at current target altitude
// if close to home enter pos1 and return at current target altitude
const bool jump_to_pos1 = dist < radius;
if (jump_to_pos1) {
int32_t target_alt_abs_cm;
if (plane.next_WP_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, target_alt_abs_cm)) {
RTL_alt_abs_cm = MIN(RTL_alt_abs_cm, target_alt_abs_cm);
}
gcs().send_text(MAV_SEVERITY_INFO,"VTOL position1 d=%.1f r=%.1f", dist, radius);
poscontrol.set_state(QuadPlane::QPOS_POSITION1);
}
plane.do_RTL(RTL_alt_abs_cm);
quadplane.poscontrol_init_approach();
if (jump_to_pos1) {
poscontrol.set_state(QuadPlane::QPOS_POSITION1);
}
if (plane.current_loc.get_alt_distance(plane.next_WP_loc, alt_diff)) {
poscontrol.slow_descent = is_positive(alt_diff);
} else {