rtl: keep rtl altitude below max altitude

land_detector: delete unused class enum
This commit is contained in:
Dennis Mannhart
2017-02-27 22:54:19 +01:00
committed by Lorenz Meier
parent b887654d69
commit 1774e01c00
3 changed files with 16 additions and 9 deletions
+9 -2
View File
@@ -84,6 +84,13 @@ RTL::on_inactive()
_rtl_state = RTL_STATE_NONE;
}
float
RTL::get_rtl_altitude()
{
return (_param_return_alt.get() < _navigator->get_land_detected()->alt_max) ? _param_return_alt.get() :
_navigator->get_land_detected()->alt_max;
}
void
RTL::on_activation()
{
@@ -101,7 +108,7 @@ RTL::on_activation()
/* if lower than return altitude, climb up first */
} else if (_navigator->get_global_position()->alt < (_navigator->get_home_position()->alt
+ _param_return_alt.get())) {
+ get_rtl_altitude())) {
_rtl_state = RTL_STATE_CLIMB;
/* otherwise go straight to return */
@@ -145,7 +152,7 @@ RTL::set_rtl_item()
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
// if we are close to home we do not climb as high, otherwise we climb to return alt
float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get();
float climb_alt = _navigator->get_home_position()->alt + get_rtl_altitude();
// we are close to home, limit climb to min
if (home_dist < _param_rtl_min_dist.get()) {
+6
View File
@@ -77,6 +77,12 @@ private:
*/
void advance_rtl();
/**
* Get rtl altitude
*/
float get_rtl_altitude();
enum RTLState {
RTL_STATE_NONE = 0,
RTL_STATE_CLIMB,