Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-25 10:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: bug fix to RTL_ALT_MIN feature

Browse Source 
commited by Randy
This commit is contained in:
Jonathan Challinger 2015-08-08 15:06:40 +09:00 committed by Randy Mackay
parent 789a588b30
commit abaebac11c
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), guided_mode(Guided_TakeOff),
rtl_state(RTL_InitialClimb), rtl_state(RTL_InitialClimb),
rtl_state_complete(false), rtl_state_complete(false),
rtl_alt(0.0f),
circle_pilot_yaw_override(false), circle_pilot_yaw_override(false),
simple_cos_yaw(1.0f), simple_cos_yaw(1.0f),
simple_sin_yaw(0.0f), simple_sin_yaw(0.0f),

1
ArduCopter/Copter.h
View File

@ -325,6 +325,7 @@ private:
// RTL // RTL
RTLState rtl_state; // records state of rtl (initial climb, returning home, etc) 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 bool rtl_state_complete; // set to true if the current state is completed
float rtl_alt; // altitude the vehicle is returning at
// Circle // Circle
bool circle_pilot_yaw_override; // true if pilot is overriding yaw 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 = RTL_InitialClimb;
rtl_state_complete = false; rtl_state_complete = false;
rtl_alt = get_RTL_alt();
// initialise waypoint and spline controller // initialise waypoint and spline controller
wp_nav.wp_and_spline_init(); wp_nav.wp_and_spline_init();
@ -89,12 +90,12 @@ void Copter::rtl_climb_start()
#if AC_RALLY == ENABLED #if AC_RALLY == ENABLED
// rally_point.alt will be the altitude of the nearest rally point or the RTL_ALT. uses absolute altitudes // 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 -= 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 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); destination.z = pv_alt_above_origin(rally_point.alt);
#else #else
destination.z = pv_alt_above_origin(get_RTL_alt()); destination.z = pv_alt_above_origin(rtl_alt);
#endif #endif
// set the destination // set the destination
@ -114,13 +115,13 @@ void Copter::rtl_return_start()
// set target to above home/rally point // set target to above home/rally point
#if AC_RALLY == ENABLED #if AC_RALLY == ENABLED
// rally_point will be the nearest rally point or home. uses absolute altitudes // 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 -= 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 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); Vector3f destination = pv_location_to_vector(rally_point);
#else #else
Vector3f destination = pv_location_to_vector(ahrs.get_home()); 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 #endif
wp_nav.set_wp_destination(destination); wp_nav.set_wp_destination(destination);
@ -414,16 +415,16 @@ void Copter::rtl_land_run()
float Copter::get_RTL_alt() float Copter::get_RTL_alt()
{ {
// maximum of current altitude + climb_min and rtl altitude // 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); float ret = max(current_loc.alt + max(0, g.rtl_climb_min), g.rtl_altitude);
rtl_alt = max(rtl_alt, RTL_ALT_MIN); ret = max(ret, RTL_ALT_MIN);
#if AC_FENCE == ENABLED #if AC_FENCE == ENABLED
// ensure not above fence altitude if alt fence is enabled // ensure not above fence altitude if alt fence is enabled
if ((fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) != 0) { 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 #endif
return rtl_alt; return ret;
} }