|
|
|
@ -2,15 +2,6 @@
@@ -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()
@@ -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() |
|
|
|
|