diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index dabef6bbfe..a050ac5f4d 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -112,6 +112,12 @@ void MulticopterLandDetector::_update_topics() } } } + + takeoff_status_s takeoff_status; + + if (_takeoff_status_sub.update(&takeoff_status)) { + _takeoff_state = takeoff_status.takeoff_state; + } } void MulticopterLandDetector::_update_params() @@ -187,6 +193,12 @@ bool MulticopterLandDetector::_get_ground_contact_state() _horizontal_movement = false; // not known } + if (lpos_available && _vehicle_local_position.dist_bottom_valid) { + _below_gnd_effect_hgt = _vehicle_local_position.dist_bottom < _get_gnd_effect_altitude(); + + } else { + _below_gnd_effect_hgt = false; + } // 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; @@ -319,9 +331,22 @@ float MulticopterLandDetector::_get_max_altitude() } } +float MulticopterLandDetector::_get_gnd_effect_altitude() +{ + if (_param_lndmc_alt_gnd_effect.get() < 0.0f) { + return INFINITY; + + } else { + return _param_lndmc_alt_gnd_effect.get(); + } +} + bool MulticopterLandDetector::_get_ground_effect_state() { - return _in_descend && !_horizontal_movement; + + return (_in_descend && !_horizontal_movement) || + (_below_gnd_effect_hgt && _takeoff_state == takeoff_status_s::TAKEOFF_STATE_FLIGHT) || + _takeoff_state == takeoff_status_s::TAKEOFF_STATE_RAMPUP; } } // namespace land_detector diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 6b1576da25..5715b2f807 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -47,6 +47,7 @@ #include #include #include +#include #include "LandDetector.h" @@ -74,6 +75,8 @@ protected: float _get_max_altitude() override; private: + float _get_gnd_effect_altitude(); + /** Time in us that freefall has to hold before triggering freefall */ static constexpr hrt_abstime FREEFALL_TRIGGER_TIME_US = 300_ms; @@ -111,6 +114,7 @@ private: 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}; @@ -119,18 +123,22 @@ private: float _actuator_controls_throttle{0.f}; + uint8_t _takeoff_state{takeoff_status_s::TAKEOFF_STATE_DISARMED}; + hrt_abstime _min_thrust_start{0}; ///< timestamp when minimum trust was applied first hrt_abstime _landed_time{0}; bool _in_descend{false}; ///< vehicle is desending bool _horizontal_movement{false}; ///< vehicle is moving horizontally + 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_alt_max, (ParamFloat) _param_lndmc_rot_max, (ParamFloat) _param_lndmc_xy_vel_max, - (ParamFloat) _param_lndmc_z_vel_max + (ParamFloat) _param_lndmc_z_vel_max, + (ParamFloat) _param_lndmc_alt_gnd_effect ); }; diff --git a/src/modules/land_detector/land_detector_params_mc.c b/src/modules/land_detector/land_detector_params_mc.c index 30d3c42d5d..e25c85b37c 100644 --- a/src/modules/land_detector/land_detector_params_mc.c +++ b/src/modules/land_detector/land_detector_params_mc.c @@ -84,3 +84,17 @@ PARAM_DEFINE_FLOAT(LNDMC_ROT_MAX, 20.0f); * */ PARAM_DEFINE_FLOAT(LNDMC_ALT_MAX, -1.0f); + +/** + * Ground effect altitude for multicopters + * + * The height above ground below which ground effect creates barometric altitude errors. + * A negative value indicates no ground effect. + * + * @unit m + * @min -1 + * @decimal 2 + * @group Land Detector + * + */ +PARAM_DEFINE_FLOAT(LNDMC_ALT_GND, -1.0f);