Skip to content

Commit

Permalink
Plane: Change glide slope to altitude slope
Browse files Browse the repository at this point in the history
Smooth altitude changes were always referred to as "glide slopes"
despite this not being the appropriate aviation term in the case of a
climb. A better option is "altitude slope", which encompasses both
smooth climbs and descents.

Changed all references to glide slopes, except those that specifically
refer to a single kind (like those used for landings), to the more
general term. This also includes changing the GLIDE_SLOPE_MIN and
GLIDE_SLOPE_THR parameters to ALT_SLOPE_MIN and ALT_SLOPE_MAXHGT,
respectively.
  • Loading branch information
rubenp02 committed Feb 4, 2025
1 parent ba7f9b4 commit ebea738
Show file tree
Hide file tree
Showing 7 changed files with 39 additions and 41 deletions.
16 changes: 8 additions & 8 deletions ArduPlane/Parameters.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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_max_height, "ALT_SLOPE_MAXHGT", 5.0),

// @Param: STICK_MIXING
// @DisplayName: Stick Mixing
Expand Down
8 changes: 4 additions & 4 deletions ArduPlane/Parameters.h
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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_max_height,
k_param_rudder_only,
k_param_gcs3, // 93
k_param_gcs_pid_mask,
Expand Down Expand Up @@ -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_max_height;
AP_Int8 rangefinder_landing;
AP_Int8 flap_slewrate;
#if HAL_WITH_IO_MCU
Expand Down
4 changes: 2 additions & 2 deletions ArduPlane/Plane.h
Original file line number Diff line number Diff line change
Expand Up @@ -775,7 +775,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
Expand Down Expand Up @@ -891,7 +891,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);
Expand Down
44 changes: 21 additions & 23 deletions ArduPlane/altitude.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -305,14 +304,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_max_height > 0) {
if (target_altitude.offset_cm > 0 && calc_altitude_error_cm() < -100 * g.alt_slope_max_height) {
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;
}
}
Expand Down Expand Up @@ -402,7 +402,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)
{
Expand All @@ -411,10 +411,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)
{
Expand All @@ -435,20 +434,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
Expand Down
4 changes: 2 additions & 2 deletions ArduPlane/commands.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion ArduPlane/commands_logic.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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();
}

Expand Down
2 changes: 1 addition & 1 deletion ArduPlane/mode_rtl.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down

0 comments on commit ebea738

Please sign in to comment.