mirror of https://github.com/ArduPilot/ardupilot
Plane: mode_qrtl: combine enter and init
This commit is contained in:
parent
fa908b0a1d
commit
63be15e018
|
@ -2,15 +2,6 @@
|
|||
#include "Plane.h"
|
||||
|
||||
bool ModeQRTL::_enter()
|
||||
{
|
||||
return plane.mode_qstabilize._enter();
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
handle QRTL mode
|
||||
*/
|
||||
void ModeQRTL::init()
|
||||
{
|
||||
// use do_RTL() to setup next_WP_loc
|
||||
plane.do_RTL(plane.home.alt + quadplane.qrtl_alt*100UL);
|
||||
|
@ -30,10 +21,11 @@ void ModeQRTL::init()
|
|||
int32_t to_alt;
|
||||
if (plane.current_loc.get_alt_cm(Location::AltFrame::ABSOLUTE,from_alt) && plane.next_WP_loc.get_alt_cm(Location::AltFrame::ABSOLUTE,to_alt)) {
|
||||
poscontrol.slow_descent = from_alt > to_alt;
|
||||
return;
|
||||
return true;
|
||||
}
|
||||
// defualt back to old method
|
||||
poscontrol.slow_descent = (plane.current_loc.alt > plane.next_WP_loc.alt);
|
||||
return true;
|
||||
}
|
||||
|
||||
void ModeQRTL::update()
|
||||
|
|
Loading…
Reference in New Issue