mirror of https://github.com/ArduPilot/ardupilot
Revert "AP_Landing: yaw correction on landing"
This reverts commit 4f9c492f78
.
That commit was causing the landing issue here:
https://github.com/ArduPilot/ardupilot/issues/9993
reverting until we find the correct fix
This commit is contained in:
parent
984ce514a7
commit
9285e9df20
|
@ -142,15 +142,6 @@ const AP_Param::GroupInfo AP_Landing::var_info[] = {
|
||||||
// @Path: AP_Landing_Deepstall.cpp
|
// @Path: AP_Landing_Deepstall.cpp
|
||||||
AP_SUBGROUPINFO(deepstall, "DS_", 15, AP_Landing, AP_Landing_Deepstall),
|
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
|
AP_GROUPEND
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -135,9 +135,6 @@ private:
|
||||||
disarm_if_autoland_complete_fn_t disarm_if_autoland_complete_fn;
|
disarm_if_autoland_complete_fn_t disarm_if_autoland_complete_fn;
|
||||||
update_flight_stage_fn_t update_flight_stage_fn;
|
update_flight_stage_fn_t update_flight_stage_fn;
|
||||||
|
|
||||||
// saved bearing for yaw correction after touchdown
|
|
||||||
float runway_bearing;
|
|
||||||
|
|
||||||
// support for deepstall landings
|
// support for deepstall landings
|
||||||
AP_Landing_Deepstall deepstall;
|
AP_Landing_Deepstall deepstall;
|
||||||
|
|
||||||
|
@ -155,7 +152,6 @@ private:
|
||||||
AP_Int8 flap_percent;
|
AP_Int8 flap_percent;
|
||||||
AP_Int8 throttle_slewrate;
|
AP_Int8 throttle_slewrate;
|
||||||
AP_Int8 type;
|
AP_Int8 type;
|
||||||
AP_Float touchdown_altitude;
|
|
||||||
|
|
||||||
// Land Type STANDARD GLIDE SLOPE
|
// Land Type STANDARD GLIDE SLOPE
|
||||||
|
|
||||||
|
@ -169,9 +165,8 @@ private:
|
||||||
struct {
|
struct {
|
||||||
// once landed, post some landing statistics to the GCS
|
// once landed, post some landing statistics to the GCS
|
||||||
bool post_stats:1;
|
bool post_stats:1;
|
||||||
bool force_flare:1;
|
|
||||||
bool has_aborted_due_to_slope_recalc:1;
|
bool has_aborted_due_to_slope_recalc:1;
|
||||||
bool touched_down:1;
|
|
||||||
} type_slope_flags;
|
} type_slope_flags;
|
||||||
|
|
||||||
void type_slope_do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude);
|
void type_slope_do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude);
|
||||||
|
|
|
@ -30,8 +30,6 @@ void AP_Landing::type_slope_do_land(const AP_Mission::Mission_Command& cmd, cons
|
||||||
// once landed, post some landing statistics to the GCS
|
// once landed, post some landing statistics to the GCS
|
||||||
type_slope_flags.post_stats = false;
|
type_slope_flags.post_stats = false;
|
||||||
|
|
||||||
type_slope_flags.force_flare = false;
|
|
||||||
|
|
||||||
type_slope_stage = SLOPE_STAGE_NORMAL;
|
type_slope_stage = SLOPE_STAGE_NORMAL;
|
||||||
gcs().send_text(MAV_SEVERITY_INFO, "Landing approach start at %.1fm", (double)relative_altitude);
|
gcs().send_text(MAV_SEVERITY_INFO, "Landing approach start at %.1fm", (double)relative_altitude);
|
||||||
}
|
}
|
||||||
|
@ -68,25 +66,6 @@ bool AP_Landing::type_slope_verify_land(const Location &prev_WP_loc, Location &n
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// 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:
|
/* Set land_complete (which starts the flare) under 3 conditions:
|
||||||
1) we are within LAND_FLARE_ALT meters of the landing altitude
|
1) we are within LAND_FLARE_ALT meters of the landing altitude
|
||||||
2) we are within LAND_FLARE_SEC of the landing point vertically
|
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) ||
|
if ((on_approach_stage && below_flare_alt) ||
|
||||||
(on_approach_stage && below_flare_sec && (wp_proportion > 0.5)) ||
|
(on_approach_stage && below_flare_sec && (wp_proportion > 0.5)) ||
|
||||||
(!rangefinder_state_in_range && wp_proportion >= 1) ||
|
(!rangefinder_state_in_range && wp_proportion >= 1) ||
|
||||||
type_slope_flags.force_flare ||
|
|
||||||
probably_crashed) {
|
probably_crashed) {
|
||||||
|
|
||||||
type_slope_flags.force_flare = false;
|
|
||||||
if (type_slope_stage != SLOPE_STAGE_FINAL) {
|
if (type_slope_stage != SLOPE_STAGE_FINAL) {
|
||||||
type_slope_flags.post_stats = true;
|
type_slope_flags.post_stats = true;
|
||||||
if (is_flying && (AP_HAL::millis()-last_flying_ms) > 3000) {
|
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
|
// Check if the landing gear was deployed before landing
|
||||||
// If not - go around
|
// 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();
|
type_slope_request_go_around();
|
||||||
gcs().send_text(MAV_SEVERITY_CRITICAL, "Landing gear was not deployed");
|
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
|
prevents sudden turns if we overshoot the landing point
|
||||||
*/
|
*/
|
||||||
struct Location land_WP_loc = next_WP_loc;
|
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,
|
location_update(land_WP_loc,
|
||||||
runway_bearing,
|
land_bearing_cd*0.01f,
|
||||||
get_distance(prev_WP_loc, current_loc) + 200);
|
get_distance(prev_WP_loc, current_loc) + 200);
|
||||||
if (!type_slope_flags.touched_down) {
|
|
||||||
nav_controller->update_waypoint(prev_WP_loc, land_WP_loc);
|
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
|
// 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
|
// 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
|
// check if we should auto-disarm after a confirmed landing
|
||||||
if (type_slope_stage == SLOPE_STAGE_FINAL) {
|
if (type_slope_stage == SLOPE_STAGE_FINAL) {
|
||||||
disarm_if_autoland_complete_fn();
|
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
|
// project a point 500 meters past the landing point, passing
|
||||||
// through the landing point
|
// through the landing point
|
||||||
const float land_projection = 500;
|
const float land_projection = 500;
|
||||||
runway_bearing = get_bearing_cd(prev_WP_loc, next_WP_loc) * 0.01f;
|
int32_t land_bearing_cd = get_bearing_cd(prev_WP_loc, next_WP_loc);
|
||||||
type_slope_flags.touched_down = false;
|
|
||||||
|
|
||||||
// now calculate our aim point, which is before the landing
|
// now calculate our aim point, which is before the landing
|
||||||
// point and above it
|
// point and above it
|
||||||
Location loc = next_WP_loc;
|
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;
|
loc.alt += aim_height*100;
|
||||||
|
|
||||||
// calculate point along that slope 500m ahead
|
// 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;
|
loc.alt -= slope * land_projection * 100;
|
||||||
|
|
||||||
// setup the offset_cm for set_target_altitude_proportion()
|
// setup the offset_cm for set_target_altitude_proportion()
|
||||||
|
|
Loading…
Reference in New Issue