diff --git a/msg/vehicle_land_detected.msg b/msg/vehicle_land_detected.msg index 6f07cf497c5c..ee4d971c3796 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/AirshipLandDetector.h b/src/modules/land_detector/AirshipLandDetector.h index 64c662a008de..abbd693ae85f 100644 --- a/src/modules/land_detector/AirshipLandDetector.h +++ b/src/modules/land_detector/AirshipLandDetector.h @@ -55,7 +55,6 @@ class AirshipLandDetector : public LandDetector 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/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index 1c0b1f976540..1f4eb5044268 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -100,16 +100,14 @@ void LandDetector::Run() // 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; + } - } else { - if (!_high_hysteresis_active && !_vehicle_local_position.dist_bottom_valid) { - _set_hysteresis_factor(3); - _high_hysteresis_active = true; + // 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 if (_high_hysteresis_active && _vehicle_local_position.dist_bottom_valid) { - _set_hysteresis_factor(1); - _high_hysteresis_active = false; - } + } else { + _set_hysteresis_factor(1); } const hrt_abstime now_us = hrt_absolute_time(); @@ -147,6 +145,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 e4d84562cbd6..09b502dfa979 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -136,6 +136,11 @@ class LandDetector : public ModuleBase, ModuleParams, px4::Schedul */ 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}; @@ -168,8 +173,6 @@ class LandDetector : public ModuleBase, ModuleParams, px4::Schedul hrt_abstime _takeoff_time{0}; hrt_abstime _total_flight_time{0}; ///< total vehicle flight time in microseconds - bool _high_hysteresis_active{false}; - perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; uORB::Publication _vehicle_land_detected_pub{ORB_ID(vehicle_land_detected)}; diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 3d953df8b376..15921ef30d0d 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -80,9 +80,6 @@ MulticopterLandDetector::MulticopterLandDetector() _paramHandle.minThrottle = param_find("MPC_THR_MIN"); _paramHandle.useHoverThrustEstimate = param_find("MPC_USE_HTE"); _paramHandle.hoverThrottle = param_find("MPC_THR_HOVER"); - - // Use Trigger time when transitioning from in-air (false) to landed (true) / ground contact (true). - _set_hysteresis_factor(1); } void MulticopterLandDetector::_update_topics() @@ -162,8 +159,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 +171,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,22 +204,18 @@ 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 if (_flag_control_climb_rate_enabled) { - vehicle_local_position_setpoint_s vehicle_local_position_setpoint; + vehicle_local_position_setpoint_s trajectory_setpoint; - if (_vehicle_local_position_setpoint_sub.update(&vehicle_local_position_setpoint)) { - // setpoints can briefly be NAN to signal resets, TODO: fix in multicopter position controller - const bool descend_vel_sp = PX4_ISFINITE(vehicle_local_position_setpoint.vz) - && (vehicle_local_position_setpoint.vz >= land_speed_threshold); - - const bool descend_acc_sp = PX4_ISFINITE(vehicle_local_position_setpoint.acceleration[2]) - && (vehicle_local_position_setpoint.acceleration[2] >= 100.f); - - _in_descend = descend_vel_sp || descend_acc_sp; + if (_trajectory_setpoint_sub.update(&trajectory_setpoint)) { + // Setpoints can be NAN + _in_descend = PX4_ISFINITE(trajectory_setpoint.vz) + && (trajectory_setpoint.vz >= land_speed_threshold); } // ground contact requires commanded descent until landed @@ -233,20 +227,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() @@ -368,9 +362,9 @@ bool MulticopterLandDetector::_is_close_to_ground() void MulticopterLandDetector::_set_hysteresis_factor(const int factor) { - _ground_contact_hysteresis.set_hysteresis_time_from(false, GROUND_CONTACT_TRIGGER_TIME_US * factor); - _landed_hysteresis.set_hysteresis_time_from(false, LAND_DETECTOR_TRIGGER_TIME_US * factor); - _maybe_landed_hysteresis.set_hysteresis_time_from(false, MAYBE_LAND_DETECTOR_TRIGGER_TIME_US * 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); } diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 86e4307ab1b7..4045d6ef1379 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -71,7 +71,11 @@ class MulticopterLandDetector : public LandDetector 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; @@ -83,15 +87,6 @@ class MulticopterLandDetector : public LandDetector /** Time in us that freefall has to hold before triggering freefall */ static constexpr hrt_abstime FREEFALL_TRIGGER_TIME_US = 300_ms; - /** Time in us that ground contact condition have to hold before triggering contact ground */ - static constexpr hrt_abstime GROUND_CONTACT_TRIGGER_TIME_US = 350_ms; - - /** Time in us that almost landing conditions have to hold before triggering almost landed . */ - static constexpr hrt_abstime MAYBE_LAND_DETECTOR_TRIGGER_TIME_US = 250_ms; - - /** Time in us that landing conditions have to hold before triggering a land. */ - static constexpr hrt_abstime LAND_DETECTOR_TRIGGER_TIME_US = 300_ms; - /** Time interval in us in which wider acceptance thresholds are used after landed. */ static constexpr hrt_abstime LAND_DETECTOR_LAND_PHASE_TIME_US = 2_s; @@ -117,9 +112,9 @@ class MulticopterLandDetector : public LandDetector uORB::Subscription _actuator_controls_sub{ORB_ID(actuator_controls_0)}; uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)}; + uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; - uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)}; uORB::Subscription _takeoff_status_sub{ORB_ID(takeoff_status)}; hrt_abstime _hover_thrust_estimate_last_valid{0}; @@ -137,10 +132,14 @@ class MulticopterLandDetector : public LandDetector 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( LandDetector, + (ParamFloat) _param_lndmc_trig_time, (ParamFloat) _param_lndmc_alt_max, (ParamFloat) _param_lndmc_rot_max, (ParamFloat) _param_lndmc_xy_vel_max, diff --git a/src/modules/land_detector/RoverLandDetector.h b/src/modules/land_detector/RoverLandDetector.h index 8cee7c1d111d..5ebc5b6677df 100644 --- a/src/modules/land_detector/RoverLandDetector.h +++ b/src/modules/land_detector/RoverLandDetector.h @@ -56,7 +56,6 @@ class RoverLandDetector : public LandDetector 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/land_detector_params_mc.c b/src/modules/land_detector/land_detector_params_mc.c index 465de6e5803b..cdfe6d5c762e 100644 --- a/src/modules/land_detector/land_detector_params_mc.c +++ b/src/modules/land_detector/land_detector_params_mc.c @@ -31,6 +31,22 @@ * ****************************************************************************/ +/** + * 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 max climb rate *