From b7f5aa7eabd19ea0c4921be43473abf5e605913d Mon Sep 17 00:00:00 2001 From: Tatsuya Yamaguchi Date: Sat, 9 Jan 2021 12:17:08 +0900 Subject: [PATCH] Copter: fix compilation when rtl option is disabled --- ArduCopter/avoidance_adsb.cpp | 18 ++++++++++++++---- ArduCopter/avoidance_adsb.h | 3 +++ 2 files changed, 17 insertions(+), 4 deletions(-) diff --git a/ArduCopter/avoidance_adsb.cpp b/ArduCopter/avoidance_adsb.cpp index eb2227c5e5..80d2e96744 100644 --- a/ArduCopter/avoidance_adsb.cpp +++ b/ArduCopter/avoidance_adsb.cpp @@ -144,6 +144,16 @@ void AP_Avoidance_Copter::set_mode_else_try_RTL_else_LAND(Mode::Number mode) } } +int16_t AP_Avoidance_Copter::get_altitude_minimum() const +{ +#if MODE_RTL_ENABLED == ENABLED + // do not descend if below RTL alt + return copter.g.rtl_altitude; +#else + return 0; +#endif +} + // check flight mode is avoid_adsb bool AP_Avoidance_Copter::check_flightmode(bool allow_mode_change) { @@ -179,8 +189,8 @@ bool AP_Avoidance_Copter::handle_avoidance_vertical(const AP_Avoidance::Obstacle velocity_neu.z = copter.wp_nav->get_default_speed_up(); } else { velocity_neu.z = -copter.wp_nav->get_default_speed_down(); - // do not descend if below RTL alt - if (copter.current_loc.alt < copter.g.rtl_altitude) { + // do not descend if below minimum altitude + if (copter.current_loc.alt < get_altitude_minimum()) { velocity_neu.z = 0.0f; } } @@ -238,8 +248,8 @@ bool AP_Avoidance_Copter::handle_avoidance_perpendicular(const AP_Avoidance::Obs velocity_neu.z *= copter.wp_nav->get_default_speed_up(); } else { velocity_neu.z *= copter.wp_nav->get_default_speed_down(); - // do not descend if below RTL alt - if (copter.current_loc.alt < copter.g.rtl_altitude) { + // do not descend if below minimum altitude + if (copter.current_loc.alt < get_altitude_minimum()) { velocity_neu.z = 0.0f; } } diff --git a/ArduCopter/avoidance_adsb.h b/ArduCopter/avoidance_adsb.h index 1d44406826..e156e1361e 100644 --- a/ArduCopter/avoidance_adsb.h +++ b/ArduCopter/avoidance_adsb.h @@ -19,6 +19,9 @@ private: // helper function to set modes and always succeed void set_mode_else_try_RTL_else_LAND(Mode::Number mode); + // get minimum limit altitude allowed on descend + int16_t get_altitude_minimum() const; + protected: // override avoidance handler MAV_COLLISION_ACTION handle_avoidance(const AP_Avoidance::Obstacle *obstacle, MAV_COLLISION_ACTION requested_action) override;