diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 8017a81c3995..e81282dd6c4d 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -83,6 +83,7 @@ MulticopterLandDetector::MulticopterLandDetector() _paramHandle.freefall_trigger_time = param_find("LNDMC_FFALL_TTRI"); _paramHandle.altitude_max = param_find("LNDMC_ALT_MAX"); _paramHandle.landSpeed = param_find("MPC_LAND_SPEED"); + _paramHandle.clearance = param_find("LNDMC_CLEARANCE"); // Use Trigger time when transitioning from in-air (false) to landed (true) / ground contact (true). _landed_hysteresis.set_hysteresis_time_from(false, LAND_DETECTOR_TRIGGER_TIME_US); @@ -101,6 +102,7 @@ void MulticopterLandDetector::_initialize_topics() _sensor_bias_sub = orb_subscribe(ORB_ID(sensor_bias)); _vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _battery_sub = orb_subscribe(ORB_ID(battery_status)); + _distance_sensor_sub = orb_subscribe_multi(ORB_ID(distance_sensor), 0); } void MulticopterLandDetector::_update_topics() @@ -112,6 +114,7 @@ void MulticopterLandDetector::_update_topics() _orb_update(ORB_ID(sensor_bias), _sensor_bias_sub, &_sensors); _orb_update(ORB_ID(vehicle_control_mode), _vehicle_control_mode_sub, &_control_mode); _orb_update(ORB_ID(battery_status), _battery_sub, &_battery); + _orb_update(ORB_ID(distance_sensor), _distance_sensor_sub, &_distance_sensor); } void MulticopterLandDetector::_update_params() @@ -129,6 +132,7 @@ void MulticopterLandDetector::_update_params() _freefall_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(1e6f * _params.freefall_trigger_time)); param_get(_paramHandle.altitude_max, &_params.altitude_max); param_get(_paramHandle.landSpeed, &_params.landSpeed); + param_get(_paramHandle.clearance, &_params.clearance); } @@ -159,6 +163,19 @@ bool MulticopterLandDetector::_get_ground_contact_state() return true; } + if (_params.clearance >= 0 && + hrt_elapsed_time(&_distance_sensor.timestamp) < 200_ms && + _distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING && + _distance_sensor.current_distance <= _distance_sensor.max_distance && + _distance_sensor.current_distance >= _distance_sensor.min_distance) + { + // rangefinder data is valid + if (_distance_sensor.current_distance <= _params.clearance) { + return true; + } + // rangefinder says we're not landed, but we still try another conditions + } + // land speed threshold float land_speed_threshold = 0.9f * math::max(_params.landSpeed, 0.1f); diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 3c3a3e2fa7fa..b53e61e31e32 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -56,6 +56,7 @@ #include #include #include +#include using namespace time_literals; @@ -107,6 +108,7 @@ class MulticopterLandDetector : public LandDetector param_t freefall_trigger_time; param_t altitude_max; param_t landSpeed; + param_t clearance; } _paramHandle{}; struct { @@ -121,6 +123,7 @@ class MulticopterLandDetector : public LandDetector float freefall_trigger_time; float altitude_max; float landSpeed; + float clearance; } _params{}; int _vehicleLocalPositionSub{-1}; @@ -130,6 +133,7 @@ class MulticopterLandDetector : public LandDetector int _sensor_bias_sub{-1}; int _vehicle_control_mode_sub{-1}; int _battery_sub{-1}; + int _distance_sensor_sub{-1}; vehicle_local_position_s _vehicleLocalPosition {}; vehicle_local_position_setpoint_s _vehicleLocalPositionSetpoint {}; @@ -138,6 +142,7 @@ class MulticopterLandDetector : public LandDetector sensor_bias_s _sensors {}; vehicle_control_mode_s _control_mode {}; battery_status_s _battery {}; + distance_sensor_s _distance_sensor {}; hrt_abstime _min_trust_start{0}; ///< timestamp when minimum trust was applied first hrt_abstime _landed_time{0}; diff --git a/src/modules/land_detector/land_detector_params_mc.c b/src/modules/land_detector/land_detector_params_mc.c index 726a8f57d7dd..9be0af5416d5 100644 --- a/src/modules/land_detector/land_detector_params_mc.c +++ b/src/modules/land_detector/land_detector_params_mc.c @@ -129,3 +129,19 @@ PARAM_DEFINE_FLOAT(LNDMC_FFALL_TTRI, 0.3); * */ PARAM_DEFINE_FLOAT(LNDMC_ALT_MAX, -1.0f); + +/** + * Rangefinder clearance for ground contact detection + * + * Ground contact will be detected, if the rangeinder + * distance is less or equal this value. + * A negative value indicates no rangefinder usage. + * + * @unit m + * @min -1 + * @max 10 + * @decimal 3 + * @group Land Detector + * + */ +PARAM_DEFINE_FLOAT(LNDMC_CLEARANCE, 0.02f);