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

Plane: QRTL: combine threshold radius for QRTL and RTL

This commit is contained in:
Iampete1 2023-01-25 10:35:25 +00:00 committed by Andrew Tridgell
parent 47e2a80be5
commit a63a32d6f3
3 changed files with 15 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduPlane/mode.h
View File

@ -621,6 +621,8 @@ public:
bool allows_throttle_nudging() const override;
float get_VTOL_return_radius() const;
protected:
bool _enter() override;

18
ArduPlane/mode_qrtl.cpp
View File

@ -20,12 +20,12 @@ bool ModeQRTL::_enter()
Location destination = plane.rally.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 = MAX(fabsf(plane.aparm.loiter_radius), fabsf(plane.g.rtl_radius));
const float radius = get_VTOL_return_radius();
// Climb at least to a cone around home of hight of QRTL alt and radius of 1.5*radius
// Climb at least to a cone around home of hight of QRTL alt and radius of radius
// Always climb up to at least Q_RTL_ALT_MIN, constrain Q_RTL_ALT_MIN between Q_LAND_FINAL_ALT and Q_RTL_ALT
const float min_climb = constrain_float(quadplane.qrtl_alt_min, quadplane.land_final_alt, quadplane.qrtl_alt);
const float target_alt = MAX(quadplane.qrtl_alt * (dist / MAX(1.5*radius, dist)), min_climb);
const float target_alt = MAX(quadplane.qrtl_alt * (dist / MAX(radius, dist)), min_climb);
#if AP_TERRAIN_AVAILABLE
@ -49,7 +49,7 @@ bool ModeQRTL::_enter()
plane.next_WP_loc.set_alt_cm(plane.current_loc.alt + dist_to_climb * 100UL, plane.current_loc.get_alt_frame());
return true;
} else if (dist < 1.5*radius) {
} else if (dist < radius) {
// Above home "cone", return at curent altitude if lower than QRTL alt
int32_t current_alt_abs_cm;
if (plane.current_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, current_alt_abs_cm)) {
@ -124,8 +124,8 @@ void ModeQRTL::run()
int32_t RTL_alt_abs_cm = plane.home.alt + quadplane.qrtl_alt*100UL;
Location destination = plane.rally.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 = MAX(fabsf(plane.aparm.loiter_radius), fabsf(plane.g.rtl_radius));
if (dist < 1.5*radius) {
const float radius = get_VTOL_return_radius();
if (dist < radius) {
// if close to home return at current target altitude
int32_t target_alt_abs_cm;
if (plane.next_WP_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, target_alt_abs_cm)) {
@ -207,4 +207,10 @@ bool ModeQRTL::allows_throttle_nudging() const
return (submode == SubMode::RTL) && (plane.quadplane.poscontrol.get_state() == QuadPlane::QPOS_APPROACH);
}
// Return the radius from destination at which pure VTOL flight should be used, no transition to FW
float ModeQRTL::get_VTOL_return_radius() const
{
return MAX(fabsf(plane.aparm.loiter_radius), fabsf(plane.g.rtl_radius)) * 1.5;
}
#endif

6
ArduPlane/mode_rtl.cpp
View File

@ -27,12 +27,8 @@ bool ModeRTL::_enter()
// 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) &&
if ((plane.current_loc.get_distance(plane.next_WP_loc) < plane.mode_qrtl.get_VTOL_return_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;