From 27f0320452da53e4c0bd48a3a4503a03ce2ec5a9 Mon Sep 17 00:00:00 2001 From: Samuel Tabor Date: Wed, 12 May 2021 12:10:57 +0100 Subject: [PATCH] AP_Landing_Slope: Log height used for flare timing. --- libraries/AP_Landing/AP_Landing.h | 2 ++ libraries/AP_Landing/AP_Landing_Slope.cpp | 8 ++++++-- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Landing/AP_Landing.h b/libraries/AP_Landing/AP_Landing.h index 2970f85787..ff7bcf99e9 100644 --- a/libraries/AP_Landing/AP_Landing.h +++ b/libraries/AP_Landing/AP_Landing.h @@ -131,6 +131,8 @@ private: // calculated approach slope during auto-landing: ((prev_WP_loc.alt - next_WP_loc.alt)*0.01f - flare_sec * sink_rate) / prev_WP_loc.get_distance(next_WP_loc) float slope; + float height_flare_log; + AP_Mission &mission; AP_AHRS &ahrs; AP_SpdHgtControl *SpdHgt_Controller; diff --git a/libraries/AP_Landing/AP_Landing_Slope.cpp b/libraries/AP_Landing/AP_Landing_Slope.cpp index e0c1d9a8d2..c75d5b8fee 100644 --- a/libraries/AP_Landing/AP_Landing_Slope.cpp +++ b/libraries/AP_Landing/AP_Landing_Slope.cpp @@ -89,6 +89,8 @@ bool AP_Landing::type_slope_verify_land(const Location &prev_WP_loc, Location &n const bool below_flare_sec = (flare_sec > 0 && height <= sink_rate * flare_sec); const bool probably_crashed = (aparm.crash_detection_enable && fabsf(sink_rate) < 0.2f && !is_flying); + height_flare_log = height; + const AP_GPS &gps = AP::gps(); if ((on_approach_stage && below_flare_alt) || @@ -405,14 +407,16 @@ void AP_Landing::type_slope_log(void) const // @Field: slope: Slope to landing point // @Field: slopeInit: Initial slope to landing point // @Field: altO: Rangefinder correction - AP::logger().Write("LAND", "TimeUS,stage,f1,f2,slope,slopeInit,altO", "QBBBfff", +// @Field: fh: Height for flare timing. + AP::logger().Write("LAND", "TimeUS,stage,f1,f2,slope,slopeInit,altO,fh", "QBBBffff", AP_HAL::micros64(), type_slope_stage, flags, type_slope_flags, (double)slope, (double)initial_slope, - (double)alt_offset); + (double)alt_offset, + (double)height_flare_log); } bool AP_Landing::type_slope_is_throttle_suppressed(void) const