From d9534d9526daf999c7b2d5fbb6d3de049730c1d2 Mon Sep 17 00:00:00 2001 From: Rishabh Date: Fri, 6 Aug 2021 00:34:22 +0530 Subject: [PATCH] AC_Precland: Add new parameter to descend only when close to target --- libraries/AC_PrecLand/AC_PrecLand.cpp | 8 ++++++++ libraries/AC_PrecLand/AC_PrecLand.h | 5 ++++- 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/libraries/AC_PrecLand/AC_PrecLand.cpp b/libraries/AC_PrecLand/AC_PrecLand.cpp index 58e61d8c15..4e9a084a44 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand.cpp @@ -114,6 +114,14 @@ const AP_Param::GroupInfo AC_PrecLand::var_info[] = { // @RebootRequired: True AP_GROUPINFO("LAG", 9, AC_PrecLand, _lag, 0.02f), // 20ms is the old default buffer size (8 frames @ 400hz/2.5ms) + // @Param: XY_DIST_MAX + // @DisplayName: Precision Landing maximum distance to target before descending + // @Description: The vehicle will not start descending if the landing target is detected and it is further than this many meters away. Set 0 to always descend. + // @Range: 0 10 + // @Units: m + // @User: Advanced + AP_GROUPINFO("XY_DIST_MAX", 10, AC_PrecLand, _xy_max_dist_desc, 2.5f), + AP_GROUPEND }; diff --git a/libraries/AC_PrecLand/AC_PrecLand.h b/libraries/AC_PrecLand/AC_PrecLand.h index aa6cf6b854..0490e65218 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.h +++ b/libraries/AC_PrecLand/AC_PrecLand.h @@ -45,6 +45,9 @@ public: // returns time of last time target was seen uint32_t last_backend_los_meas_ms() const { return _last_backend_los_meas_ms; } + // vehicle has to be closer than this many cm's to the target before descending towards target + float get_max_xy_error_before_descending_cm() const { return _xy_max_dist_desc * 100.0f; } + // returns ekf outlier count uint32_t ekf_outlier_count() const { return _outlier_reject_count; } @@ -116,7 +119,7 @@ private: AP_Float _land_ofs_cm_y; // Desired landing position of the camera right of the target in vehicle body frame AP_Float _accel_noise; // accelerometer process noise AP_Vector3f _cam_offset; // Position of the camera relative to the CG - + AP_Float _xy_max_dist_desc; // Vehicle doing prec land will only descent vertically when horizontal error (in m) is below this limit uint32_t _last_update_ms; // system time in millisecond when update was last called bool _target_acquired; // true if target has been seen recently after estimator is initialized bool _estimator_initialized; // true if estimator has been initialized after few seconds of the target being detected by sensor