From 034cd2413e28ccb4f2797e29d5e189e7d8cd9366 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Wed, 6 Jan 2016 18:30:41 -0800 Subject: [PATCH] Plane: re-calc landing glide slope to gracefully handle baro offset during long flights New param: LAND_SLOPE_RCALC @Description: This parameter is used when using a rangefinder during landing for altitude correction from baro drift (RNGFND_LANDING=1) and the altitude correction indicates your altitude is lower than the intended slope path. This value is the threshold of the correction to re-calculate the landing approach slope. Set to zero to keep the original slope all the way down and any detected baro drift will be corrected by pitching/throttling up to snap back to resume the original slope path. Otherwise, when a rangefinder altitude correction exceeds this threshold it will trigger a slope re-calculate to give a shallower slope. This also smoothes out the approach when flying over objects such as trees. Recommend a value of 2m. default value is 2 (so, enabled by default) --- ArduPlane/Parameters.cpp | 9 +++++++++ ArduPlane/Parameters.h | 2 ++ ArduPlane/Plane.h | 2 ++ ArduPlane/altitude.cpp | 1 + ArduPlane/landing.cpp | 24 ++++++++++++++++++++++++ 5 files changed, 38 insertions(+) diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 0aa1844946..12625077d9 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -240,6 +240,15 @@ const AP_Param::Info Plane::var_info[] = { // @User: User GSCALAR(level_roll_limit, "LEVEL_ROLL_LIMIT", 5), + // @Param: LAND_SLOPE_RCALC + // @DisplayName: Landing slope re-calc threshold + // @Description: This parameter is used when using a rangefinder during landing for altitude correction from baro drift (RNGFND_LANDING=1) and the altitude correction indicates your altitude is lower than the intended slope path. This value is the threshold of the correction to re-calculate the landing approach slope. Set to zero to keep the original slope all the way down and any detected baro drift will be corrected by pitching/throttling up to snap back to resume the original slope path. Otherwise, when a rangefinder altitude correction exceeds this threshold it will trigger a slope re-calculate to give a shallower slope. This also smoothes out the approach when flying over objects such as trees. Recommend a value of 2m. + // @Range: 0 5 + // @Units: meters + // @Increment: 0.5 + // @User: Advanced + GSCALAR(land_slope_recalc_shallow_threshold, "LAND_SLOPE_RCALC", 2.0f), + // @Param: LAND_PITCH_CD // @DisplayName: Landing Pitch // @Description: Used in autoland to give the minimum pitch in the final stage of landing (after the flare). This parameter can be used to ensure that the final landing attitude is appropriate for the type of undercarriage on the aircraft. Note that it is a minimum pitch only - the landing code will control pitch above this value to try to achieve the configured landing sink rate. diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index be3221eb93..2ef5495637 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -300,6 +300,7 @@ public: k_param_flight_mode5, k_param_flight_mode6, k_param_initial_mode, + k_param_land_slope_recalc_shallow_threshold, // // 220: Waypoint data @@ -472,6 +473,7 @@ public: AP_Int8 land_abort_throttle_enable; AP_Float land_pre_flare_alt; AP_Float land_pre_flare_sec; + AP_Float land_slope_recalc_shallow_threshold; AP_Int32 min_gndspeed_cm; AP_Int16 pitch_trim_cd; AP_Int16 FBWB_min_altitude_cm; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 4b006860d0..b9745e3f90 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -198,6 +198,7 @@ private: float initial_range; float correction; float initial_correction; + float last_stable_correction; uint32_t last_correction_time_ms; uint8_t in_range_count; float height_estimate; @@ -889,6 +890,7 @@ private: bool verify_land(); void disarm_if_autoland_complete(); void setup_landing_glide_slope(void); + void adjust_landing_slope_for_rangefinder_bump(void); bool jump_to_landing_sequence(void); float tecs_hgt_afe(void); void set_nav_controller(void); diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index e3f9becaa7..15c02b7ef2 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -37,6 +37,7 @@ void Plane::adjust_altitude_target() } else if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH || flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE) { setup_landing_glide_slope(); + adjust_landing_slope_for_rangefinder_bump(); } else if (reached_loiter_target()) { // once we reach a loiter target then lock to the final // altitude target diff --git a/ArduPlane/landing.cpp b/ArduPlane/landing.cpp index 75c55450ff..b2dd5ecdfc 100644 --- a/ArduPlane/landing.cpp +++ b/ArduPlane/landing.cpp @@ -160,7 +160,31 @@ void Plane::disarm_if_autoland_complete() } } +void Plane::adjust_landing_slope_for_rangefinder_bump(void) +{ +#if RANGEFINDER_ENABLED == ENABLED + // check the rangefinder correction for a large change. When found, recalculate the glide slope. This is done by + // determining the slope from your current location to the land point then following that back up to the approach + // altitude and moving the prev_wp to that location. From there + float correction_delta = fabsf(rangefinder_state.last_stable_correction) - fabsf(rangefinder_state.correction); + + if (g.land_slope_recalc_shallow_threshold <= 0 || + fabsf(correction_delta) < g.land_slope_recalc_shallow_threshold) { + return; + } + + rangefinder_state.last_stable_correction = rangefinder_state.correction; + float corrected_alt_m = (adjusted_altitude_cm() - next_WP_loc.alt)*0.01f - rangefinder_state.correction; + float total_distance_m = get_distance(prev_WP_loc, next_WP_loc); + float top_of_glide_slope_alt_m = total_distance_m * corrected_alt_m / auto_state.wp_distance; + prev_WP_loc.alt = top_of_glide_slope_alt_m*100 + next_WP_loc.alt; + + // re-calculate with updated prev_WP_loc + setup_landing_glide_slope(); + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "Slope re-calculated"); +#endif +} /* a special glide slope calculation for the landing approach