diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 061d3cb5ac6598..e8fca9e1d8e408 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -88,23 +88,23 @@ const AP_Param::Info Plane::var_info[] = { // @User: Advanced GSCALAR(stab_pitch_down, "STAB_PITCH_DOWN", 2.0f), - // @Param: GLIDE_SLOPE_MIN - // @DisplayName: Glide slope minimum - // @Description: This controls the minimum altitude change for a waypoint before a glide slope will be used instead of an immediate altitude change. The default value is 15 meters, which helps to smooth out waypoint missions where small altitude changes happen near waypoints. If you don't want glide slopes to be used in missions then you can set this to zero, which will disable glide slope calculations. Otherwise you can set it to a minimum number of meters of altitude error to the destination waypoint before a glide slope will be used to change altitude. + // @Param: ALT_SLOPE_MIN + // @DisplayName: Altitude slope minimum + // @Description: This controls the minimum altitude change for a waypoint before an altitude slope will be used instead of an immediate altitude change. The default value is 15 meters, which helps to smooth out waypoint missions where small altitude changes happen near waypoints. If you don't want altitude slopes to be used in missions then you can set this to zero, which will disable altitude slope calculations. Otherwise you can set it to a minimum number of meters of altitude error to the destination waypoint before an altitude slope will be used to change altitude. // @Range: 0 1000 // @Increment: 1 // @Units: m // @User: Advanced - GSCALAR(glide_slope_min, "GLIDE_SLOPE_MIN", 15), + GSCALAR(alt_slope_min, "ALT_SLOPE_MIN", 15), - // @Param: GLIDE_SLOPE_THR - // @DisplayName: Glide slope threshold - // @Description: This controls the height above the glide slope the plane may be before rebuilding a glide slope. This is useful for smoothing out an autotakeoff + // @Param: ALT_SLOPE_MAXHGT + // @DisplayName: Altitude slope maximum height + // @Description: This controls the height above the altitude slope the plane may be before rebuilding it. This is useful for smoothing out an auto-takeoff. // @Range: 0 100 // @Increment: 1 // @Units: m // @User: Advanced - GSCALAR(glide_slope_threshold, "GLIDE_SLOPE_THR", 5.0), + GSCALAR(alt_slope_threshold, "ALT_SLOPE_THRESH", 5.0), // @Param: STICK_MIXING // @DisplayName: Stick Mixing diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 9945869250b4bd..31ea9b395550de 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -118,7 +118,7 @@ class Parameters { k_param_terrain, k_param_terrain_follow, k_param_stab_pitch_down_cd_old, // deprecated - k_param_glide_slope_min, + k_param_alt_slope_min, k_param_stab_pitch_down, k_param_terrain_lookahead, k_param_fbwa_tdrag_chan, // unused - moved to RC option @@ -134,7 +134,7 @@ class Parameters { k_param_trim_rc_at_start, // unused k_param_hil_mode_unused, // unused k_param_land_disarm_delay, // unused - moved to AP_Landing - k_param_glide_slope_threshold, + k_param_alt_slope_threshold, k_param_rudder_only, k_param_gcs3, // 93 k_param_gcs_pid_mask, @@ -467,8 +467,8 @@ class Parameters { AP_Int32 terrain_follow; AP_Int16 terrain_lookahead; #endif - AP_Int16 glide_slope_min; - AP_Float glide_slope_threshold; + AP_Int16 alt_slope_min; + AP_Float alt_slope_threshold; AP_Int8 rangefinder_landing; AP_Int8 flap_slewrate; #if HAL_WITH_IO_MCU diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 1dda8aa741f40e..8f593cb287e96e 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -781,7 +781,7 @@ class Plane : public AP_Vehicle { int32_t amsl_cm; // Altitude difference between previous and current waypoint in - // centimeters. Used for glide slope handling + // centimeters. Used for altitude slope handling int32_t offset_cm; #if AP_TERRAIN_AVAILABLE @@ -899,7 +899,7 @@ class Plane : public AP_Vehicle { void adjust_nav_pitch_throttle(void); void update_load_factor(void); void adjust_altitude_target(); - void setup_glide_slope(void); + void setup_alt_slope(void); int32_t get_RTL_altitude_cm() const; float relative_ground_altitude(bool use_rangefinder_if_available); float relative_ground_altitude(bool use_rangefinder_if_available, bool use_terrain_if_available); diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index e96e778b82fa50..a6005dddfe5602 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -50,9 +50,9 @@ void Plane::check_home_alt_change(void) } /* - setup for a gradual glide slope to the next waypoint, if appropriate + setup for a gradual altitude slope to the next waypoint, if appropriate */ -void Plane::setup_glide_slope(void) +void Plane::setup_alt_slope(void) { // establish the distance we are travelling to the next waypoint, // for calculating out rate of change of altitude @@ -81,8 +81,7 @@ void Plane::setup_glide_slope(void) break; case Mode::Number::AUTO: - - //climb without doing glide slope if option is enabled + // climb without doing slope if option is enabled if (!above_location_current(next_WP_loc) && plane.flight_option_enabled(FlightOptions::IMMEDIATE_CLIMB_IN_AUTO)) { reset_offset_altitude(); break; @@ -190,7 +189,7 @@ void Plane::set_target_altitude_current(void) // target altitude target_altitude.amsl_cm = current_loc.alt; - // reset any glide slope offset + // reset any altitude slope offset reset_offset_altitude(); #if AP_TERRAIN_AVAILABLE @@ -316,14 +315,15 @@ void Plane::set_target_altitude_proportion(const Location &loc, float proportion set_target_altitude_location(loc); proportion = constrain_float(proportion, 0.0f, 1.0f); change_target_altitude(-target_altitude.offset_cm*proportion); - //rebuild the glide slope if we are above it and supposed to be climbing - if(g.glide_slope_threshold > 0) { - if(target_altitude.offset_cm > 0 && calc_altitude_error_cm() < -100 * g.glide_slope_threshold) { + + // rebuild the altitude slope if we are above it and supposed to be climbing + if (g.alt_slope_threshold > 0) { + if (target_altitude.offset_cm > 0 && calc_altitude_error_cm() < -100 * g.alt_slope_threshold) { set_target_altitude_location(loc); set_offset_altitude_location(current_loc, loc); change_target_altitude(-target_altitude.offset_cm*proportion); - //adjust the new target offset altitude to reflect that we are partially already done - if(proportion > 0.0f) + // adjust the new target offset altitude to reflect that we are partially already done + if (proportion > 0.0f) target_altitude.offset_cm = ((float)target_altitude.offset_cm)/proportion; } } @@ -413,7 +413,7 @@ void Plane::check_fbwb_altitude(void) } /* - reset the altitude offset used for glide slopes + reset the altitude offset used for altitude slopes */ void Plane::reset_offset_altitude(void) { @@ -422,10 +422,9 @@ void Plane::reset_offset_altitude(void) /* - reset the altitude offset used for glide slopes, based on difference - between altitude at a destination and a specified start altitude. If - destination is above the starting altitude then the result is - positive. + reset the altitude offset used for slopes, based on difference between + altitude at a destination and a specified start altitude. If destination is + above the starting altitude then the result is positive. */ void Plane::set_offset_altitude_location(const Location &start_loc, const Location &destination_loc) { @@ -446,20 +445,19 @@ void Plane::set_offset_altitude_location(const Location &start_loc, const Locati #endif if (flight_stage != AP_FixedWing::FlightStage::LAND) { - // if we are within GLIDE_SLOPE_MIN meters of the target altitude - // then reset the offset to not use a glide slope. This allows for - // more accurate flight of missions where the aircraft may lose or - // gain a bit of altitude near waypoint turn points due to local - // terrain changes - if (g.glide_slope_min <= 0 || - labs(target_altitude.offset_cm)*0.01f < g.glide_slope_min) { + // if we are within ALT_SLOPE_MIN meters of the target altitude then + // reset the offset to not use an altitude slope. This allows for more + // accurate flight of missions where the aircraft may lose or gain a bit + // of altitude near waypoint turn points due to local terrain changes + if (g.alt_slope_min <= 0 || + labs(target_altitude.offset_cm)*0.01f < g.alt_slope_min) { target_altitude.offset_cm = 0; } } } /* - return true if current_loc is above loc. Used for glide slope + return true if current_loc is above loc. Used for altitude slope calculations. "above" is simple if we are not terrain following, as it just means diff --git a/ArduPlane/commands.cpp b/ArduPlane/commands.cpp index 7879defdd794df..17dc32eade5b57 100644 --- a/ArduPlane/commands.cpp +++ b/ArduPlane/commands.cpp @@ -57,7 +57,7 @@ void Plane::set_next_WP(const Location &loc) // zero out our loiter vals to watch for missed waypoints loiter_angle_reset(); - setup_glide_slope(); + setup_alt_slope(); setup_turn_angle(); // update plane.target_altitude straight away, or if we are too @@ -89,7 +89,7 @@ void Plane::set_guided_WP(const Location &loc) // ----------------------------------------------- set_target_altitude_current(); - setup_glide_slope(); + setup_alt_slope(); setup_turn_angle(); // disable crosstrack, head directly to the point diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index b5f91f89d5b421..ad30553c6f61e8 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -346,7 +346,7 @@ void Plane::do_RTL(int32_t rtl_altitude_AMSL_cm) loiter.direction = 1; } - setup_glide_slope(); + setup_alt_slope(); setup_turn_angle(); } diff --git a/ArduPlane/mode_rtl.cpp b/ArduPlane/mode_rtl.cpp index a6f2fef8927c88..3ea9a69047185f 100644 --- a/ArduPlane/mode_rtl.cpp +++ b/ArduPlane/mode_rtl.cpp @@ -62,7 +62,7 @@ void ModeRTL::update() if (!plane.rtl.done_climb && alt_threshold_reached) { plane.prev_WP_loc = plane.current_loc; - plane.setup_glide_slope(); + plane.setup_alt_slope(); plane.rtl.done_climb = true; } if (!plane.rtl.done_climb) {