From 4f9c492f7847ebe185ab21289616e43ecdebc83b Mon Sep 17 00:00:00 2001 From: "DOMINATOR\\Eugene" Date: Sun, 18 Nov 2018 10:11:06 +0200 Subject: [PATCH] AP_Landing: yaw correction on landing --- 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, 56 insertions(+), 9 deletions(-) diff --git a/libraries/AP_Landing/AP_Landing.cpp b/libraries/AP_Landing/AP_Landing.cpp index 80d7f9f968..67bbaa701f 100644 --- a/libraries/AP_Landing/AP_Landing.cpp +++ b/libraries/AP_Landing/AP_Landing.cpp @@ -142,6 +142,15 @@ 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 6b655f8763..f1f8f06acb 100644 --- a/libraries/AP_Landing/AP_Landing.h +++ b/libraries/AP_Landing/AP_Landing.h @@ -134,6 +134,9 @@ 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; @@ -152,6 +155,7 @@ private: AP_Int8 flap_percent; AP_Int8 throttle_slewrate; AP_Int8 type; + AP_Float touchdown_altitude; // Land Type STANDARD GLIDE SLOPE @@ -165,8 +169,9 @@ 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 e9260d7ab8..f65f6362b3 100644 --- a/libraries/AP_Landing/AP_Landing_Slope.cpp +++ b/libraries/AP_Landing/AP_Landing_Slope.cpp @@ -29,6 +29,8 @@ 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); @@ -65,7 +67,26 @@ 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 @@ -90,8 +111,10 @@ 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) { @@ -107,8 +130,7 @@ 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 - AP_LandingGear *LG_inst = AP_LandingGear::instance(); - if (LG_inst != nullptr && !LG_inst->check_before_land()) { + if (landinggear != nullptr && !landinggear->check_before_land()) { type_slope_request_go_around(); gcs().send_text(MAV_SEVERITY_CRITICAL, "Landing gear was not deployed"); } @@ -136,11 +158,14 @@ 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, - land_bearing_cd*0.01f, + runway_bearing, get_distance(prev_WP_loc, current_loc) + 200); - nav_controller->update_waypoint(prev_WP_loc, land_WP_loc); + 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); + } // 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 @@ -152,6 +177,13 @@ 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"); + } } /* @@ -288,16 +320,17 @@ 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; - int32_t land_bearing_cd = get_bearing_cd(prev_WP_loc, next_WP_loc); + runway_bearing = get_bearing_cd(prev_WP_loc, next_WP_loc) * 0.01f; + type_slope_flags.touched_down = false; // now calculate our aim point, which is before the landing // point and above it Location loc = next_WP_loc; - location_update(loc, land_bearing_cd*0.01f, -flare_distance); + location_update(loc, runway_bearing, -flare_distance); loc.alt += aim_height*100; // calculate point along that slope 500m ahead - location_update(loc, land_bearing_cd*0.01f, land_projection); + location_update(loc, runway_bearing, land_projection); loc.alt -= slope * land_projection * 100; // setup the offset_cm for set_target_altitude_proportion()