From 269ce07cb5d9f79d7d58d5812e1313c2b038f87b Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Mon, 15 Mar 2021 15:05:14 +0300 Subject: [PATCH] land detector: log more states in order to facilitate debugging ground contact state Signed-off-by: RomanBapst --- msg/vehicle_land_detected.msg | 5 ++++ src/modules/land_detector/LandDetector.cpp | 5 ++++ src/modules/land_detector/LandDetector.h | 5 ++++ .../land_detector/MulticopterLandDetector.cpp | 24 ++++++++++--------- .../land_detector/MulticopterLandDetector.h | 9 ++++++- 5 files changed, 36 insertions(+), 12 deletions(-) diff --git a/msg/vehicle_land_detected.msg b/msg/vehicle_land_detected.msg index 6f07cf497c..ee4d971c37 100644 --- a/msg/vehicle_land_detected.msg +++ b/msg/vehicle_land_detected.msg @@ -5,3 +5,8 @@ bool ground_contact # true if vehicle has ground contact but is not landed (1. s bool maybe_landed # true if the vehicle might have landed (2. stage) bool landed # true if vehicle is currently landed on the ground (3. stage) bool in_ground_effect # indicates if from the perspective of the landing detector the vehicle might be in ground effect (baro). This flag will become true if the vehicle is not moving horizontally and is descending (crude assumption that user is landing). +bool in_descend +bool has_low_throttle +bool vertical_movement +bool horizontal_movement +bool close_to_ground_or_skipped_check diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index 1c0b1f9765..a4cac6f101 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -147,6 +147,11 @@ void LandDetector::Run() _land_detected.ground_contact = ground_contactDetected; _land_detected.alt_max = alt_max; _land_detected.in_ground_effect = in_ground_effect; + _land_detected.in_descend = _get_in_descend(); + _land_detected.has_low_throttle = _get_has_low_throttle(); + _land_detected.horizontal_movement = _get_horizontal_movement(); + _land_detected.vertical_movement = _get_vertical_movement(); + _land_detected.close_to_ground_or_skipped_check = _get_close_to_ground_or_skipped_check(); _land_detected.timestamp = hrt_absolute_time(); _vehicle_land_detected_pub.publish(_land_detected); } diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index e4d84562cb..129a209991 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -136,6 +136,11 @@ protected: */ virtual bool _get_ground_effect_state() { return false; } + virtual bool _get_in_descend() { return false; } + virtual bool _get_has_low_throttle() { return false; } + virtual bool _get_horizontal_movement() { return false; } + virtual bool _get_vertical_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}; diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 3d953df8b3..b15c65ff6c 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -162,8 +162,6 @@ bool MulticopterLandDetector::_get_ground_contact_state() // land speed threshold, 90% of MPC_LAND_SPEED const float land_speed_threshold = 0.9f * math::max(_params.landSpeed, 0.1f); - bool vertical_movement = true; - if (lpos_available && _vehicle_local_position.v_z_valid) { // Check if we are moving vertically - this might see a spike after arming due to // throttle-up vibration. If accelerating fast the throttle thresholds will still give @@ -176,7 +174,10 @@ bool MulticopterLandDetector::_get_ground_contact_state() max_climb_rate = _param_lndmc_z_vel_max.get() * 2.5f; } - vertical_movement = (fabsf(_vehicle_local_position.vz) > max_climb_rate); + _vertical_movement = (fabsf(_vehicle_local_position.vz) > max_climb_rate); + + } else { + _vertical_movement = true; } @@ -206,7 +207,8 @@ bool MulticopterLandDetector::_get_ground_contact_state() // low thrust: 30% of throttle range between min and hover, relaxed to 60% if hover thrust estimate available const float thr_pct_hover = _hover_thrust_estimate_valid ? 0.6f : 0.3f; const float sys_low_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * thr_pct_hover; - bool ground_contact = (_actuator_controls_throttle <= sys_low_throttle); + _has_low_throttle = (_actuator_controls_throttle <= sys_low_throttle); + bool ground_contact = _has_low_throttle; // if we have a valid velocity setpoint and the vehicle is demanded to go down but no vertical movement present, // we then can assume that the vehicle hit ground @@ -233,20 +235,20 @@ bool MulticopterLandDetector::_get_ground_contact_state() _in_descend = false; } + // if there is no distance to ground estimate available then don't enforce using it. + // if a distance to the ground estimate is generally available (_dist_bottom_is_observable=true), then + // we already increased the hysteresis for the land detection states in order to reduce the chance of false positives. + const bool skip_close_to_ground_check = !_dist_bottom_is_observable || !_vehicle_local_position.dist_bottom_valid; + _close_to_ground_or_skipped_check = _is_close_to_ground() || skip_close_to_ground_check; // When not armed, consider to have ground-contact if (!_armed) { return true; } - // if there is no distance to ground estimate available then don't enforce using it. - // if a distance to the ground estimate is generally available (_dist_bottom_is_observable=true), then - // we already increased the hysteresis for the land detection states in order to reduce the chance of false positives. - const bool skip_close_to_ground_check = !_dist_bottom_is_observable || !_vehicle_local_position.dist_bottom_valid; - // TODO: we need an accelerometer based check for vertical movement for flying without GPS - return (_is_close_to_ground() || skip_close_to_ground_check) && ground_contact && !_horizontal_movement - && !vertical_movement; + return _close_to_ground_or_skipped_check && ground_contact && !_horizontal_movement + && !_vertical_movement; } bool MulticopterLandDetector::_get_maybe_landed_state() diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 86e4307ab1..ede50711df 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -71,7 +71,11 @@ protected: bool _get_maybe_landed_state() override; bool _get_freefall_state() override; bool _get_ground_effect_state() override; - + bool _get_in_descend() override { return _in_descend; } + bool _get_has_low_throttle() override { return _has_low_throttle; } + bool _get_horizontal_movement() override { return _horizontal_movement; } + bool _get_vertical_movement() override { return _vertical_movement; } + bool _get_close_to_ground_or_skipped_check() override { return _close_to_ground_or_skipped_check; } float _get_max_altitude() override; void _set_hysteresis_factor(const int factor) override; @@ -137,6 +141,9 @@ private: bool _in_descend{false}; ///< vehicle is desending bool _horizontal_movement{false}; ///< vehicle is moving horizontally + bool _vertical_movement{false}; + bool _has_low_throttle{false}; + bool _close_to_ground_or_skipped_check{false}; bool _below_gnd_effect_hgt{false}; ///< vehicle height above ground is below height where ground effect occurs DEFINE_PARAMETERS_CUSTOM_PARENT(