Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: bug fix to RTL_ALT_MIN feature 

commited by Randy
This commit is contained in:
Jonathan Challinger 2015-08-08 15:06:40 +09:00 committed by Randy Mackay
parent c29b19f768
commit a18f71b29e
3 changed files with 11 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduCopter/Copter.cpp
View File

@ -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),

1
ArduCopter/Copter.h
View File

@ -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

17
ArduCopter/control_rtl.cpp
View File

@ -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;
}