From 9285e9df200e884f3be366e737b9ba36a98554e4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 12 Dec 2018 15:50:28 +1100 Subject: [PATCH] Revert "AP_Landing: yaw correction on landing" This reverts commit 4f9c492f7847ebe185ab21289616e43ecdebc83b. That commit was causing the landing issue here: https://github.com/ArduPilot/ardupilot/issues/9993 reverting until we find the correct fix --- libraries/AP_Landing/AP_Landing.cpp | 9 ----- libraries/AP_Landing/AP_Landing.h | 7 +--- libraries/AP_Landing/AP_Landing_Slope.cpp | 49 ++++------------------- 3 files changed, 9 insertions(+), 56 deletions(-) diff --git a/libraries/AP_Landing/AP_Landing.cpp b/libraries/AP_Landing/AP_Landing.cpp index 67bbaa701f..80d7f9f968 100644 --- a/libraries/AP_Landing/AP_Landing.cpp +++ b/libraries/AP_Landing/AP_Landing.cpp @@ -142,15 +142,6 @@ const AP_Param::GroupInfo AP_Landing::var_info[] = { // @Path: AP_Landing_Deepstall.cpp AP_SUBGROUPINFO(deepstall, "DS_", 15, AP_Landing, AP_Landing_Deepstall), - // @Param: TD_ALT - // @DisplayName: Touch down altitude - // @Description: Altitude to trigger touchdown condition if weight on wheels sensor is not available. Disabled when 0. Recommend using an RTK GPS or rangefinder for accurate altitude - // @Units: m - // @Range: 0 5 - // @Increment: 0.01 - // @User: Advanced - AP_GROUPINFO("TD_ALT", 16, AP_Landing, touchdown_altitude, 0.0f), - AP_GROUPEND }; diff --git a/libraries/AP_Landing/AP_Landing.h b/libraries/AP_Landing/AP_Landing.h index f1f8f06acb..6b655f8763 100644 --- a/libraries/AP_Landing/AP_Landing.h +++ b/libraries/AP_Landing/AP_Landing.h @@ -134,9 +134,6 @@ private: adjusted_relative_altitude_cm_fn_t adjusted_relative_altitude_cm_fn; disarm_if_autoland_complete_fn_t disarm_if_autoland_complete_fn; update_flight_stage_fn_t update_flight_stage_fn; - - // saved bearing for yaw correction after touchdown - float runway_bearing; // support for deepstall landings AP_Landing_Deepstall deepstall; @@ -155,7 +152,6 @@ private: AP_Int8 flap_percent; AP_Int8 throttle_slewrate; AP_Int8 type; - AP_Float touchdown_altitude; // Land Type STANDARD GLIDE SLOPE @@ -169,9 +165,8 @@ private: struct { // once landed, post some landing statistics to the GCS bool post_stats:1; - bool force_flare:1; + bool has_aborted_due_to_slope_recalc:1; - bool touched_down:1; } type_slope_flags; void type_slope_do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude); diff --git a/libraries/AP_Landing/AP_Landing_Slope.cpp b/libraries/AP_Landing/AP_Landing_Slope.cpp index f65f6362b3..e9260d7ab8 100644 --- a/libraries/AP_Landing/AP_Landing_Slope.cpp +++ b/libraries/AP_Landing/AP_Landing_Slope.cpp @@ -29,8 +29,6 @@ void AP_Landing::type_slope_do_land(const AP_Mission::Mission_Command& cmd, cons // once landed, post some landing statistics to the GCS type_slope_flags.post_stats = false; - - type_slope_flags.force_flare = false; type_slope_stage = SLOPE_STAGE_NORMAL; gcs().send_text(MAV_SEVERITY_INFO, "Landing approach start at %.1fm", (double)relative_altitude); @@ -67,26 +65,7 @@ bool AP_Landing::type_slope_verify_land(const Location &prev_WP_loc, Location &n type_slope_stage = SLOPE_STAGE_APPROACH; } } - - // Check for weight-on-wheels touchdown - AP_LandingGear *landinggear = AP_LandingGear::instance(); - bool touchdown_debounced = false; - if (landinggear != nullptr) { - touchdown_debounced = (landinggear->get_wow_state() == AP_LandingGear::LG_WOW) && landinggear->get_wow_state_duration_ms() >= 500; - } - const bool touchdown_alt_override = !is_zero(touchdown_altitude) && - (height < touchdown_altitude); - if (!type_slope_flags.touched_down) { - if (touchdown_alt_override || touchdown_debounced) { - type_slope_flags.touched_down = true; // sticky flag - type_slope_flags.force_flare = true; // force flare - gcs().send_text(MAV_SEVERITY_INFO, "Touchdown encountered"); - } - } else if (type_slope_stage != SLOPE_STAGE_FINAL) { - type_slope_flags.touched_down = false; - } - /* Set land_complete (which starts the flare) under 3 conditions: 1) we are within LAND_FLARE_ALT meters of the landing altitude 2) we are within LAND_FLARE_SEC of the landing point vertically @@ -111,10 +90,8 @@ bool AP_Landing::type_slope_verify_land(const Location &prev_WP_loc, Location &n if ((on_approach_stage && below_flare_alt) || (on_approach_stage && below_flare_sec && (wp_proportion > 0.5)) || (!rangefinder_state_in_range && wp_proportion >= 1) || - type_slope_flags.force_flare || probably_crashed) { - type_slope_flags.force_flare = false; if (type_slope_stage != SLOPE_STAGE_FINAL) { type_slope_flags.post_stats = true; if (is_flying && (AP_HAL::millis()-last_flying_ms) > 3000) { @@ -130,7 +107,8 @@ bool AP_Landing::type_slope_verify_land(const Location &prev_WP_loc, Location &n // Check if the landing gear was deployed before landing // If not - go around - if (landinggear != nullptr && !landinggear->check_before_land()) { + AP_LandingGear *LG_inst = AP_LandingGear::instance(); + if (LG_inst != nullptr && !LG_inst->check_before_land()) { type_slope_request_go_around(); gcs().send_text(MAV_SEVERITY_CRITICAL, "Landing gear was not deployed"); } @@ -158,14 +136,11 @@ bool AP_Landing::type_slope_verify_land(const Location &prev_WP_loc, Location &n prevents sudden turns if we overshoot the landing point */ struct Location land_WP_loc = next_WP_loc; + int32_t land_bearing_cd = get_bearing_cd(prev_WP_loc, next_WP_loc); location_update(land_WP_loc, - runway_bearing, + land_bearing_cd*0.01f, get_distance(prev_WP_loc, current_loc) + 200); - if (!type_slope_flags.touched_down) { - nav_controller->update_waypoint(prev_WP_loc, land_WP_loc); - } else { - nav_controller->update_heading_hold(runway_bearing*100); - } + nav_controller->update_waypoint(prev_WP_loc, land_WP_loc); // once landed and stationary, post some statistics // this is done before disarm_if_autoland_complete() so that it happens on the next loop after the disarm @@ -177,13 +152,6 @@ bool AP_Landing::type_slope_verify_land(const Location &prev_WP_loc, Location &n // check if we should auto-disarm after a confirmed landing if (type_slope_stage == SLOPE_STAGE_FINAL) { disarm_if_autoland_complete_fn(); - - // Check for touchdown - if (!type_slope_flags.touched_down && - (touchdown_alt_override || touchdown_debounced)) { - type_slope_flags.touched_down = true; - gcs().send_text(MAV_SEVERITY_INFO, "Touchdown encountered"); - } } /* @@ -320,17 +288,16 @@ void AP_Landing::type_slope_setup_landing_glide_slope(const Location &prev_WP_lo // project a point 500 meters past the landing point, passing // through the landing point const float land_projection = 500; - runway_bearing = get_bearing_cd(prev_WP_loc, next_WP_loc) * 0.01f; - type_slope_flags.touched_down = false; + int32_t land_bearing_cd = get_bearing_cd(prev_WP_loc, next_WP_loc); // now calculate our aim point, which is before the landing // point and above it Location loc = next_WP_loc; - location_update(loc, runway_bearing, -flare_distance); + location_update(loc, land_bearing_cd*0.01f, -flare_distance); loc.alt += aim_height*100; // calculate point along that slope 500m ahead - location_update(loc, runway_bearing, land_projection); + location_update(loc, land_bearing_cd*0.01f, land_projection); loc.alt -= slope * land_projection * 100; // setup the offset_cm for set_target_altitude_proportion()