Mirrors_Framework/ardupilot
1
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot.git synced 2026-10-06 19:00:27 +08:00
Code Issues Packages Projects Releases Wiki Activity

Copter: RTL params moved to class and use meters

Browse Source
This commit is contained in:
Randy Mackay
2026-01-23 11:51:54 +11:00
committed by Peter Barker
parent 767390c17c
commit 5e32cdffc7
6 changed files with 141 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ArduCopter/AP_Arming_Copter.cpp
+3 -3
View File
@@ -283,9 +283,9 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure)
check_failed(Check::PARAMETERS, display_failure, failure_template, "no rangefinder");
return false;
}
// check if RTL_ALT is higher than rangefinder's max range
if (copter.g.rtl_altitude_cm > copter.rangefinder.max_distance_orient(ROTATION_PITCH_270) * 100) {
check_failed(Check::PARAMETERS, display_failure, failure_template, "RTL_ALT (in cm) above RNGFND_MAX (in metres)");
// check if RTL_ALT_M is higher than rangefinder's max range
if (copter.mode_rtl.get_altitude_m() > copter.rangefinder.max_distance_orient(ROTATION_PITCH_270)) {
check_failed(Check::PARAMETERS, display_failure, failure_template, "RTL_ALT_M above RNGFND_MAX");
return false;
}
#else
ArduCopter/Parameters.cpp
+14 -41
View File
@@ -76,15 +76,6 @@ const AP_Param::Info Copter::var_info[] = {
GSCALAR(gcs_pid_mask, "GCS_PID_MASK", 0),
#if MODE_RTL_ENABLED
// @Param: RTL_ALT
// @DisplayName: RTL Altitude
// @Description: The minimum alt above home the vehicle will climb to before returning. If the vehicle is flying higher than this value it will return at its current altitude.
// @Units: cm
// @Range: 30 300000
// @Increment: 1
// @User: Standard
GSCALAR(rtl_altitude_cm, "RTL_ALT", RTL_ALT),
// @Param: RTL_CONE_SLOPE
// @DisplayName: RTL cone slope
// @Description: Defines a cone above home which determines maximum climb
@@ -94,33 +85,6 @@ const AP_Param::Info Copter::var_info[] = {
// @User: Standard
GSCALAR(rtl_cone_slope, "RTL_CONE_SLOPE", RTL_CONE_SLOPE_DEFAULT),
// @Param: RTL_SPEED
// @DisplayName: RTL speed
// @Description: Defines the speed in cm/s which the aircraft will attempt to maintain horizontally while flying home. If this is set to zero, WPNAV_SPEED will be used instead.
// @Units: cm/s
// @Range: 0 2000
// @Increment: 50
// @User: Standard
GSCALAR(rtl_speed_cms, "RTL_SPEED", 0),
// @Param: RTL_ALT_FINAL
// @DisplayName: RTL Final Altitude
// @Description: This is the altitude the vehicle will move to as the final stage of Returning to Launch or after completing a mission. Set to zero to land.
// @Units: cm
// @Range: 0 1000
// @Increment: 1
// @User: Standard
GSCALAR(rtl_alt_final_cm, "RTL_ALT_FINAL", RTL_ALT_FINAL),
// @Param: RTL_CLIMB_MIN
// @DisplayName: RTL minimum climb
// @Description: The vehicle will climb this many cm during the initial climb portion of the RTL
// @Units: cm
// @Range: 0 3000
// @Increment: 10
// @User: Standard
GSCALAR(rtl_climb_min_cm, "RTL_CLIMB_MIN", RTL_CLIMB_MIN_DEFAULT),
// @Param: RTL_LOIT_TIME
// @DisplayName: RTL loiter time
// @Description: Time (in milliseconds) to loiter above home before beginning final descent
@@ -1228,6 +1192,12 @@ const AP_Param::GroupInfo ParametersG2::var_info2[] = {
AP_GROUPINFO("TUNE2", 13, ParametersG2, rc_tuning2_param, 0),
#endif // AP_RC_TRANSMITTER_TUNING_ENABLED
#if MODE_RTL_ENABLED
// @Group: RTL_
// @Path: mode_rtl.cpp
AP_SUBGROUPPTR(mode_rtl_ptr, "RTL_", 14, ParametersG2, ModeRTL),
#endif
// ID 62 is reserved for the AP_SUBGROUPEXTENSION
AP_GROUPEND
@@ -1290,6 +1260,9 @@ ParametersG2::ParametersG2(void) :
#if WEATHERVANE_ENABLED
,weathervane()
#endif
#if MODE_RTL_ENABLED
,mode_rtl_ptr(&copter.mode_rtl)
#endif
{
AP_Param::setup_object_defaults(this, var_info);
AP_Param::setup_object_defaults(this, var_info2);
@@ -1299,11 +1272,6 @@ void Copter::load_parameters(void)
{
AP_Vehicle::load_parameters(g.format_version, Parameters::k_format_version);
#if MODE_RTL_ENABLED
// PARAMETER_CONVERSION - Added: Sep-2021
g.rtl_altitude_cm.convert_parameter_width(AP_PARAM_INT16);
#endif
// PARAMETER_CONVERSION - Added: Mar-2022
#if AP_FENCE_ENABLED
AP_Param::convert_class(g.k_param_fence_old, &fence, fence.var_info, 0, true);
@@ -1359,6 +1327,11 @@ void Copter::load_parameters(void)
}
#endif // HAL_GCS_ENABLED
#if MODE_RTL_ENABLED
// convert RTL parameters
copter.mode_rtl.convert_params();
#endif
// setup AP_Param frame type flags
AP_Param::set_frame_type_flags(AP_PARAM_FRAME_COPTER);
}
ArduCopter/Parameters.h
+10 -8
View File
@@ -6,6 +6,8 @@
#include "RC_Channel_Copter.h"
#include <AP_Proximity/AP_Proximity.h>
class ModeRTL;
#if MODE_FOLLOW_ENABLED
# include <AP_Follow/AP_Follow.h>
#endif
@@ -229,7 +231,7 @@ public:
//
// 135 : reserved for Solo until features merged with master
//
k_param_rtl_speed_cms = 135,
k_param_rtl_speed_cms = 135, // remove
k_param_fs_batt_curr_rtl,
k_param_rtl_cone_slope, // 137
@@ -259,10 +261,10 @@ public:
//
// 160: Navigation parameters
//
k_param_rtl_altitude_cm = 160,
k_param_rtl_altitude_cm = 160, // remove
k_param_crosstrack_gain, // deprecated - remove with next eeprom number change
k_param_rtl_loiter_time,
k_param_rtl_alt_final_cm,
k_param_rtl_alt_final_cm, // remove
k_param_tilt_comp, // 164 deprecated - remove with next eeprom number change
@@ -370,7 +372,7 @@ public:
k_param_autotune_aggressiveness, // remove
k_param_pi_vel_xy, // remove
k_param_fs_ekf_action,
k_param_rtl_climb_min_cm,
k_param_rtl_climb_min_cm, // remove
k_param_rpm_sensor_old, // remove
k_param_autotune_min_d, // remove
k_param_arming, // 252 - AP_Arming
@@ -396,11 +398,7 @@ public:
AP_Float pilot_takeoff_alt_cm;
#if MODE_RTL_ENABLED
AP_Int32 rtl_altitude_cm;
AP_Int16 rtl_speed_cms;
AP_Float rtl_cone_slope;
AP_Int16 rtl_alt_final_cm;
AP_Int16 rtl_climb_min_cm; // rtl minimum climb in cm
AP_Int32 rtl_loiter_time;
AP_Enum<ModeRTL::RTLAltType> rtl_alt_type;
#endif
@@ -701,6 +699,10 @@ public:
AP_Float rc_tuning2_min;
AP_Float rc_tuning2_max;
#endif // AP_RC_TRANSMITTER_TUNING_ENABLED
#if MODE_RTL_ENABLED
void *mode_rtl_ptr;
#endif
};
extern const AP_Param::Info var_info[];
ArduCopter/config.h
+18 -7
View File
@@ -406,20 +406,20 @@
#endif
// RTL Mode
#ifndef RTL_ALT_FINAL
# define RTL_ALT_FINAL 0 // the altitude, in cm, the vehicle will move to as the final stage of Returning to Launch. Set to zero to land.
#ifndef RTL_ALT_FINAL_M_DEFAULT
# define RTL_ALT_FINAL_M_DEFAULT 0 // the altitude, in meters, the vehicle will move to as the final stage of Returning to Launch. Set to zero to land.
#endif
#ifndef RTL_ALT
# define RTL_ALT 1500 // default alt to return to home in cm, 0 = Maintain current altitude
#ifndef RTL_ALT_M_DEFAULT
# define RTL_ALT_M_DEFAULT 15 // default alt to return to home in meters, 0 = Maintain current altitude
#endif
#ifndef RTL_ALT_MIN_M
# define RTL_ALT_MIN_M 0.30 // min height above ground for RTL (i.e 0.3 m)
# define RTL_ALT_MIN_M 0.30 // min height above ground for RTL (i.e 0.3 m)
#endif
#ifndef RTL_CLIMB_MIN_DEFAULT
# define RTL_CLIMB_MIN_DEFAULT 0 // vehicle will always climb this many cm as first stage of RTL
#ifndef RTL_CLIMB_MIN_M_DEFAULT
# define RTL_CLIMB_MIN_M_DEFAULT 0 // vehicle will always climb this many meters during the first stage of RTL
#endif
#ifndef RTL_CONE_SLOPE_DEFAULT
@@ -434,6 +434,17 @@
# define RTL_LOITER_TIME 5000 // Time (in milliseconds) to loiter above home before beginning final descent
#endif
// error if old RTL parameter default definitions are used
#ifdef RTL_ALT_FINAL
#error "RTL_ALT_FINAL definition replaced with RTL_ALT_FINAL_M_DEFAULT"
#endif
#ifdef RTL_ALT
#error "RTL_ALT definition replaced with RTL_ALT_M_DEFAULT"
#endif
#ifdef RTL_CLIMB_MIN_DEFAULT
#error "RTL_CLIMB_MIN_DEFAULT definition replaced with RTL_CLIMB_MIN_M_DEFAULT"
#endif
// AUTO Mode
#ifndef WP_YAW_BEHAVIOR_DEFAULT
# define WP_YAW_BEHAVIOR_DEFAULT WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL
ArduCopter/mode.h
+20 -2
View File
@@ -1470,8 +1470,8 @@ private:
class ModeRTL : public Mode {
public:
// inherit constructor
using Mode::Mode;
// need a constructor for parameters
ModeRTL(void);
Number mode_number() const override { return Number::RTL; }
bool init(bool ignore_checks) override;
@@ -1526,6 +1526,18 @@ public:
};
ModeRTL::RTLAltType get_alt_type() const;
// parameter accessors
float get_altitude_m() const { return altitude_m.get(); }
float get_speed_ms() const { return speed_ms.get(); }
float get_alt_final_m() const { return alt_final_m.get(); }
float get_climb_min_m() const { return climb_min_m.get(); }
// convert parameters
void convert_params();
// mode specific parameter variable table
static const struct AP_Param::GroupInfo var_info[];
protected:
const char *name() const override { return "RTL"; }
@@ -1553,6 +1565,12 @@ private:
void build_path();
void compute_return_target();
// RTL parameters
AP_Float altitude_m;
AP_Float speed_ms;
AP_Float alt_final_m;
AP_Float climb_min_m;
SubMode _state = SubMode::INITIAL_CLIMB; // records state of rtl (initial climb, returning home, etc)
bool _state_complete = false; // set to true if the current state is completed
ArduCopter/mode_rtl.cpp
+76 -8
View File
@@ -2,6 +2,74 @@
#if MODE_RTL_ENABLED
// table of user settable parameters
const AP_Param::GroupInfo ModeRTL::var_info[] = {
// @Param: ALT_M
// @DisplayName: RTL Altitude
// @Description: The minimum alt above home the vehicle will climb to before returning. If the vehicle is flying higher than this value it will return at its current altitude.
// @Units: m
// @Range: 0.30 3000
// @Increment: 0.1
// @User: Standard
AP_GROUPINFO("ALT_M", 1, ModeRTL, altitude_m, RTL_ALT_M_DEFAULT),
// @Param: ALT_FINAL_M
// @DisplayName: RTL Final Altitude
// @Description: Altitude the vehicle will move to as the final stage of Returning to Launch or after completing a mission. Set to zero to land.
// @Units: m
// @Range: 0 10
// @Increment: 0.1
// @User: Standard
AP_GROUPINFO("ALT_FINAL_M", 2, ModeRTL, alt_final_m, RTL_ALT_FINAL_M_DEFAULT),
// @Param: CLIMB_MIN_M
// @DisplayName: RTL minimum climb
// @Description: The vehicle will climb this many meters during the initial climb portion of the RTL
// @Units: m
// @Range: 0 30
// @Increment: 0.1
// @User: Standard
AP_GROUPINFO("CLIMB_MIN_M", 3, ModeRTL, climb_min_m, RTL_CLIMB_MIN_M_DEFAULT),
// @Param: SPEED_MS
// @DisplayName: RTL speed
// @Description: The speed in m/s which the aircraft will attempt to maintain horizontally while flying home. If this is set to zero, WPNAV_SPEED will be used instead.
// @Units: m/s
// @Range: 0 20
// @Increment: 0.5
// @User: Standard
AP_GROUPINFO("SPEED_MS", 4, ModeRTL, speed_ms, 0),
AP_GROUPEND
};
// constructor
ModeRTL::ModeRTL() : Mode()
{
// load parameter defaults
AP_Param::setup_object_defaults(this, var_info);
}
// convert parameters
void ModeRTL::convert_params()
{
// PARAMETER_CONVERSION - Added: Jan 2026
// return immediately if parameter conversion has already been performed
if (altitude_m.configured() || speed_ms.configured() || alt_final_m.configured() || climb_min_m.configured()) {
return;
}
static const AP_Param::ConversionInfo conversion_info[] = {
{ Parameters::k_param_rtl_altitude_cm, 0, AP_PARAM_INT32, "RTL_ALT_M" }, // RTL_ALT moved to RTL_ALT_M
{ Parameters::k_param_rtl_speed_cms, 0, AP_PARAM_INT16, "RTL_SPEED_MS" }, // RTL_SPEED moved to RTL_SPEED_MS
{ Parameters::k_param_rtl_alt_final_cm, 0, AP_PARAM_INT16, "RTL_ALT_FINAL_M" }, // RTL_ALT_FINAL moved to RTL_ALT_FINAL_M
{ Parameters::k_param_rtl_climb_min_cm, 0, AP_PARAM_INT16, "RTL_CLIMB_MIN_M" }, // RTL_CLIMB_MIN moved to RTL_CLIMB_MIN_M
};
AP_Param::convert_old_parameters_scaled(conversion_info, ARRAY_SIZE(conversion_info), 0.01, 0);
}
/*
* Init and run calls for RTL flight mode
*
@@ -18,7 +86,7 @@ bool ModeRTL::init(bool ignore_checks)
}
}
// initialise waypoint and spline controller
wp_nav->wp_and_spline_init_m(g.rtl_speed_cms * 0.01);
wp_nav->wp_and_spline_init_m(speed_ms.get());
_state = SubMode::STARTING;
_state_complete = true; // see run() method below
terrain_following_allowed = !copter.failsafe.terrain;
@@ -255,7 +323,7 @@ void ModeRTL::descent_start()
#endif
}
// rtl_descent_run - implements the final descent to the RTL_ALT
// rtl_descent_run - implements the final descent to the RTL_ALT_M
// called by rtl_run at 100hz or more
void ModeRTL::descent_run()
{
@@ -384,14 +452,14 @@ void ModeRTL::build_path()
// climb target is above our origin point at the return altitude
rtl_path.climb_target = Location(rtl_path.origin_point.lat, rtl_path.origin_point.lng, rtl_path.return_target.alt, rtl_path.return_target.get_alt_frame());
// descent target is below return target at rtl_alt_final_cm
rtl_path.descent_target = Location(rtl_path.return_target.lat, rtl_path.return_target.lng, g.rtl_alt_final_cm, Location::AltFrame::ABOVE_HOME);
// descent target is below return target at rtl_alt_final_m
rtl_path.descent_target = Location(rtl_path.return_target.lat, rtl_path.return_target.lng, alt_final_m.get() * 100, Location::AltFrame::ABOVE_HOME);
// Target altitude is passed directly to the position controller so must be relative to origin
rtl_path.descent_target.change_alt_frame(Location::AltFrame::ABOVE_ORIGIN);
// set land flag
rtl_path.land = g.rtl_alt_final_cm <= 0;
rtl_path.land = alt_final_m.get() <= 0;
}
// compute the return target - home or rally point
@@ -437,7 +505,7 @@ void ModeRTL::compute_return_target()
// subtract vertical offset from altitude.
curr_alt_m -= pos_offset_u_m;
// set return_target.alt
rtl_path.return_target.set_alt_m(MAX(curr_alt_m + MAX(0.0, g.rtl_climb_min_cm * 0.01), MAX(g.rtl_altitude_cm * 0.01, RTL_ALT_MIN_M)), Location::AltFrame::ABOVE_TERRAIN);
rtl_path.return_target.set_alt_m(MAX(curr_alt_m + MAX(0.0f, climb_min_m.get()), MAX(altitude_m.get(), RTL_ALT_MIN_M)), Location::AltFrame::ABOVE_TERRAIN);
} else {
// fallback to relative alt and warn user
alt_type = ReturnTargetAltType::RELATIVE;
@@ -479,8 +547,8 @@ void ModeRTL::compute_return_target()
float target_alt_m = MAX(rtl_path.return_target.alt, 0) * 0.01;
// increase target to maximum of current altitude + climb_min and rtl altitude
const float min_rtl_alt_m = MAX(RTL_ALT_MIN_M, curr_alt_m + MAX(0.0, g.rtl_climb_min_cm * 0.01));
target_alt_m = MAX(target_alt_m, MAX(g.rtl_altitude_cm * 0.01, min_rtl_alt_m));
const float min_rtl_alt_m = MAX(RTL_ALT_MIN_M, curr_alt_m + MAX(0.0f, climb_min_m.get()));
target_alt_m = MAX(target_alt_m, MAX(altitude_m.get(), min_rtl_alt_m));
// reduce climb if close to return target
float rtl_return_dist_m = rtl_path.return_target.get_distance(rtl_path.origin_point);