From 62f5f5267ed60e6a069291065f1fc9a2516f6389 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 19 Feb 2026 21:17:03 +0100 Subject: [PATCH] Land detector: Remove LNDMC_TRIG_TIME parameter (#25251) * LandDetector: remove unused LNDMC_TRIG_TIME parameter * LandDetector: remove unnecessarily complicated global set_hysteresis_factor() function * Apply suggestions from code review Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> * Apply suggestions from code review * remove param --------- Co-authored-by: Hamish Willee Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> Co-authored-by: Jacob Dahl --- .../scripts/itcm_functions_includes.ld | 1 - .../scripts/itcm_functions_includes.ld | 1 - .../scripts/itcm_functions_includes.ld | 1 - .../amovlabf410_drone_v1.15.4.params | 5 ++- docs/en/advanced_config/land_detector.md | 2 +- .../land_detector/AirshipLandDetector.h | 1 - .../land_detector/FixedwingLandDetector.h | 1 - src/modules/land_detector/LandDetector.cpp | 14 --------- src/modules/land_detector/LandDetector.h | 2 -- .../land_detector/MulticopterLandDetector.cpp | 31 +++++++++++++------ .../land_detector/MulticopterLandDetector.h | 10 ++---- src/modules/land_detector/RoverLandDetector.h | 1 - .../land_detector/land_detector_params_mc.c | 16 ---------- 13 files changed, 26 insertions(+), 60 deletions(-) diff --git a/boards/nxp/mr-tropic/nuttx-config/scripts/itcm_functions_includes.ld b/boards/nxp/mr-tropic/nuttx-config/scripts/itcm_functions_includes.ld index 92a796498b..dd9dc4edeb 100644 --- a/boards/nxp/mr-tropic/nuttx-config/scripts/itcm_functions_includes.ld +++ b/boards/nxp/mr-tropic/nuttx-config/scripts/itcm_functions_includes.ld @@ -269,7 +269,6 @@ *(.text.udp_pollsetup) *(.text._ZL14timer_callbackPv) *(.text._ZN3Ekf4fuseERKN6matrix6VectorIfLj24EEEf) -*(.text._ZN13land_detector23MulticopterLandDetector22_set_hysteresis_factorEi) *(.text.nxsem_wait_irq) *(.text._ZN20MavlinkCommandSender4lockEv) *(.text.MEM_LongCopyEnd) diff --git a/boards/nxp/tropic-community/nuttx-config/scripts/itcm_functions_includes.ld b/boards/nxp/tropic-community/nuttx-config/scripts/itcm_functions_includes.ld index 92a796498b..dd9dc4edeb 100644 --- a/boards/nxp/tropic-community/nuttx-config/scripts/itcm_functions_includes.ld +++ b/boards/nxp/tropic-community/nuttx-config/scripts/itcm_functions_includes.ld @@ -269,7 +269,6 @@ *(.text.udp_pollsetup) *(.text._ZL14timer_callbackPv) *(.text._ZN3Ekf4fuseERKN6matrix6VectorIfLj24EEEf) -*(.text._ZN13land_detector23MulticopterLandDetector22_set_hysteresis_factorEi) *(.text.nxsem_wait_irq) *(.text._ZN20MavlinkCommandSender4lockEv) *(.text.MEM_LongCopyEnd) diff --git a/boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld b/boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld index 98bbccdf0b..d8394d443a 100644 --- a/boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld +++ b/boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld @@ -273,7 +273,6 @@ *(.text.udp_pollsetup) *(.text._ZL14timer_callbackPv) *(.text._ZN3Ekf4fuseERKN6matrix6VectorIfLj24EEEf) -*(.text._ZN13land_detector23MulticopterLandDetector22_set_hysteresis_factorEi) *(.text.nxsem_wait_irq) *(.text._ZN20MavlinkCommandSender4lockEv) *(.text.MEM_LongCopyEnd) diff --git a/docs/assets/airframes/multicopter/amovlab_f410/amovlabf410_drone_v1.15.4.params b/docs/assets/airframes/multicopter/amovlab_f410/amovlabf410_drone_v1.15.4.params index 4cea5fc9d4..0b19f32c09 100644 --- a/docs/assets/airframes/multicopter/amovlab_f410/amovlabf410_drone_v1.15.4.params +++ b/docs/assets/airframes/multicopter/amovlab_f410/amovlabf410_drone_v1.15.4.params @@ -1,8 +1,8 @@ # Onboard parameters for Vehicle 1 # # Stack: PX4 Pro -# Vehicle: Multi-Rotor -# Version: 1.15.4 +# Vehicle: Amovlab F410 +# Version: 1.15.4 # Git Revision: 99c40407ff000000 # # Vehicle-Id Component-Id Name Value Type @@ -546,7 +546,6 @@ 1 1 IMU_INTEG_RATE 200 6 1 1 LNDMC_ALT_GND 2.000000000000000000 9 1 1 LNDMC_ROT_MAX 20.000000000000000000 9 -1 1 LNDMC_TRIG_TIME 1.000000000000000000 9 1 1 LNDMC_XY_VEL_MAX 1.500000000000000000 9 1 1 LNDMC_Z_VEL_MAX 0.250000000000000000 9 1 1 LND_FLIGHT_T_HI 5 6 diff --git a/docs/en/advanced_config/land_detector.md b/docs/en/advanced_config/land_detector.md index dcaf4f45c5..d50f5d8417 100644 --- a/docs/en/advanced_config/land_detector.md +++ b/docs/en/advanced_config/land_detector.md @@ -38,7 +38,7 @@ In order to detect landing, the multicopter first has to go through three differ If a condition cannot be reached because of missing sensors, then the condition is true by default. For instance, in [Acro mode](../flight_modes_mc/acro.md) and no sensor is active except for the gyro sensor, then the detection solely relies on thrust output and time. -In order to proceed to the next state, each condition has to be true for a third of the configured total land detector trigger time [LNDMC_TRIG_TIME](../advanced_config/parameter_reference.md#LNDMC_TRIG_TIME). +In order to proceed to the next state, each condition has to be true for 300ms. If the vehicle is equipped with a distance sensor, but the distance to ground is currently not measurable (usually because it is too large), the trigger time is increased by a factor of 3. If one condition fails, the land detector drops out of the current state immediately. diff --git a/src/modules/land_detector/AirshipLandDetector.h b/src/modules/land_detector/AirshipLandDetector.h index abbd693ae8..24475866ad 100644 --- a/src/modules/land_detector/AirshipLandDetector.h +++ b/src/modules/land_detector/AirshipLandDetector.h @@ -54,7 +54,6 @@ public: protected: bool _get_ground_contact_state() override; bool _get_landed_state() override; - void _set_hysteresis_factor(const int factor) override {}; }; } // namespace land_detector diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index 2454b83f43..01b1361a13 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -63,7 +63,6 @@ public: protected: bool _get_landed_state() override; - void _set_hysteresis_factor(const int factor) override {}; private: uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)}; diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index 2050c6d2e5..dabeb9145c 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -122,20 +122,6 @@ void LandDetector::Run() _update_topics(); - if (!_dist_bottom_is_observable) { - // we consider the distance to the ground observable if the system is using a range sensor - _dist_bottom_is_observable = _vehicle_local_position.dist_bottom_sensor_bitfield & - vehicle_local_position_s::DIST_BOTTOM_SENSOR_RANGE; - } - - // Increase land detection time if not close to ground - if (_dist_bottom_is_observable && !_vehicle_local_position.dist_bottom_valid) { - _set_hysteresis_factor(3); - - } else { - _set_hysteresis_factor(1); - } - const hrt_abstime now_us = hrt_absolute_time(); _freefall_hysteresis.set_state_and_update(_get_freefall_state(), now_us); diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index 754df0068b..61375d9560 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -142,7 +142,6 @@ protected: virtual bool _get_vertical_movement() { return false; } virtual bool _get_rotational_movement() { return false; } virtual bool _get_close_to_ground_or_skipped_check() { return false; } - virtual void _set_hysteresis_factor(const int factor) = 0; systemlib::Hysteresis _freefall_hysteresis{false}; systemlib::Hysteresis _landed_hysteresis{true}; @@ -158,7 +157,6 @@ protected: bool _armed{false}; bool _previous_armed_state{false}; ///< stores the previous actuator_armed.armed state - bool _dist_bottom_is_observable{false}; private: void Run() override; diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 78e153ee89..3e84d73b0f 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -44,7 +44,7 @@ *State 2 (=maybe_landed): *maybe_landed can only occur if the internal ground_contact hysteresis state is true. maybe_landed criteria requires to have no motion in x and y, *no rotation and a thrust below 0.1 of the thrust_range (thrust_hover - thrust_min). In addition, the mc_pos_control turns off the thrust_sp in - *body frame along x and y which helps to detect maybe_landed. The criteria for maybe_landed needs to be true for (LNDMC_TRIG_TIME / 3) seconds. + *body frame along x and y which helps to detect maybe_landed. The criteria for maybe_landed needs to be true for 1/3 seconds. * *State 3 (=landed) *landed can only be detected if maybe_landed is true for LAND_DETECTOR_TRIGGER_TIME_US. No farther criteria is tested, but the mc_pos_control goes into @@ -82,6 +82,7 @@ MulticopterLandDetector::MulticopterLandDetector() _paramHandle.landSpeed = param_find("MPC_LAND_SPEED"); _paramHandle.crawlSpeed = param_find("MPC_LAND_CRWL"); _minimum_thrust_8s_hysteresis.set_hysteresis_time_from(false, 8_s); + _freefall_hysteresis.set_hysteresis_time_from(false, 300_ms); } void MulticopterLandDetector::_update_topics() @@ -114,6 +115,24 @@ void MulticopterLandDetector::_update_topics() if (_takeoff_status_sub.update(&takeoff_status)) { _takeoff_state = takeoff_status.takeoff_state; } + + // Update distance sensor observability + if (!_dist_bottom_is_observable) { + // we consider the distance to the ground observable if the system is using a range sensor + _dist_bottom_is_observable = + _vehicle_local_position.dist_bottom_sensor_bitfield & vehicle_local_position_s::DIST_BOTTOM_SENSOR_RANGE; + } + + // Increase land detection time if not close to ground + hrt_abstime land_detection_time = 1_s; + + if (_dist_bottom_is_observable && !_vehicle_local_position.dist_bottom_valid) { + land_detection_time = 3_s; + } + + _ground_contact_hysteresis.set_hysteresis_time_from(false, land_detection_time / 3); + _landed_hysteresis.set_hysteresis_time_from(false, land_detection_time / 3); + _maybe_landed_hysteresis.set_hysteresis_time_from(false, land_detection_time / 3); } void MulticopterLandDetector::_update_params() @@ -306,19 +325,11 @@ bool MulticopterLandDetector::_get_ground_effect_state() bool MulticopterLandDetector::_is_close_to_ground() { if (_vehicle_local_position.dist_bottom_valid) { - return _vehicle_local_position.dist_bottom < DIST_FROM_GROUND_THRESHOLD; + return _vehicle_local_position.dist_bottom < 1.f; } else { return false; } } -void MulticopterLandDetector::_set_hysteresis_factor(const int factor) -{ - _ground_contact_hysteresis.set_hysteresis_time_from(false, _param_lndmc_trig_time.get() * 1_s / 3 * factor); - _landed_hysteresis.set_hysteresis_time_from(false, _param_lndmc_trig_time.get() * 1_s / 3 * factor); - _maybe_landed_hysteresis.set_hysteresis_time_from(false, _param_lndmc_trig_time.get() * 1_s / 3 * factor); - _freefall_hysteresis.set_hysteresis_time_from(false, FREEFALL_TRIGGER_TIME_US); -} - } // namespace land_detector diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 01535be1dc..58f730b029 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -77,16 +77,9 @@ protected: bool _get_rotational_movement() override { return _rotational_movement; } bool _get_close_to_ground_or_skipped_check() override { return _close_to_ground_or_skipped_check; } - void _set_hysteresis_factor(const int factor) override; private: bool _is_close_to_ground(); - /** Time in us that freefall has to hold before triggering freefall */ - static constexpr hrt_abstime FREEFALL_TRIGGER_TIME_US = 300_ms; - - /** Distance above ground below which entering ground contact state is possible when distance to ground is available. */ - static constexpr float DIST_FROM_GROUND_THRESHOLD = 1.0f; - struct { param_t minThrottle; param_t hoverThrottle; @@ -124,6 +117,8 @@ private: systemlib::Hysteresis _minimum_thrust_8s_hysteresis{false}; + bool _dist_bottom_is_observable{false}; ///< there was a valid distance to bottom estimate before so we have a downwards facing distance sensor + bool _in_descend{false}; ///< vehicle is commanded to desend bool _horizontal_movement{false}; ///< vehicle is moving horizontally bool _vertical_movement{false}; @@ -134,7 +129,6 @@ private: DEFINE_PARAMETERS_CUSTOM_PARENT( LandDetector, - (ParamFloat) _param_lndmc_trig_time, (ParamFloat) _param_lndmc_rot_max, (ParamFloat) _param_lndmc_xy_vel_max, (ParamFloat) _param_lndmc_z_vel_max, diff --git a/src/modules/land_detector/RoverLandDetector.h b/src/modules/land_detector/RoverLandDetector.h index 408fdb30cb..c7cf303e73 100644 --- a/src/modules/land_detector/RoverLandDetector.h +++ b/src/modules/land_detector/RoverLandDetector.h @@ -58,7 +58,6 @@ public: protected: bool _get_ground_contact_state() override; bool _get_landed_state() override; - void _set_hysteresis_factor(const int factor) override {}; private: uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; diff --git a/src/modules/land_detector/land_detector_params_mc.c b/src/modules/land_detector/land_detector_params_mc.c index 1d61a7e00b..43786f78fb 100644 --- a/src/modules/land_detector/land_detector_params_mc.c +++ b/src/modules/land_detector/land_detector_params_mc.c @@ -31,22 +31,6 @@ * ****************************************************************************/ -/** - * Multicopter land detection trigger time - * - * Total time it takes to go through all three land detection stages: - * ground contact, maybe landed, landed - * when all necessary conditions are constantly met. - * - * @unit s - * @min 0.1 - * @max 10.0 - * @decimal 1 - * - * @group Land Detector - */ -PARAM_DEFINE_FLOAT(LNDMC_TRIG_TIME, 1.0f); - /** * Multicopter vertical velocity threshold *