diff --git a/msg/VehicleLandDetected.msg b/msg/VehicleLandDetected.msg index a35a8414c3..fc0ca4a6d8 100644 --- a/msg/VehicleLandDetected.msg +++ b/msg/VehicleLandDetected.msg @@ -12,6 +12,7 @@ bool has_low_throttle bool vertical_movement bool horizontal_movement +bool rotational_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 f16e8ed383..4ae554f1b8 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -57,6 +57,7 @@ LandDetector::LandDetector() : _land_detected.has_low_throttle = false; _land_detected.vertical_movement = false; _land_detected.horizontal_movement = false; + _land_detected.rotational_movement = false; _land_detected.close_to_ground_or_skipped_check = true; _land_detected.at_rest = true; } @@ -174,6 +175,7 @@ void LandDetector::Run() _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.rotational_movement = _get_rotational_movement(); _land_detected.close_to_ground_or_skipped_check = _get_close_to_ground_or_skipped_check(); _land_detected.at_rest = at_rest; _land_detected.timestamp = hrt_absolute_time(); diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index 26e74eacd4..6c8247b84e 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -138,6 +138,7 @@ protected: 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_rotational_movement() { return false; } virtual bool _get_close_to_ground_or_skipped_check() { return false; } virtual void _set_hysteresis_factor(const int factor) = 0; diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index ab569a20d7..e3c0e23122 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -278,21 +278,22 @@ bool MulticopterLandDetector::_get_maybe_landed_state() return false; } + // Next look if vehicle is not rotating (do not consider yaw) + float max_rotation_threshold = math::radians(_param_lndmc_rot_max.get()); - float landThresholdFactor = 1.f; - - // Widen acceptance thresholds for landed state right after landed + // Widen max rotation thresholds for landed state right after landed if ((time_now_us - _landed_time) < LAND_DETECTOR_LAND_PHASE_TIME_US) { - landThresholdFactor = 2.5f; + max_rotation_threshold *= 2.5f; } - // Next look if all rotation angles are not moving. - const float max_rotation_scaled = math::radians(_param_lndmc_rot_max.get()) * landThresholdFactor; - matrix::Vector2f angular_velocity{_angular_velocity(0), _angular_velocity(1)}; - if (angular_velocity.norm() > max_rotation_scaled) { + if (angular_velocity.norm() > max_rotation_threshold) { + _rotational_movement = true; return false; + + } else { + _rotational_movement = false; } // If vertical velocity is available: ground contact, no thrust, no movement -> landed diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 1bbd7c7315..1d35b98d92 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -74,6 +74,7 @@ protected: 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_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; @@ -131,6 +132,7 @@ private: bool _in_descend{false}; ///< vehicle is commanded to desend bool _horizontal_movement{false}; ///< vehicle is moving horizontally bool _vertical_movement{false}; + bool _rotational_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