From d53b177877490fa4166aa0d489932328609d5c6f Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Wed, 23 Nov 2016 01:36:28 -0800 Subject: [PATCH] AP_Landing: migrate aparm "LAND_" params from plane to AP_Landing --- libraries/AP_Landing/AP_Landing.cpp | 26 +++++++++++++------------- libraries/AP_Landing/AP_Landing.h | 2 +- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/libraries/AP_Landing/AP_Landing.cpp b/libraries/AP_Landing/AP_Landing.cpp index 83cf424532..74755f0ee5 100644 --- a/libraries/AP_Landing/AP_Landing.cpp +++ b/libraries/AP_Landing/AP_Landing.cpp @@ -150,8 +150,8 @@ bool AP_Landing::verify_land(const AP_SpdHgtControl::FlightStage flight_stage, c 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 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) || @@ -183,9 +183,9 @@ bool AP_Landing::verify_land(const AP_SpdHgtControl::FlightStage flight_stage, c 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); + } 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(); @@ -230,8 +230,8 @@ void AP_Landing::adjust_landing_slope_for_rangefinder_bump(AP_Vehicle::FixedWing // 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) { + if (slope_recalc_shallow_threshold <= 0 || + fabsf(correction_delta) < slope_recalc_shallow_threshold) { return; } @@ -249,7 +249,7 @@ void AP_Landing::adjust_landing_slope_for_rangefinder_bump(AP_Vehicle::FixedWing // 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 (aparm.land_slope_recalc_steep_threshold_to_abort > 0 && !has_aborted_due_to_slope_recalc) { + } 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 @@ -261,7 +261,7 @@ void AP_Landing::adjust_landing_slope_for_rangefinder_bump(AP_Vehicle::FixedWing float initial_slope_deg = degrees(atan(initial_slope)); // is projected slope too steep? - if (new_slope_deg - initial_slope_deg > aparm.land_slope_recalc_steep_threshold_to_abort) { + 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!"); @@ -310,14 +310,14 @@ void AP_Landing::setup_landing_glide_slope(const Location &prev_WP_loc, const Lo 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 = aparm.land_flare_sec * sink_rate; + float aim_height = flare_sec * sink_rate; if (aim_height <= 0) { - aim_height = aparm.land_flare_alt; + aim_height = flare_alt; } // don't allow the aim height to be too far above LAND_FLARE_ALT - if (aparm.land_flare_alt > 0 && aim_height > aparm.land_flare_alt*2) { - aim_height = aparm.land_flare_alt*2; + if (flare_alt > 0 && aim_height > flare_alt*2) { + aim_height = flare_alt*2; } // calculate slope to landing point diff --git a/libraries/AP_Landing/AP_Landing.h b/libraries/AP_Landing/AP_Landing.h index 5f88a1e28e..ede0994bf0 100644 --- a/libraries/AP_Landing/AP_Landing.h +++ b/libraries/AP_Landing/AP_Landing.h @@ -90,7 +90,7 @@ public: // are we in auto and flight mode is approach || pre-flare || final (flare) bool in_progress; - // calculated approach slope during auto-landing: ((prev_WP_loc.alt - next_WP_loc.alt)*0.01f - aparm.land_flare_sec * sink_rate) / get_distance(prev_WP_loc, next_WP_loc) + // calculated approach slope during auto-landing: ((prev_WP_loc.alt - next_WP_loc.alt)*0.01f - flare_sec * sink_rate) / get_distance(prev_WP_loc, next_WP_loc) float slope; // same as land_slope but sampled once before a rangefinder changes the slope. This should be the original mission planned slope