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

Copter: add RTL_CONE_SLOPE

This commit is contained in:
Jonathan Challinger 2016-01-05 23:00:29 -08:00 committed by Randy Mackay
parent dcd16696a2
commit 3800c66f07
4 changed files with 27 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ArduCopter/Parameters.cpp
View File

@ -127,6 +127,14 @@ const AP_Param::Info Copter::var_info[] = {
// @Increment: 1 // @Increment: 1
// @User: Standard // @User: Standard
GSCALAR(rtl_altitude, "RTL_ALT", RTL_ALT), GSCALAR(rtl_altitude, "RTL_ALT", RTL_ALT),
// @Param: RTL_CONE_SLOPE
// @DisplayName: RTL cone slope
// @Description: Defines a cone above home which determines maximum climb
// @Range: 0.5 10.0
// @Increment: .1
// @User: Standard
GSCALAR(rtl_cone_slope, "RTL_CONE_SLOPE", RTL_CONE_SLOPE),
// @Param: RTL_SPEED // @Param: RTL_SPEED
// @DisplayName: RTL speed // @DisplayName: RTL speed

4
ArduCopter/Parameters.h
View File

@ -217,7 +217,8 @@ public:
// 135 : reserved for Solo until features merged with master // 135 : reserved for Solo until features merged with master
// //
k_param_rtl_speed_cms = 135, k_param_rtl_speed_cms = 135,
k_param_fs_batt_curr_rtl, // 136 k_param_fs_batt_curr_rtl,
k_param_rtl_cone_slope, // 137
// //
// 140: Sensor parameters // 140: Sensor parameters
@ -380,6 +381,7 @@ public:
AP_Int16 rtl_altitude; AP_Int16 rtl_altitude;
AP_Int16 rtl_speed_cms; AP_Int16 rtl_speed_cms;
AP_Float rtl_cone_slope;
AP_Float sonar_gain; AP_Float sonar_gain;
AP_Int8 failsafe_battery_enabled; // battery failsafe enabled AP_Int8 failsafe_battery_enabled; // battery failsafe enabled

12
ArduCopter/config.h
View File

@ -484,10 +484,22 @@
# define RTL_ALT 1500 // default alt to return to home in cm, 0 = Maintain current altitude # define RTL_ALT 1500 // default alt to return to home in cm, 0 = Maintain current altitude
#endif #endif
#ifndef RTL_CONE_SLOPE
# define RTL_CONE_SLOPE 3.0f // slope of RTL cone. 0 = No cone
#endif
#ifndef RTL_ALT_MIN #ifndef RTL_ALT_MIN
# define RTL_ALT_MIN 200 // min height above ground for RTL (i.e 2m) # define RTL_ALT_MIN 200 // min height above ground for RTL (i.e 2m)
#endif #endif
#ifndef RTL_ABS_MIN_CLIMB
# define RTL_ABS_MIN_CLIMB 250 // absolute minimum initial climb
#endif
#ifndef RTL_MIN_CONE_SLOPE
# define RTL_MIN_CONE_SLOPE 0.5f // absolute minimum initial climb
#endif
#ifndef RTL_CLIMB_MIN_DEFAULT #ifndef RTL_CLIMB_MIN_DEFAULT
# define RTL_CLIMB_MIN_DEFAULT 0 // vehicle will always climb this many cm as first stage of RTL # define RTL_CLIMB_MIN_DEFAULT 0 // vehicle will always climb this many cm as first stage of RTL
#endif #endif

4
ArduCopter/control_rtl.cpp
View File

@ -430,6 +430,10 @@ float Copter::rtl_compute_return_alt_above_origin(float rtl_return_dist)
// maximum of current altitude + climb_min and rtl altitude // maximum of current altitude + climb_min and rtl altitude
float ret = MAX(current_loc.alt + MAX(0, g.rtl_climb_min), MAX(g.rtl_altitude, RTL_ALT_MIN)); float ret = MAX(current_loc.alt + MAX(0, g.rtl_climb_min), MAX(g.rtl_altitude, RTL_ALT_MIN));
if (g.rtl_cone_slope >= RTL_MIN_CONE_SLOPE) { // don't allow really shallow slopes
ret = MAX(current_loc.alt, MIN(ret, MAX(rtl_return_dist*g.rtl_cone_slope, current_loc.alt+RTL_ABS_MIN_CLIMB)));
}
#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) {