From b2f3123b0c4d9723af4801cf2c5b57f1294303b5 Mon Sep 17 00:00:00 2001 From: Shiv Tyagi Date: Sun, 12 Jun 2022 19:42:14 +0530 Subject: [PATCH] Copter: avoid ascend beyond fence's max alt in circle mode --- ArduCopter/mode_circle.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ArduCopter/mode_circle.cpp b/ArduCopter/mode_circle.cpp index eca217bc05..7ec53c2674 100644 --- a/ArduCopter/mode_circle.cpp +++ b/ArduCopter/mode_circle.cpp @@ -90,6 +90,9 @@ void ModeCircle::run() // get pilot desired climb rate (or zero if in radio failsafe) float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); + // get avoidance adjusted climb rate + target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); + // if not armed set throttle to zero and exit immediately if (is_disarmed_or_landed()) { make_safe_ground_handling();