Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-05 15:33:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: rebuild the glide slope if we are above it and already climbing

Browse Source 
(controlled by GLIDE_SLOPE_THR)
This commit is contained in:
Michael du Breuil 2015-04-12 19:20:01 -07:00
parent 13ee34c58a
commit 4b1d71e390
3 changed files with 28 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduPlane/Parameters.h
View File

@ -119,7 +119,7 @@ public:
k_param_terrain, k_param_terrain,
k_param_terrain_follow, k_param_terrain_follow,
k_param_stab_pitch_down_cd_old, // deprecated k_param_stab_pitch_down_cd_old, // deprecated
k_param_glide_slope_threshold, k_param_glide_slope_min,
k_param_stab_pitch_down, k_param_stab_pitch_down,
k_param_terrain_lookahead, k_param_terrain_lookahead,
k_param_fbwa_tdrag_chan, k_param_fbwa_tdrag_chan,
@ -135,6 +135,7 @@ public:
k_param_trim_rc_at_start, k_param_trim_rc_at_start,
k_param_hil_mode, k_param_hil_mode,
k_param_land_disarm_delay, k_param_land_disarm_delay,
k_param_glide_slope_threshold,
// 100: Arming parameters // 100: Arming parameters
k_param_arming = 100, k_param_arming = 100,
@ -471,7 +472,8 @@ public:
AP_Int8 terrain_follow; AP_Int8 terrain_follow;
AP_Int16 terrain_lookahead; AP_Int16 terrain_lookahead;
#endif #endif
AP_Int16 glide_slope_threshold; AP_Int16 glide_slope_min;
AP_Float glide_slope_threshold;
AP_Int8 fbwa_tdrag_chan; AP_Int8 fbwa_tdrag_chan;
AP_Int8 rangefinder_landing; AP_Int8 rangefinder_landing;
AP_Int8 flap_slewrate; AP_Int8 flap_slewrate;

13
ArduPlane/Parameters.pde
View File

@ -94,13 +94,22 @@ const AP_Param::Info var_info[] PROGMEM = {
GSCALAR(stab_pitch_down, "STAB_PITCH_DOWN", 2.0f), GSCALAR(stab_pitch_down, "STAB_PITCH_DOWN", 2.0f),
// @Param: GLIDE_SLOPE_MIN // @Param: GLIDE_SLOPE_MIN
// @DisplayName: Glide slope threshold // @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. // @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.
// @Range: 0 1000 // @Range: 0 1000
// @Increment: 1 // @Increment: 1
// @Units: meters // @Units: meters
// @User: Advanced // @User: Advanced
GSCALAR(glide_slope_threshold, "GLIDE_SLOPE_MIN", 15), GSCALAR(glide_slope_min, "GLIDE_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
// @Range: 0 100
// @Increment: 1
// @Units: meters
// @User: Advanced
GSCALAR(glide_slope_threshold, "GLIDE_SLOPE_THR", 5.0),
// @Param: STICK_MIXING // @Param: STICK_MIXING
// @DisplayName: Stick Mixing // @DisplayName: Stick Mixing

15
ArduPlane/altitude.pde
View File

@ -256,6 +256,17 @@ static void set_target_altitude_proportion(const Location &loc, float proportion
set_target_altitude_location(loc); set_target_altitude_location(loc);
proportion = constrain_float(proportion, 0.0f, 1.0f); proportion = constrain_float(proportion, 0.0f, 1.0f);
change_target_altitude(-target_altitude.offset_cm*proportion); 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) {
set_target_altitude_location(loc);
set_offset_altitude_location(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)
target_altitude.offset_cm = ((float)target_altitude.offset_cm)/proportion;
}
}
} }
/* /*
@ -350,8 +361,8 @@ static void set_offset_altitude_location(const Location &loc)
// more accurate flight of missions where the aircraft may lose or // more accurate flight of missions where the aircraft may lose or
// gain a bit of altitude near waypoint turn points due to local // gain a bit of altitude near waypoint turn points due to local
// terrain changes // terrain changes
if (g.glide_slope_threshold <= 0 || if (g.glide_slope_min <= 0 ||
labs(target_altitude.offset_cm)*0.01f < g.glide_slope_threshold) { labs(target_altitude.offset_cm)*0.01f < g.glide_slope_min) {
target_altitude.offset_cm = 0; target_altitude.offset_cm = 0;
} }
} }