diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index e3c0e23122..c329b684c0 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -75,12 +75,10 @@ namespace land_detector MulticopterLandDetector::MulticopterLandDetector() { - _paramHandle.landSpeed = param_find("MPC_LAND_SPEED"); _paramHandle.minManThrottle = param_find("MPC_MANTHR_MIN"); _paramHandle.minThrottle = param_find("MPC_THR_MIN"); _paramHandle.useHoverThrustEstimate = param_find("MPC_USE_HTE"); _paramHandle.hoverThrottle = param_find("MPC_THR_HOVER"); - _paramHandle.crawlSpeed = param_find("MPC_LAND_CRWL"); } void MulticopterLandDetector::_update_topics() @@ -119,16 +117,6 @@ void MulticopterLandDetector::_update_params() { param_get(_paramHandle.minThrottle, &_params.minThrottle); param_get(_paramHandle.minManThrottle, &_params.minManThrottle); - param_get(_paramHandle.landSpeed, &_params.landSpeed); - param_get(_paramHandle.crawlSpeed, &_params.crawlSpeed); - - if (_param_lndmc_z_vel_max.get() > _params.landSpeed) { - PX4_ERR("LNDMC_Z_VEL_MAX > MPC_LAND_SPEED, updating %.3f -> %.3f", - (double)_param_lndmc_z_vel_max.get(), (double)_params.landSpeed); - - _param_lndmc_z_vel_max.set(_params.landSpeed); - _param_lndmc_z_vel_max.commit_no_notification(); - } int32_t use_hover_thrust_estimate = 0; param_get(_paramHandle.useHoverThrustEstimate, &use_hover_thrust_estimate); @@ -158,22 +146,19 @@ bool MulticopterLandDetector::_get_ground_contact_state() const bool lpos_available = ((time_now_us - _vehicle_local_position.timestamp) < 1_s); - // land speed threshold, 90% of MPC_LAND_SPEED - const float crawl_speed_threshold = 0.9f * math::max(_params.crawlSpeed, 0.1f); - 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 // an accurate in-air indication. - float max_climb_rate = math::min(crawl_speed_threshold * 0.5f, _param_lndmc_z_vel_max.get()); + float max_vertical_velocity = _param_lndmc_z_vel_max.get(); if ((time_now_us - _landed_time) < LAND_DETECTOR_LAND_PHASE_TIME_US) { // Widen acceptance thresholds for landed state right after arming // so that motor spool-up and other effects do not trigger false negatives. - max_climb_rate = _param_lndmc_z_vel_max.get() * 2.5f; + max_vertical_velocity *= 2.5f; } - _vertical_movement = (fabsf(_vehicle_local_position.vz) > max_climb_rate); + _vertical_movement = (fabsf(_vehicle_local_position.vz) > max_vertical_velocity); } else { _vertical_movement = true; @@ -217,7 +202,7 @@ bool MulticopterLandDetector::_get_ground_contact_state() if (_trajectory_setpoint_sub.update(&trajectory_setpoint)) { // Setpoints can be NAN _in_descend = PX4_ISFINITE(trajectory_setpoint.velocity[2]) - && (trajectory_setpoint.velocity[2] >= crawl_speed_threshold); + && (trajectory_setpoint.velocity[2] > FLT_EPSILON); } // ground contact requires commanded descent until landed diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 1d35b98d92..6d73451cd0 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -95,8 +95,6 @@ private: param_t minThrottle; param_t hoverThrottle; param_t minManThrottle; - param_t landSpeed; - param_t crawlSpeed; param_t useHoverThrustEstimate; } _paramHandle{}; @@ -104,8 +102,6 @@ private: float minThrottle; float hoverThrottle; float minManThrottle; - float landSpeed; - float crawlSpeed; bool useHoverThrustEstimate; } _params{}; diff --git a/src/modules/land_detector/land_detector_params_mc.c b/src/modules/land_detector/land_detector_params_mc.c index 3c979ac767..d3db8a38c8 100644 --- a/src/modules/land_detector/land_detector_params_mc.c +++ b/src/modules/land_detector/land_detector_params_mc.c @@ -50,14 +50,16 @@ PARAM_DEFINE_FLOAT(LNDMC_TRIG_TIME, 1.0f); /** * Multicopter max climb rate * - * Maximum vertical velocity allowed in the landed state + * Maximum vertical velocity allowed in the landed state. + * Should be set lower than MPC_LAND_SPEED (and MPC_LAND_CRWL + * if distance sensor is present). * * @unit m/s * @decimal 1 * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.50f); +PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.25f); /** * Multicopter max horizontal velocity diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 36769f03c0..3006995d92 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -448,8 +448,9 @@ PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND, 12.0f); PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 0.7f); /** - * Land crawl descend rate. Used below MPC_LAND_ALT3 if distance - * sensor data is availabe. + * Land crawl descend rate + * + * Used below MPC_LAND_ALT3 if distance sensor data is availabe. * * @unit m/s * @min 0.1