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

View File

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

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); 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 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 // 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 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 #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()); plane.next_WP_loc.set_alt_cm(plane.current_loc.alt + dist_to_climb * 100UL, plane.current_loc.get_alt_frame());
return true; return true;
} else if (dist < 1.5*radius) { } else if (dist < radius) {
// Above home "cone", return at curent altitude if lower than QRTL alt // Above home "cone", return at curent altitude if lower than QRTL alt
int32_t current_alt_abs_cm; int32_t current_alt_abs_cm;
if (plane.current_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, 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; 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); 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 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();
if (dist < 1.5*radius) { if (dist < radius) {
// if close to home return at current target altitude // if close to home return at current target altitude
int32_t target_alt_abs_cm; int32_t target_alt_abs_cm;
if (plane.next_WP_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, 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 (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 #endif

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 // 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); 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)) { 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; 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.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); plane.set_mode(plane.mode_qrtl, ModeReason::QRTL_INSTEAD_OF_RTL);
return true; return true;