From 012ddbefac4d1990c6f97cab99b7d0701d2d2335 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Mon, 13 Nov 2017 10:25:46 -0700 Subject: [PATCH] AP_Landing: Support absolute altitude deepstalls --- libraries/AP_Landing/AP_Landing_Deepstall.cpp | 32 +++++++++++++++---- libraries/AP_Landing/AP_Landing_Deepstall.h | 1 + 2 files changed, 26 insertions(+), 7 deletions(-) diff --git a/libraries/AP_Landing/AP_Landing_Deepstall.cpp b/libraries/AP_Landing/AP_Landing_Deepstall.cpp index 0956bb7162..1ca4d5d9f0 100644 --- a/libraries/AP_Landing/AP_Landing_Deepstall.cpp +++ b/libraries/AP_Landing/AP_Landing_Deepstall.cpp @@ -159,6 +159,13 @@ void AP_Landing_Deepstall::do_land(const AP_Mission::Mission_Command& cmd, const // load the landing point in, the rest of path building is deferred for a better wind estimate memcpy(&landing_point, &cmd.content.location, sizeof(Location)); + + if (!landing_point.flags.relative_alt && !landing_point.flags.terrain_alt) { + approach_alt_offset = cmd.p1; + landing_point.alt += approach_alt_offset * 100; + } else { + approach_alt_offset = 0.0f; + } } // currently identical to the slope aborts @@ -190,7 +197,7 @@ bool AP_Landing_Deepstall::verify_land(const Location &prev_WP_loc, Location &ne case DEEPSTALL_STAGE_ESTIMATE_WIND: { landing.nav_controller->update_loiter(landing_point, landing.aparm.loiter_radius, landing_point.flags.loiter_ccw ? -1 : 1); - if (!landing.nav_controller->reached_loiter_target() || (fabsf(height) > DEEPSTALL_LOITER_ALT_TOLERANCE)) { + if (!landing.nav_controller->reached_loiter_target() || (fabsf(height - approach_alt_offset) > DEEPSTALL_LOITER_ALT_TOLERANCE)) { // wait until the altitude is correct before considering a breakout return false; } @@ -218,7 +225,7 @@ bool AP_Landing_Deepstall::verify_land(const Location &prev_WP_loc, Location &ne if (loiter_sum_cd < 36000) { build_approach_path(false); } - if (!verify_breakout(current_loc, arc_entry, height)) { + if (!verify_breakout(current_loc, arc_entry, height - approach_alt_offset)) { int32_t target_bearing = landing.nav_controller->target_bearing_cd(); int32_t delta = wrap_180_cd(target_bearing - last_target_bearing); if (delta > 0) { // only accumulate turns in the correct direction @@ -255,10 +262,20 @@ bool AP_Landing_Deepstall::verify_land(const Location &prev_WP_loc, Location &ne Location entry_point; landing.nav_controller->update_waypoint(arc_exit, extended_approach); - float relative_alt_D; - landing.ahrs.get_relative_position_D_home(relative_alt_D); + float height_above_target; + if (is_zero(approach_alt_offset)) { + landing.ahrs.get_relative_position_D_home(height_above_target); + height_above_target = -height_above_target; + } else { + Location position; + if (landing.ahrs.get_position(position)) { + height_above_target = (position.alt - landing_point.alt + approach_alt_offset * 100) * 1e-2f; + } else { + height_above_target = approach_alt_offset; + } + } - const float travel_distance = predict_travel_distance(landing.ahrs.wind_estimate(), -relative_alt_D, false); + const float travel_distance = predict_travel_distance(landing.ahrs.wind_estimate(), height_above_target, false); memcpy(&entry_point, &landing_point, sizeof(Location)); location_update(entry_point, target_heading_deg + 180.0, travel_distance); @@ -270,7 +287,7 @@ bool AP_Landing_Deepstall::verify_land(const Location &prev_WP_loc, Location &ne } return false; } - predict_travel_distance(landing.ahrs.wind_estimate(), -relative_alt_D, true); + predict_travel_distance(landing.ahrs.wind_estimate(), height_above_target, true); stage = DEEPSTALL_STAGE_LAND; stall_entry_time = AP_HAL::millis(); @@ -470,7 +487,8 @@ void AP_Landing_Deepstall::build_approach_path(bool use_current_heading) //extend the approach point to 1km away so that there is always a navigational target location_update(extended_approach, target_heading_deg, 1000.0); - float expected_travel_distance = predict_travel_distance(wind, landing_point.alt * 0.01f, false); + float expected_travel_distance = predict_travel_distance(wind, is_zero(approach_alt_offset) ? landing_point.alt * 0.01f : approach_alt_offset, + false); float approach_extension_m = expected_travel_distance + approach_extension; float loiter_radius_m_abs = fabsf(loiter_radius); // an approach extensions must be at least half the loiter radius, or the aircraft has a diff --git a/libraries/AP_Landing/AP_Landing_Deepstall.h b/libraries/AP_Landing/AP_Landing_Deepstall.h index b6db2aa8b4..4d365cf0cc 100644 --- a/libraries/AP_Landing/AP_Landing_Deepstall.h +++ b/libraries/AP_Landing/AP_Landing_Deepstall.h @@ -87,6 +87,7 @@ private: float crosstrack_error; // current crosstrack error float predicted_travel_distance; // distance the aircraft is perdicted to travel during deepstall bool hold_level; // locks the target yaw rate of the aircraft to 0, serves to hold the aircraft level at all times + float approach_alt_offset; // approach altitude offset //public AP_Landing interface void do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude);