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

Plane: instantly switch from RTL to QRTL if within radius

This commit is contained in:
Iampete1 2021-01-30 20:14:19 +00:00 committed by Andrew Tridgell
parent 553425f994
commit e600df680e
2 changed files with 35 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduPlane/mode.h
View File

@ -284,6 +284,11 @@ public:
protected:
bool _enter() override;
private:
// Switch to QRTL if enabled and within radius
bool switch_QRTL();
};
class ModeStabilize : public Mode

45
ArduPlane/mode_rtl.cpp
View File

@ -7,6 +7,8 @@ bool ModeRTL::_enter()
plane.do_RTL(plane.get_RTL_altitude());
plane.rtl.done_climb = false;
switch_QRTL();
return true;
}
@ -33,22 +35,8 @@ void ModeRTL::update()
void ModeRTL::navigate()
{
uint16_t qrtl_radius = abs(plane.g.rtl_radius);
if (qrtl_radius == 0) {
qrtl_radius = abs(plane.aparm.loiter_radius);
}
if (plane.quadplane.available() && plane.quadplane.rtl_mode == 1 &&
(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())) &&
AP_HAL::millis() - plane.last_mode_change_ms > 1000) {
/*
for a quadplane in RTL mode we switch to QRTL when we
are within the maximum of the stopping distance and the
RTL_RADIUS
*/
plane.set_mode(plane.mode_qrtl, ModeReason::RTL_COMPLETE_SWITCHING_TO_VTOL_LAND_RTL);
if ((AP_HAL::millis() - plane.last_mode_change_ms > 1000) && switch_QRTL()) {
return;
}
@ -88,3 +76,30 @@ void ModeRTL::navigate()
plane.update_loiter(radius);
}
// Switch to QRTL if enabled and within radius
bool ModeRTL::switch_QRTL()
{
if (!plane.quadplane.available() || (plane.quadplane.rtl_mode != 1)) {
return false;
}
uint16_t qrtl_radius = abs(plane.g.rtl_radius);
if (qrtl_radius == 0) {
qrtl_radius = abs(plane.aparm.loiter_radius);
}
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())) {
/*
for a quadplane in RTL mode we switch to QRTL when we
are within the maximum of the stopping distance and the
RTL_RADIUS
*/
plane.set_mode(plane.mode_qrtl, ModeReason::RTL_COMPLETE_SWITCHING_TO_VTOL_LAND_RTL);
return true;
}
return false;
}