Browse Source

Copter: build return path in run() rather than init()

This avoids attempting to build a return path if we don't currently have
a home or origin
master
Peter Barker 6 years ago committed by Randy Mackay
parent
commit
d34c4b01f4
  1. 4
      ArduCopter/mode.h
  2. 7
      ArduCopter/mode_rtl.cpp

4
ArduCopter/mode.h

@ -1027,7 +1027,9 @@ private: @@ -1027,7 +1027,9 @@ private:
} rtl_path;
// Loiter timer - Records how long we have been in loiter
uint32_t _loiter_start_time = 0;
uint32_t _loiter_start_time;
bool path_built;
};

7
ArduCopter/mode_rtl.cpp

@ -19,7 +19,7 @@ bool Copter::ModeRTL::init(bool ignore_checks) @@ -19,7 +19,7 @@ bool Copter::ModeRTL::init(bool ignore_checks)
}
// initialise waypoint and spline controller
wp_nav->wp_and_spline_init();
build_path(!copter.failsafe.terrain);
path_built = false;
climb_start();
return true;
}
@ -39,6 +39,11 @@ void Copter::ModeRTL::restart_without_terrain() @@ -39,6 +39,11 @@ void Copter::ModeRTL::restart_without_terrain()
// should be called at 100hz or more
void Copter::ModeRTL::run(bool disarm_on_land)
{
if (!path_built) {
path_built = true;
build_path(!copter.failsafe.terrain);
}
// check if we need to move to next state
if (_state_complete) {
switch (_state) {

Loading…
Cancel
Save