From 1a70e2f3536b57619c353b95359a7f8d751a4c9b Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Wed, 23 Nov 2016 17:26:06 -0800 Subject: [PATCH] AP_Landing: new param LAND_TYPE to split current behavior into AP_Landing_Slope.cpp --- libraries/AP_Landing/AP_Landing.cpp | 264 ++----------------- libraries/AP_Landing/AP_Landing.h | 20 ++ libraries/AP_Landing/AP_Landing_Slope.cpp | 295 ++++++++++++++++++++++ 3 files changed, 335 insertions(+), 244 deletions(-) create mode 100644 libraries/AP_Landing/AP_Landing_Slope.cpp diff --git a/libraries/AP_Landing/AP_Landing.cpp b/libraries/AP_Landing/AP_Landing.cpp index 9d090cc654..27ba265e9a 100644 --- a/libraries/AP_Landing/AP_Landing.cpp +++ b/libraries/AP_Landing/AP_Landing.cpp @@ -19,7 +19,6 @@ #include "AP_Landing.h" #include -#include // table of user settable parameters const AP_Param::GroupInfo AP_Landing::var_info[] = { @@ -132,6 +131,13 @@ const AP_Param::GroupInfo AP_Landing::var_info[] = { // @User: Advanced AP_GROUPINFO("FLAP_PERCNT", 13, AP_Landing, flap_percent, 0), + // @Param: TYPE + // @DisplayName: Auto-landing type + // @Description: Specifies the auto-landing type to use + // @Values: 0:Standard Glide Slope + // @User: Standard + AP_GROUPINFO("TYPE", 14, AP_Landing, type, TYPE_STANDARD_GLIDE_SLOPE), + AP_GROUPEND }; @@ -143,172 +149,20 @@ const AP_Param::GroupInfo AP_Landing::var_info[] = { bool AP_Landing::verify_land(const AP_SpdHgtControl::FlightStage flight_stage, const Location &prev_WP_loc, Location &next_WP_loc, const Location ¤t_loc, const int32_t auto_state_takeoff_altitude_rel_cm, const float height, const float sink_rate, const float wp_proportion, const uint32_t last_flying_ms, const bool is_armed, const bool is_flying, const bool rangefinder_state_in_range, bool &throttle_suppressed) { - // we don't 'verify' landing in the sense that it never completes, - // so we don't verify command completion. Instead we use this to - // adjust final landing parameters - - // when aborting a landing, mimic the verify_takeoff with steering hold. Once - // the altitude has been reached, restart the landing sequence - if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT) { - - throttle_suppressed = false; - - - complete = false; - pre_flare = false; - nav_controller->update_heading_hold(get_bearing_cd(prev_WP_loc, next_WP_loc)); - - // see if we have reached abort altitude - if (adjusted_relative_altitude_cm_fn() > auto_state_takeoff_altitude_rel_cm) { - next_WP_loc = current_loc; - mission.stop(); - if (restart_landing_sequence()) { - mission.resume(); - } - // else we're in AUTO with a stopped mission and handle_auto_mode() will set RTL - } - // make sure to return false so it leaves the mission index alone - return false; + switch (type) { + default: + case TYPE_STANDARD_GLIDE_SLOPE: + return type_slope_verify_land(flight_stage,prev_WP_loc, next_WP_loc, current_loc, + auto_state_takeoff_altitude_rel_cm, height,sink_rate, wp_proportion, last_flying_ms, is_armed, is_flying, rangefinder_state_in_range, throttle_suppressed); } - - - /* 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 - by the calculated sink rate (if LAND_FLARE_SEC != 0) - 3) we have gone past the landing point and don't have - rangefinder data (to prevent us keeping throttle on - after landing if we've had positive baro drift) - */ - - // flare check: - // 1) below flare alt/sec requires approach stage check because if sec/alt are set too - // large, and we're on a hard turn to line up for approach, we'll prematurely flare by - // skipping approach phase and the extreme roll limits will make it hard to line up with runway - // 2) passed land point and don't have an accurate AGL - // 3) probably crashed (ensures motor gets turned off) - - bool on_approach_stage = (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH || - flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE); - bool below_flare_alt = (height <= flare_alt); - bool below_flare_sec = (flare_sec > 0 && height <= sink_rate * flare_sec); - bool probably_crashed = (aparm.crash_detection_enable && fabsf(sink_rate) < 0.2f && !is_flying); - - if ((on_approach_stage && below_flare_alt) || - (on_approach_stage && below_flare_sec && (wp_proportion > 0.5)) || - (!rangefinder_state_in_range && wp_proportion >= 1) || - probably_crashed) { - - if (!complete) { - post_stats = true; - if (is_flying && (AP_HAL::millis()-last_flying_ms) > 3000) { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "Flare crash detected: speed=%.1f", (double)ahrs.get_gps().ground_speed()); - } else { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Flare %.1fm sink=%.2f speed=%.1f dist=%.1f", - (double)height, (double)sink_rate, - (double)ahrs.get_gps().ground_speed(), - (double)get_distance(current_loc, next_WP_loc)); - } - complete = true; - update_flight_stage_fn(); - } - - - if (ahrs.get_gps().ground_speed() < 3) { - // reload any airspeed or groundspeed parameters that may have - // been set for landing. We don't do this till ground - // speed drops below 3.0 m/s as otherwise we will change - // target speeds too early. - aparm.airspeed_cruise_cm.load(); - aparm.min_gndspeed_cm.load(); - aparm.throttle_cruise.load(); - } - } else if (!complete && !pre_flare && pre_flare_airspeed > 0) { - bool reached_pre_flare_alt = pre_flare_alt > 0 && (height <= pre_flare_alt); - bool reached_pre_flare_sec = pre_flare_sec > 0 && (height <= sink_rate * pre_flare_sec); - if (reached_pre_flare_alt || reached_pre_flare_sec) { - pre_flare = true; - update_flight_stage_fn(); - } - } - - /* - when landing we keep the L1 navigation waypoint 200m ahead. This - 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, - get_distance(prev_WP_loc, current_loc) + 200); - 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 - if (post_stats && !is_armed) { - post_stats = false; - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Distance from LAND point=%.2fm", (double)get_distance(current_loc, next_WP_loc)); - } - - // check if we should auto-disarm after a confirmed landing - disarm_if_autoland_complete_fn(); - - /* - we return false as a landing mission item never completes - - we stay on this waypoint unless the GCS commands us to change - mission item, reset the mission, command a go-around or finish - a land_abort procedure. - */ - return false; } void AP_Landing::adjust_landing_slope_for_rangefinder_bump(AP_Vehicle::FixedWing::Rangefinder_State &rangefinder_state, Location &prev_WP_loc, Location &next_WP_loc, const Location ¤t_loc, const float wp_distance, int32_t &target_altitude_offset_cm) { - // 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 (slope_recalc_shallow_threshold <= 0 || - fabsf(correction_delta) < slope_recalc_shallow_threshold) { - return; - } - - rangefinder_state.last_stable_correction = rangefinder_state.correction; - - float corrected_alt_m = (adjusted_altitude_cm_fn() - 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 / wp_distance; - prev_WP_loc.alt = top_of_glide_slope_alt_m*100 + next_WP_loc.alt; - - // re-calculate auto_state.land_slope with updated prev_WP_loc - setup_landing_glide_slope(prev_WP_loc, next_WP_loc, current_loc, target_altitude_offset_cm); - - if (rangefinder_state.correction >= 0) { // we're too low or object is below us - // correction positive means we're too low so we should continue on with - // the newly computed shallower slope instead of pitching/throttling up - - } else if (slope_recalc_steep_threshold_to_abort > 0 && !has_aborted_due_to_slope_recalc) { - // correction negative means we're too high and need to point down (and speed up) to re-align - // to land on target. A large negative correction means we would have to dive down a lot and will - // generating way too much speed that we can not bleed off in time. It is better to remember - // the large baro altitude offset and abort the landing to come around again with the correct altitude - // offset and "perfect" slope. - - // calculate projected slope with projected alt - float new_slope_deg = degrees(atan(slope)); - float initial_slope_deg = degrees(atan(initial_slope)); - - // is projected slope too steep? - if (new_slope_deg - initial_slope_deg > slope_recalc_steep_threshold_to_abort) { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Steep landing slope (%.0fm %.1fdeg)", - (double)rangefinder_state.correction, (double)(new_slope_deg - initial_slope_deg)); - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "aborting landing!"); - alt_offset = rangefinder_state.correction; - commanded_go_around = true; - has_aborted_due_to_slope_recalc = true; // only allow this once. - } + switch (type) { + default: + case TYPE_STANDARD_GLIDE_SLOPE: + type_slope_adjust_landing_slope_for_rangefinder_bump(rangefinder_state, prev_WP_loc, next_WP_loc, current_loc, wp_distance, target_altitude_offset_cm); } } @@ -323,89 +177,11 @@ void AP_Landing::adjust_landing_slope_for_rangefinder_bump(AP_Vehicle::FixedWing void AP_Landing::setup_landing_glide_slope(const Location &prev_WP_loc, const Location &next_WP_loc, const Location ¤t_loc, int32_t &target_altitude_offset_cm) { - float total_distance = get_distance(prev_WP_loc, next_WP_loc); - - // If someone mistakenly puts all 0's in their LAND command then total_distance - // will be calculated as 0 and cause a divide by 0 error below. Lets avoid that. - if (total_distance < 1) { - total_distance = 1; + switch (type) { + default: + case TYPE_STANDARD_GLIDE_SLOPE: + type_slope_setup_landing_glide_slope(prev_WP_loc, next_WP_loc, current_loc, target_altitude_offset_cm); } - - // height we need to sink for this WP - float sink_height = (prev_WP_loc.alt - next_WP_loc.alt)*0.01f; - - // current ground speed - float groundspeed = ahrs.groundspeed(); - if (groundspeed < 0.5f) { - groundspeed = 0.5f; - } - - // calculate time to lose the needed altitude - float sink_time = total_distance / groundspeed; - if (sink_time < 0.5f) { - sink_time = 0.5f; - } - - // find the sink rate needed for the target location - float sink_rate = sink_height / sink_time; - - // the height we aim for is the one to give us the right flare point - float aim_height = flare_sec * sink_rate; - if (aim_height <= 0) { - aim_height = flare_alt; - } - - // don't allow the aim height to be too far above LAND_FLARE_ALT - if (flare_alt > 0 && aim_height > flare_alt*2) { - aim_height = flare_alt*2; - } - - // calculate slope to landing point - bool is_first_calc = is_zero(slope); - slope = (sink_height - aim_height) / total_distance; - if (is_first_calc) { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Landing glide slope %.1f degrees", (double)degrees(atanf(slope))); - } - - - // time before landing that we will flare - float flare_time = aim_height / SpdHgt_Controller->get_land_sinkrate(); - - // distance to flare is based on ground speed, adjusted as we - // get closer. This takes into account the wind - float flare_distance = groundspeed * flare_time; - - // don't allow the flare before half way along the final leg - if (flare_distance > total_distance/2) { - flare_distance = total_distance/2; - } - - // 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); - - // 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); - loc.alt += aim_height*100; - - // calculate point along that slope 500m ahead - 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() - target_altitude_offset_cm = loc.alt - prev_WP_loc.alt; - - // calculate the proportion we are to the target - float land_proportion = location_path_proportion(current_loc, prev_WP_loc, loc); - - // now setup the glide slope for landing - set_target_altitude_proportion_fn(loc, 1.0f - land_proportion); - - // stay within the range of the start and end locations in altitude - constrain_target_altitude_location_fn(loc, prev_WP_loc); } /* diff --git a/libraries/AP_Landing/AP_Landing.h b/libraries/AP_Landing/AP_Landing.h index be0078abc2..c962f7cd65 100644 --- a/libraries/AP_Landing/AP_Landing.h +++ b/libraries/AP_Landing/AP_Landing.h @@ -28,6 +28,13 @@ class AP_Landing { public: + enum Landing_Type { + TYPE_STANDARD_GLIDE_SLOPE = 0, +// TODO: TYPE_DEEPSTALL, +// TODO: TYPE_PARACHUTE, +// TODO: TYPE_HELICAL, + }; + FUNCTOR_TYPEDEF(set_target_altitude_proportion_fn_t, void, const Location&, float); set_target_altitude_proportion_fn_t set_target_altitude_proportion_fn; @@ -140,4 +147,17 @@ private: AP_Int8 abort_throttle_enable; AP_Int8 flap_percent; AP_Int8 throttle_slewrate; + AP_Int8 type; + + + + // Land Type STANDARD GLIDE SLOPE + bool type_slope_verify_land(const AP_SpdHgtControl::FlightStage flight_stage, const Location &prev_WP_loc, Location &next_WP_loc, const Location ¤t_loc, + const int32_t auto_state_takeoff_altitude_rel_cm, const float height, const float sink_rate, const float wp_proportion, const uint32_t last_flying_ms, const bool is_armed, const bool is_flying, const bool rangefinder_state_in_range, bool &throttle_suppressed); + + void type_slope_adjust_landing_slope_for_rangefinder_bump(AP_Vehicle::FixedWing::Rangefinder_State &rangefinder_state, Location &prev_WP_loc, Location &next_WP_loc, const Location ¤t_loc, const float wp_distance, int32_t &target_altitude_offset_cm); + + void type_slope_setup_landing_glide_slope(const Location &prev_WP_loc, const Location &next_WP_loc, const Location ¤t_loc, int32_t &target_altitude_offset_cm); + void type_slope_check_if_need_to_abort(const AP_Vehicle::FixedWing::Rangefinder_State &rangefinder_state); + }; diff --git a/libraries/AP_Landing/AP_Landing_Slope.cpp b/libraries/AP_Landing/AP_Landing_Slope.cpp new file mode 100644 index 0000000000..1510a1ce86 --- /dev/null +++ b/libraries/AP_Landing/AP_Landing_Slope.cpp @@ -0,0 +1,295 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +/* + * AP_Landing_Slope.cpp - Landing logic handler for ArduPlane for STANDARD_GLIDE_SLOPE + */ + +#include "AP_Landing.h" +#include +#include + + +/* + update navigation for landing. Called when on landing approach or + final flare + */ +bool AP_Landing::type_slope_verify_land(const AP_SpdHgtControl::FlightStage flight_stage, const Location &prev_WP_loc, Location &next_WP_loc, const Location ¤t_loc, + const int32_t auto_state_takeoff_altitude_rel_cm, const float height, const float sink_rate, const float wp_proportion, const uint32_t last_flying_ms, const bool is_armed, const bool is_flying, const bool rangefinder_state_in_range, bool &throttle_suppressed) +{ + // we don't 'verify' landing in the sense that it never completes, + // so we don't verify command completion. Instead we use this to + // adjust final landing parameters + + // when aborting a landing, mimic the verify_takeoff with steering hold. Once + // the altitude has been reached, restart the landing sequence + if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT) { + + throttle_suppressed = false; + + + complete = false; + pre_flare = false; + nav_controller->update_heading_hold(get_bearing_cd(prev_WP_loc, next_WP_loc)); + + // see if we have reached abort altitude + if (adjusted_relative_altitude_cm_fn() > auto_state_takeoff_altitude_rel_cm) { + next_WP_loc = current_loc; + mission.stop(); + if (restart_landing_sequence()) { + mission.resume(); + } + // else we're in AUTO with a stopped mission and handle_auto_mode() will set RTL + } + // make sure to return false so it leaves the mission index alone + return 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 + by the calculated sink rate (if LAND_FLARE_SEC != 0) + 3) we have gone past the landing point and don't have + rangefinder data (to prevent us keeping throttle on + after landing if we've had positive baro drift) + */ + + // flare check: + // 1) below flare alt/sec requires approach stage check because if sec/alt are set too + // large, and we're on a hard turn to line up for approach, we'll prematurely flare by + // skipping approach phase and the extreme roll limits will make it hard to line up with runway + // 2) passed land point and don't have an accurate AGL + // 3) probably crashed (ensures motor gets turned off) + + bool on_approach_stage = (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH || + flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE); + bool below_flare_alt = (height <= flare_alt); + bool below_flare_sec = (flare_sec > 0 && height <= sink_rate * flare_sec); + bool probably_crashed = (aparm.crash_detection_enable && fabsf(sink_rate) < 0.2f && !is_flying); + + if ((on_approach_stage && below_flare_alt) || + (on_approach_stage && below_flare_sec && (wp_proportion > 0.5)) || + (!rangefinder_state_in_range && wp_proportion >= 1) || + probably_crashed) { + + if (!complete) { + post_stats = true; + if (is_flying && (AP_HAL::millis()-last_flying_ms) > 3000) { + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "Flare crash detected: speed=%.1f", (double)ahrs.get_gps().ground_speed()); + } else { + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Flare %.1fm sink=%.2f speed=%.1f dist=%.1f", + (double)height, (double)sink_rate, + (double)ahrs.get_gps().ground_speed(), + (double)get_distance(current_loc, next_WP_loc)); + } + complete = true; + update_flight_stage_fn(); + } + + + if (ahrs.get_gps().ground_speed() < 3) { + // reload any airspeed or groundspeed parameters that may have + // been set for landing. We don't do this till ground + // speed drops below 3.0 m/s as otherwise we will change + // target speeds too early. + aparm.airspeed_cruise_cm.load(); + aparm.min_gndspeed_cm.load(); + aparm.throttle_cruise.load(); + } + } else if (!complete && !pre_flare && pre_flare_airspeed > 0) { + bool reached_pre_flare_alt = pre_flare_alt > 0 && (height <= pre_flare_alt); + bool reached_pre_flare_sec = pre_flare_sec > 0 && (height <= sink_rate * pre_flare_sec); + if (reached_pre_flare_alt || reached_pre_flare_sec) { + pre_flare = true; + update_flight_stage_fn(); + } + } + + /* + when landing we keep the L1 navigation waypoint 200m ahead. This + 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, + get_distance(prev_WP_loc, current_loc) + 200); + 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 + if (post_stats && !is_armed) { + post_stats = false; + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Distance from LAND point=%.2fm", (double)get_distance(current_loc, next_WP_loc)); + } + + // check if we should auto-disarm after a confirmed landing + disarm_if_autoland_complete_fn(); + + /* + we return false as a landing mission item never completes + + we stay on this waypoint unless the GCS commands us to change + mission item, reset the mission, command a go-around or finish + a land_abort procedure. + */ + return false; +} + +void AP_Landing::type_slope_adjust_landing_slope_for_rangefinder_bump(AP_Vehicle::FixedWing::Rangefinder_State &rangefinder_state, Location &prev_WP_loc, Location &next_WP_loc, const Location ¤t_loc, const float wp_distance, int32_t &target_altitude_offset_cm) +{ + // 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 (slope_recalc_shallow_threshold <= 0 || + fabsf(correction_delta) < slope_recalc_shallow_threshold) { + return; + } + + rangefinder_state.last_stable_correction = rangefinder_state.correction; + + float corrected_alt_m = (adjusted_altitude_cm_fn() - 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 / wp_distance; + prev_WP_loc.alt = top_of_glide_slope_alt_m*100 + next_WP_loc.alt; + + // re-calculate auto_state.land_slope with updated prev_WP_loc + setup_landing_glide_slope(prev_WP_loc, next_WP_loc, current_loc, target_altitude_offset_cm); + + if (rangefinder_state.correction >= 0) { // we're too low or object is below us + // correction positive means we're too low so we should continue on with + // the newly computed shallower slope instead of pitching/throttling up + + } else if (slope_recalc_steep_threshold_to_abort > 0 && !has_aborted_due_to_slope_recalc) { + // correction negative means we're too high and need to point down (and speed up) to re-align + // to land on target. A large negative correction means we would have to dive down a lot and will + // generating way too much speed that we can not bleed off in time. It is better to remember + // the large baro altitude offset and abort the landing to come around again with the correct altitude + // offset and "perfect" slope. + + // calculate projected slope with projected alt + float new_slope_deg = degrees(atan(slope)); + float initial_slope_deg = degrees(atan(initial_slope)); + + // is projected slope too steep? + if (new_slope_deg - initial_slope_deg > slope_recalc_steep_threshold_to_abort) { + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Steep landing slope (%.0fm %.1fdeg)", + (double)rangefinder_state.correction, (double)(new_slope_deg - initial_slope_deg)); + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "aborting landing!"); + alt_offset = rangefinder_state.correction; + commanded_go_around = true; + has_aborted_due_to_slope_recalc = true; // only allow this once. + } + } +} + +/* + a special glide slope calculation for the landing approach + + During the land approach use a linear glide slope to a point + projected through the landing point. We don't use the landing point + itself as that leads to discontinuities close to the landing point, + which can lead to erratic pitch control + */ + +void AP_Landing::type_slope_setup_landing_glide_slope(const Location &prev_WP_loc, const Location &next_WP_loc, const Location ¤t_loc, int32_t &target_altitude_offset_cm) +{ + float total_distance = get_distance(prev_WP_loc, next_WP_loc); + + // If someone mistakenly puts all 0's in their LAND command then total_distance + // will be calculated as 0 and cause a divide by 0 error below. Lets avoid that. + if (total_distance < 1) { + total_distance = 1; + } + + // height we need to sink for this WP + float sink_height = (prev_WP_loc.alt - next_WP_loc.alt)*0.01f; + + // current ground speed + float groundspeed = ahrs.groundspeed(); + if (groundspeed < 0.5f) { + groundspeed = 0.5f; + } + + // calculate time to lose the needed altitude + float sink_time = total_distance / groundspeed; + if (sink_time < 0.5f) { + sink_time = 0.5f; + } + + // find the sink rate needed for the target location + float sink_rate = sink_height / sink_time; + + // the height we aim for is the one to give us the right flare point + float aim_height = flare_sec * sink_rate; + if (aim_height <= 0) { + aim_height = flare_alt; + } + + // don't allow the aim height to be too far above LAND_FLARE_ALT + if (flare_alt > 0 && aim_height > flare_alt*2) { + aim_height = flare_alt*2; + } + + // calculate slope to landing point + bool is_first_calc = is_zero(slope); + slope = (sink_height - aim_height) / total_distance; + if (is_first_calc) { + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Landing glide slope %.1f degrees", (double)degrees(atanf(slope))); + } + + + // time before landing that we will flare + float flare_time = aim_height / SpdHgt_Controller->get_land_sinkrate(); + + // distance to flare is based on ground speed, adjusted as we + // get closer. This takes into account the wind + float flare_distance = groundspeed * flare_time; + + // don't allow the flare before half way along the final leg + if (flare_distance > total_distance/2) { + flare_distance = total_distance/2; + } + + // 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); + + // 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); + loc.alt += aim_height*100; + + // calculate point along that slope 500m ahead + 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() + target_altitude_offset_cm = loc.alt - prev_WP_loc.alt; + + // calculate the proportion we are to the target + float land_proportion = location_path_proportion(current_loc, prev_WP_loc, loc); + + // now setup the glide slope for landing + set_target_altitude_proportion_fn(loc, 1.0f - land_proportion); + + // stay within the range of the start and end locations in altitude + constrain_target_altitude_location_fn(loc, prev_WP_loc); +}