mirror of https://github.com/ArduPilot/ardupilot
Copter: stop disarming as part of running rtl as mission item
Fixed #6324 Note that the land-as-mission-item code is duplicated from the rtl-as-mode code but omits the disarm part, so doesn't suffer from the same bug.
This commit is contained in:
parent
8029a7f10e
commit
5b793252ed
|
@ -932,7 +932,7 @@ private:
|
||||||
|
|
||||||
bool rtl_init(bool ignore_checks);
|
bool rtl_init(bool ignore_checks);
|
||||||
void rtl_restart_without_terrain();
|
void rtl_restart_without_terrain();
|
||||||
void rtl_run();
|
void rtl_run(bool disarm_on_land=true);
|
||||||
void rtl_climb_start();
|
void rtl_climb_start();
|
||||||
void rtl_return_start();
|
void rtl_return_start();
|
||||||
void rtl_climb_return_run();
|
void rtl_climb_return_run();
|
||||||
|
@ -941,7 +941,7 @@ private:
|
||||||
void rtl_descent_start();
|
void rtl_descent_start();
|
||||||
void rtl_descent_run();
|
void rtl_descent_run();
|
||||||
void rtl_land_start();
|
void rtl_land_start();
|
||||||
void rtl_land_run();
|
void rtl_land_run(bool disarm_on_land);
|
||||||
void rtl_build_path(bool terrain_following_allowed);
|
void rtl_build_path(bool terrain_following_allowed);
|
||||||
void rtl_compute_return_target(bool terrain_following_allowed);
|
void rtl_compute_return_target(bool terrain_following_allowed);
|
||||||
bool smart_rtl_init(bool ignore_checks);
|
bool smart_rtl_init(bool ignore_checks);
|
||||||
|
|
|
@ -425,7 +425,7 @@ void Copter::auto_rtl_start()
|
||||||
void Copter::auto_rtl_run()
|
void Copter::auto_rtl_run()
|
||||||
{
|
{
|
||||||
// call regular rtl flight mode run function
|
// call regular rtl flight mode run function
|
||||||
rtl_run();
|
rtl_run(false);
|
||||||
}
|
}
|
||||||
|
|
||||||
// auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location
|
// auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location
|
||||||
|
|
|
@ -35,7 +35,7 @@ void Copter::rtl_restart_without_terrain()
|
||||||
|
|
||||||
// rtl_run - runs the return-to-launch controller
|
// rtl_run - runs the return-to-launch controller
|
||||||
// should be called at 100hz or more
|
// should be called at 100hz or more
|
||||||
void Copter::rtl_run()
|
void Copter::rtl_run(bool disarm_on_land)
|
||||||
{
|
{
|
||||||
// check if we need to move to next state
|
// check if we need to move to next state
|
||||||
if (rtl_state_complete) {
|
if (rtl_state_complete) {
|
||||||
|
@ -82,7 +82,7 @@ void Copter::rtl_run()
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case RTL_Land:
|
case RTL_Land:
|
||||||
rtl_land_run();
|
rtl_land_run(disarm_on_land);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -358,7 +358,7 @@ void Copter::rtl_land_start()
|
||||||
|
|
||||||
// rtl_returnhome_run - return home
|
// rtl_returnhome_run - return home
|
||||||
// called by rtl_run at 100hz or more
|
// called by rtl_run at 100hz or more
|
||||||
void Copter::rtl_land_run()
|
void Copter::rtl_land_run(bool disarm_on_land)
|
||||||
{
|
{
|
||||||
// if not auto armed or landing completed or motor interlock not enabled set throttle to zero and exit immediately
|
// if not auto armed or landing completed or motor interlock not enabled set throttle to zero and exit immediately
|
||||||
if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) {
|
if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) {
|
||||||
|
@ -375,7 +375,7 @@ void Copter::rtl_land_run()
|
||||||
wp_nav->init_loiter_target();
|
wp_nav->init_loiter_target();
|
||||||
|
|
||||||
// disarm when the landing detector says we've landed
|
// disarm when the landing detector says we've landed
|
||||||
if (ap.land_complete) {
|
if (ap.land_complete && disarm_on_land) {
|
||||||
init_disarm_motors();
|
init_disarm_motors();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue