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

Plane: QRTL if RTL is expecting to VTOL land and close home with VTOL motors active

This commit is contained in:
Iampete1 2022-11-24 22:50:36 +00:00 committed by Andrew Tridgell
parent 7fca4607db
commit f1b62fe3c7
2 changed files with 35 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduPlane/mode.h
View File

@ -341,7 +341,7 @@ protected:
private:
// Switch to QRTL if enabled and within radius
bool switch_QRTL(bool check_loiter_target = true);
bool switch_QRTL();
};
class ModeStabilize : public Mode

41
ArduPlane/mode_rtl.cpp
View File

@ -9,6 +9,8 @@ bool ModeRTL::_enter()
#if HAL_QUADPLANE_ENABLED
plane.vtol_approach_s.approach_stage = Plane::Landing_ApproachStage::RTL;
// Quadplane specific checks
if (plane.quadplane.available()) {
// treat RTL as QLAND if we are in guided wait takeoff state, to cope
// with failsafes during GUIDED->AUTO takeoff sequence
if (plane.quadplane.guided_wait_takeoff_on_mode_enter) {
@ -16,8 +18,27 @@ bool ModeRTL::_enter()
return true;
}
// do not check if we have reached the loiter target if switching from loiter this will trigger as the nav controller has not yet proceeded the new destination
switch_QRTL(false);
// if Q_RTL_MODE is QRTL always, immediately switch to QRTL mode
if (plane.quadplane.rtl_mode == QuadPlane::RTL_MODE::QRTL_ALWAYS) {
plane.set_mode(plane.mode_qrtl, ModeReason::QRTL_INSTEAD_OF_RTL);
return true;
}
// if VTOL landing is expected and quadplane motors are active and within QRTL radius and under QRTL altitude then switch to QRTL
const bool vtol_landing = (plane.quadplane.rtl_mode == QuadPlane::RTL_MODE::SWITCH_QRTL) || (plane.quadplane.rtl_mode == QuadPlane::RTL_MODE::VTOL_APPROACH_QRTL);
if (vtol_landing && (quadplane.motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED)) {
uint16_t qrtl_radius = abs(plane.g.rtl_radius);
if (qrtl_radius == 0) {
qrtl_radius = abs(plane.aparm.loiter_radius);
}
int32_t alt_cm;
if ((plane.current_loc.get_distance(plane.next_WP_loc) < qrtl_radius) &&
plane.current_loc.get_alt_cm(Location::AltFrame::ABOVE_HOME, alt_cm) && (alt_cm < plane.quadplane.qrtl_alt*100)) {
plane.set_mode(plane.mode_qrtl, ModeReason::QRTL_INSTEAD_OF_RTL);
return true;
}
}
}
#endif
return true;
@ -60,10 +81,10 @@ void ModeRTL::update()
void ModeRTL::navigate()
{
#if HAL_QUADPLANE_ENABLED
if (plane.control_mode->mode_number() != QRTL) {
if ((plane.control_mode->mode_number() != QRTL) && plane.quadplane.available()) {
// QRTL shares this navigate function with RTL
if (plane.quadplane.available() && (plane.quadplane.rtl_mode == QuadPlane::RTL_MODE::VTOL_APPROACH_QRTL)) {
if (plane.quadplane.rtl_mode == QuadPlane::RTL_MODE::VTOL_APPROACH_QRTL) {
// VTOL approach landing
AP_Mission::Mission_Command cmd;
cmd.content.location = plane.next_WP_loc;
@ -118,24 +139,18 @@ void ModeRTL::navigate()
#if HAL_QUADPLANE_ENABLED
// Switch to QRTL if enabled and within radius
bool ModeRTL::switch_QRTL(bool check_loiter_target)
bool ModeRTL::switch_QRTL()
{
if (!plane.quadplane.available() || ((plane.quadplane.rtl_mode != QuadPlane::RTL_MODE::SWITCH_QRTL) && (plane.quadplane.rtl_mode != QuadPlane::RTL_MODE::QRTL_ALWAYS))) {
if (plane.quadplane.rtl_mode != QuadPlane::RTL_MODE::SWITCH_QRTL) {
return false;
}
// if Q_RTL_MODE is QRTL always, then immediately switch to QRTL mode
if (plane.quadplane.rtl_mode == QuadPlane::RTL_MODE::QRTL_ALWAYS) {
plane.set_mode(plane.mode_qrtl, ModeReason::QRTL_INSTEAD_OF_RTL);
return true;
}
uint16_t qrtl_radius = abs(plane.g.rtl_radius);
if (qrtl_radius == 0) {
qrtl_radius = abs(plane.aparm.loiter_radius);
}
if ( (check_loiter_target && plane.nav_controller->reached_loiter_target()) ||
if (plane.nav_controller->reached_loiter_target() ||
plane.current_loc.past_interval_finish_line(plane.prev_WP_loc, plane.next_WP_loc) ||
plane.auto_state.wp_distance < MAX(qrtl_radius, plane.quadplane.stopping_distance())) {
/*