Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 15:53:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: avoid transition in QRTL

Browse Source 
if close to home and already in a VTOL mode then don't transition
This commit is contained in:
Andrew Tridgell 2021-06-01 16:21:26 +10:00
parent 3bb840f794
commit fbf5083e6a
1 changed files with 7 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ArduPlane/quadplane.cpp
View File

@ -3089,6 +3089,13 @@ void QuadPlane::init_qrtl(void)
pos_control->set_accel_desired_xy_cmss(Vector2f());
pos_control->init_xy_controller();
poscontrol_init_approach();
float dist = plane.next_WP_loc.get_distance(plane.current_loc);
const float radius = MAX(fabsf(plane.aparm.loiter_radius), fabsf(plane.g.rtl_radius));
if (dist < 1.5*radius &&
motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED) {
// we're close to destination and already running VTOL motors, don't transition
poscontrol.set_state(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)) {