From a18f71b29e85a820819afa385b1e4cede7b5f880 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Sat, 8 Aug 2015 15:06:40 +0900 Subject: [PATCH] Copter: bug fix to RTL_ALT_MIN feature commited by Randy --- ArduCopter/Copter.cpp | 1 + ArduCopter/Copter.h | 1 + ArduCopter/control_rtl.cpp | 17 +++++++++-------- 3 files changed, 11 insertions(+), 8 deletions(-) diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 87a72b3630..2555e78628 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -50,6 +50,7 @@ Copter::Copter(void) : guided_mode(Guided_TakeOff), rtl_state(RTL_InitialClimb), rtl_state_complete(false), + rtl_alt(0.0f), circle_pilot_yaw_override(false), simple_cos_yaw(1.0f), simple_sin_yaw(0.0f), diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 3aad8570d2..49e692815d 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -325,6 +325,7 @@ private: // RTL RTLState rtl_state; // records state of rtl (initial climb, returning home, etc) bool rtl_state_complete; // set to true if the current state is completed + float rtl_alt; // altitude the vehicle is returning at // Circle bool circle_pilot_yaw_override; // true if pilot is overriding yaw diff --git a/ArduCopter/control_rtl.cpp b/ArduCopter/control_rtl.cpp index 52c18b300c..16b6d0a0d7 100644 --- a/ArduCopter/control_rtl.cpp +++ b/ArduCopter/control_rtl.cpp @@ -79,6 +79,7 @@ void Copter::rtl_climb_start() { rtl_state = RTL_InitialClimb; rtl_state_complete = false; + rtl_alt = get_RTL_alt(); // initialise waypoint and spline controller wp_nav.wp_and_spline_init(); @@ -89,12 +90,12 @@ void Copter::rtl_climb_start() #if AC_RALLY == ENABLED // rally_point.alt will be the altitude of the nearest rally point or the RTL_ALT. uses absolute altitudes - Location rally_point = rally.calc_best_rally_or_home_location(current_loc, get_RTL_alt()+ahrs.get_home().alt); + Location rally_point = rally.calc_best_rally_or_home_location(current_loc, rtl_alt+ahrs.get_home().alt); rally_point.alt -= ahrs.get_home().alt; // convert to altitude above home rally_point.alt = max(rally_point.alt, current_loc.alt); // ensure we do not descend before reaching home destination.z = pv_alt_above_origin(rally_point.alt); #else - destination.z = pv_alt_above_origin(get_RTL_alt()); + destination.z = pv_alt_above_origin(rtl_alt); #endif // set the destination @@ -114,13 +115,13 @@ void Copter::rtl_return_start() // set target to above home/rally point #if AC_RALLY == ENABLED // rally_point will be the nearest rally point or home. uses absolute altitudes - Location rally_point = rally.calc_best_rally_or_home_location(current_loc, get_RTL_alt()+ahrs.get_home().alt); + Location rally_point = rally.calc_best_rally_or_home_location(current_loc, rtl_alt+ahrs.get_home().alt); rally_point.alt -= ahrs.get_home().alt; // convert to altitude above home rally_point.alt = max(rally_point.alt, current_loc.alt); // ensure we do not descend before reaching home Vector3f destination = pv_location_to_vector(rally_point); #else Vector3f destination = pv_location_to_vector(ahrs.get_home()); - destination.z = pv_alt_above_origin(get_RTL_alt()); + destination.z = pv_alt_above_origin(rtl_alt)); #endif wp_nav.set_wp_destination(destination); @@ -414,16 +415,16 @@ void Copter::rtl_land_run() float Copter::get_RTL_alt() { // maximum of current altitude + climb_min and rtl altitude - float rtl_alt = max(current_loc.alt + max(0, g.rtl_climb_min), g.rtl_altitude); - rtl_alt = max(rtl_alt, RTL_ALT_MIN); + float ret = max(current_loc.alt + max(0, g.rtl_climb_min), g.rtl_altitude); + ret = max(ret, RTL_ALT_MIN); #if AC_FENCE == ENABLED // ensure not above fence altitude if alt fence is enabled if ((fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) != 0) { - rtl_alt = min(rtl_alt, fence.get_safe_alt()*100.0f); + ret = min(ret, fence.get_safe_alt()*100.0f); } #endif - return rtl_alt; + return ret; }