mirror of https://github.com/ArduPilot/ardupilot
Plane: QRTL: combine threshold radius for QRTL and RTL
This commit is contained in:
parent
47e2a80be5
commit
a63a32d6f3
|
@ -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;
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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;
|
||||||
|
|
Loading…
Reference in New Issue