diff --git a/ArduCopter/AP_Arming_Copter.cpp b/ArduCopter/AP_Arming_Copter.cpp index 930c433ceea..390967a20d7 100644 --- a/ArduCopter/AP_Arming_Copter.cpp +++ b/ArduCopter/AP_Arming_Copter.cpp @@ -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 diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 19d5e8a048a..b87836ed24f 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -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); } diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index b21fdb49388..947a116034a 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -6,6 +6,8 @@ #include "RC_Channel_Copter.h" #include +class ModeRTL; + #if MODE_FOLLOW_ENABLED # include #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 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[]; diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 2e822e8162e..a8c19589edd 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -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 diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index b3cbcf6b25a..cd4711a0866 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -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 diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index 45f114697ad..3b1f4eee0a8 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -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);