From 75e625fd3096c6ff6362c1bff98bcbaedb01ada1 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Mon, 21 Nov 2016 16:09:17 -0800 Subject: [PATCH] AP_landing: port the rest of plane/landing.cpp --- libraries/AP_Landing/AP_Landing.cpp | 167 +++++++++++++++++++++++++++- libraries/AP_Landing/AP_Landing.h | 52 +++++++-- 2 files changed, 209 insertions(+), 10 deletions(-) diff --git a/libraries/AP_Landing/AP_Landing.cpp b/libraries/AP_Landing/AP_Landing.cpp index d6689bccc9..511a4abd2f 100644 --- a/libraries/AP_Landing/AP_Landing.cpp +++ b/libraries/AP_Landing/AP_Landing.cpp @@ -19,6 +19,7 @@ #include "AP_Landing.h" #include +#include // table of user settable parameters const AP_Param::GroupInfo AP_Landing::var_info[] = { @@ -110,7 +111,8 @@ bool AP_Landing::jump_to_landing_sequence(void) itself as that leads to discontinuities close to the landing point, which can lead to erratic pitch control */ -Location AP_Landing::setup_landing_glide_slope(const Location &prev_WP_loc, const Location &next_WP_loc) + +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); @@ -184,7 +186,17 @@ Location AP_Landing::setup_landing_glide_slope(const Location &prev_WP_loc, cons location_update(loc, land_bearing_cd*0.01f, land_projection); loc.alt -= slope * land_projection * 100; - return loc; + // 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); } void AP_Landing::check_if_need_to_abort(const AP_Vehicle::FixedWing::Rangefinder_State &rangefinder_state) @@ -215,3 +227,154 @@ void AP_Landing::check_if_need_to_abort(const AP_Vehicle::FixedWing::Rangefinder } } } + +bool AP_Landing::verify_land(AP_SpdHgtControl::FlightStage flight_stage, const Location &prev_WP_loc, Location &next_WP_loc, const Location ¤t_loc, + int32_t auto_state_takeoff_altitude_rel_cm, float height, float sink_rate, float wp_proportion, uint32_t last_flying_ms, bool is_armed, bool is_flying, 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 <= aparm.land_flare_alt); + bool below_flare_sec = (aparm.land_flare_sec > 0 && height <= sink_rate * aparm.land_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 && aparm.land_pre_flare_airspeed > 0) { + bool reached_pre_flare_alt = aparm.land_pre_flare_alt > 0 && (height <= aparm.land_pre_flare_alt); + bool reached_pre_flare_sec = aparm.land_pre_flare_sec > 0 && (height <= sink_rate * aparm.land_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, 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 (aparm.land_slope_recalc_shallow_threshold <= 0 || + fabsf(correction_delta) < aparm.land_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); + + check_if_need_to_abort(rangefinder_state); +} + + + diff --git a/libraries/AP_Landing/AP_Landing.h b/libraries/AP_Landing/AP_Landing.h index fde4794758..ec5a55fbb8 100644 --- a/libraries/AP_Landing/AP_Landing.h +++ b/libraries/AP_Landing/AP_Landing.h @@ -20,6 +20,7 @@ #include #include #include +#include /// @class AP_Landing /// @brief Class managing ArduPlane landing methods @@ -27,19 +28,54 @@ class AP_Landing { public: + FUNCTOR_TYPEDEF(set_target_altitude_proportion_fn_t, void, const Location&, float); + set_target_altitude_proportion_fn_t set_target_altitude_proportion_fn; + + FUNCTOR_TYPEDEF(constrain_target_altitude_location_fn_t, void, const Location&, const Location&); + constrain_target_altitude_location_fn_t constrain_target_altitude_location_fn; + + FUNCTOR_TYPEDEF(adjusted_altitude_cm_fn_t, int32_t); + adjusted_altitude_cm_fn_t adjusted_altitude_cm_fn; + + FUNCTOR_TYPEDEF(adjusted_relative_altitude_cm_fn_t, int32_t); + adjusted_relative_altitude_cm_fn_t adjusted_relative_altitude_cm_fn; + + FUNCTOR_TYPEDEF(disarm_if_autoland_complete_fn_t, void); + disarm_if_autoland_complete_fn_t disarm_if_autoland_complete_fn; + + FUNCTOR_TYPEDEF(update_flight_stage_fn_t, void); + update_flight_stage_fn_t update_flight_stage_fn; + // constructor - AP_Landing(AP_Mission &_mission, AP_AHRS &_ahrs, AP_SpdHgtControl *_SpdHgt_Controller, const AP_Vehicle::FixedWing &_aparm) : - mission(_mission), - ahrs(_ahrs), - SpdHgt_Controller(_SpdHgt_Controller), - aparm(_aparm) { + AP_Landing(AP_Mission &_mission, AP_AHRS &_ahrs, AP_SpdHgtControl *_SpdHgt_Controller, AP_Navigation *_nav_controller, AP_Vehicle::FixedWing &_aparm, + set_target_altitude_proportion_fn_t _set_target_altitude_proportion_fn, + constrain_target_altitude_location_fn_t _constrain_target_altitude_location_fn, + adjusted_altitude_cm_fn_t _adjusted_altitude_cm_fn, + 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): + mission(_mission) + ,ahrs(_ahrs) + ,SpdHgt_Controller(_SpdHgt_Controller) + ,nav_controller(_nav_controller) + ,aparm(_aparm) + ,set_target_altitude_proportion_fn(_set_target_altitude_proportion_fn) + ,constrain_target_altitude_location_fn(_constrain_target_altitude_location_fn) + ,adjusted_altitude_cm_fn(_adjusted_altitude_cm_fn) + ,adjusted_relative_altitude_cm_fn(_adjusted_relative_altitude_cm_fn) + ,disarm_if_autoland_complete_fn(_disarm_if_autoland_complete_fn) + ,update_flight_stage_fn(_update_flight_stage_fn) { AP_Param::setup_object_defaults(this, var_info); } bool restart_landing_sequence(); bool jump_to_landing_sequence(void); + bool verify_land(AP_SpdHgtControl::FlightStage flight_stage, const Location &prev_WP_loc, Location &next_WP_loc,const Location ¤t_loc, + int32_t auto_state_takeoff_altitude_rel_cm, float height, float sink_rate, float wp_proportion, uint32_t last_flying_ms, bool is_armed, bool is_flying, bool rangefinder_state_in_range, bool &throttle_suppressed); - Location setup_landing_glide_slope(const Location &prev_WP_loc, const Location &next_WP_loc); + void 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, float wp_distance, int32_t &target_altitude_offset_cm); + + void 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 check_if_need_to_abort(const AP_Vehicle::FixedWing::Rangefinder_State &rangefinder_state); static const struct AP_Param::GroupInfo var_info[]; @@ -80,7 +116,7 @@ private: AP_Mission &mission; AP_AHRS &ahrs; AP_SpdHgtControl *SpdHgt_Controller; + AP_Navigation *nav_controller; - const AP_Vehicle::FixedWing &aparm; - + AP_Vehicle::FixedWing &aparm; };