diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 3e84d73b0f..fb7f032734 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -77,8 +77,7 @@ MulticopterLandDetector::MulticopterLandDetector() { _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.mpc_thr_hover = param_find("MPC_THR_HOVER"); _paramHandle.landSpeed = param_find("MPC_LAND_SPEED"); _paramHandle.crawlSpeed = param_find("MPC_LAND_CRWL"); _minimum_thrust_8s_hysteresis.set_hysteresis_time_from(false, 8_s); @@ -99,12 +98,12 @@ void MulticopterLandDetector::_update_topics() _flag_control_climb_rate_enabled = vehicle_control_mode.flag_control_climb_rate_enabled; } - if (_params.useHoverThrustEstimate) { + if (_hover_thrust_estimate_sub.updated()) { hover_thrust_estimate_s hte; - if (_hover_thrust_estimate_sub.update(&hte)) { + if (_hover_thrust_estimate_sub.copy(&hte)) { if (hte.valid) { - _params.hoverThrottle = hte.hover_thrust; + _hover_thrust_estimate = hte.hover_thrust; _hover_thrust_estimate_last_valid = hte.timestamp; } } @@ -138,6 +137,7 @@ void MulticopterLandDetector::_update_topics() void MulticopterLandDetector::_update_params() { param_get(_paramHandle.minThrottle, &_params.minThrottle); + param_get(_paramHandle.mpc_thr_hover, &_params.mpc_thr_hover); param_get(_paramHandle.minManThrottle, &_params.minManThrottle); param_get(_paramHandle.landSpeed, &_params.landSpeed); param_get(_paramHandle.crawlSpeed, &_params.crawlSpeed); @@ -152,21 +152,6 @@ void MulticopterLandDetector::_update_params() _param_lndmc_z_vel_max.set(lndmc_upper_threshold); _param_lndmc_z_vel_max.commit_no_notification(); } - - int32_t use_hover_thrust_estimate = 0; - param_get(_paramHandle.useHoverThrustEstimate, &use_hover_thrust_estimate); - _params.useHoverThrustEstimate = (use_hover_thrust_estimate == 1); - - if (!_params.useHoverThrustEstimate || !_hover_thrust_initialized) { - param_get(_paramHandle.hoverThrottle, &_params.hoverThrottle); - - // HTE runs based on the position controller so, even if we wish to use - // the estimate, it is only available in altitude and position modes. - // Therefore, we need to always initialize the hoverThrottle using the hover - // thrust parameter in case we fly in stabilized - // TODO: this can be removed once HTE runs in all modes - _hover_thrust_initialized = true; - } } bool MulticopterLandDetector::_get_freefall_state() @@ -234,7 +219,8 @@ 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; + const float hover_thrust = PX4_ISFINITE(_hover_thrust_estimate) ? _hover_thrust_estimate : _params.mpc_thr_hover; + const float sys_low_throttle = _params.minThrottle + (hover_thrust - _params.minThrottle) * thr_pct_hover; _has_low_throttle = (_vehicle_thrust_setpoint_throttle <= sys_low_throttle); bool ground_contact = _has_low_throttle; @@ -278,7 +264,8 @@ bool MulticopterLandDetector::_get_maybe_landed_state() if (_flag_control_climb_rate_enabled) { // 10% of throttle range between min and hover - minimum_thrust_threshold = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * 0.1f; + const float hover_thrust = PX4_ISFINITE(_hover_thrust_estimate) ? _hover_thrust_estimate : _params.mpc_thr_hover; + minimum_thrust_threshold = _params.minThrottle + (hover_thrust - _params.minThrottle) * 0.1f; } else { minimum_thrust_threshold = (_params.minManThrottle + 0.01f); diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 58f730b029..99ee96739a 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -82,18 +82,16 @@ private: struct { param_t minThrottle; - param_t hoverThrottle; + param_t mpc_thr_hover; param_t minManThrottle; - param_t useHoverThrustEstimate; param_t landSpeed; param_t crawlSpeed; } _paramHandle{}; struct { float minThrottle; - float hoverThrottle; + float mpc_thr_hover; float minManThrottle; - bool useHoverThrustEstimate; float landSpeed; float crawlSpeed; } _params{}; @@ -105,11 +103,11 @@ private: uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::Subscription _takeoff_status_sub{ORB_ID(takeoff_status)}; + float _hover_thrust_estimate{NAN}; hrt_abstime _hover_thrust_estimate_last_valid{0}; bool _hover_thrust_estimate_valid{false}; bool _flag_control_climb_rate_enabled{false}; - bool _hover_thrust_initialized{false}; float _vehicle_thrust_setpoint_throttle{0.f}; diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index 626b27f5a8..5c6f147ff6 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -294,7 +294,7 @@ void MulticopterPositionControl::parameters_update(bool force) "Hover thrust has been constrained by min/max thrust", _param_mpc_thr_hover.get()); } - if (!_param_mpc_use_hte.get() || !_hover_thrust_initialized) { + if (!_hover_thrust_initialized) { _control.setHoverThrust(_param_mpc_thr_hover.get()); _hover_thrust_initialized = true; } @@ -403,7 +403,7 @@ void MulticopterPositionControl::Run() if (_vehicle_control_mode_sub.updated()) { const bool previous_position_control_enabled = _vehicle_control_mode.flag_multicopter_position_control_enabled; - if (_vehicle_control_mode_sub.update(&_vehicle_control_mode)) { + if (_vehicle_control_mode_sub.copy(&_vehicle_control_mode)) { if (!previous_position_control_enabled && _vehicle_control_mode.flag_multicopter_position_control_enabled) { _time_position_control_enabled = _vehicle_control_mode.timestamp; @@ -417,10 +417,10 @@ void MulticopterPositionControl::Run() _vehicle_land_detected_sub.update(&_vehicle_land_detected); - if (_param_mpc_use_hte.get()) { + if (_hover_thrust_estimate_sub.updated()) { hover_thrust_estimate_s hte; - if (_hover_thrust_estimate_sub.update(&hte)) { + if (_hover_thrust_estimate_sub.copy(&hte)) { if (hte.valid) { _control.updateHoverThrust(hte.hover_thrust); } diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp index 4c6b1ed914..8268399753 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -150,7 +150,6 @@ private: (ParamFloat) _param_mpc_z_vel_max_dn, (ParamFloat) _param_mpc_tiltmax_air, (ParamFloat) _param_mpc_thr_hover, - (ParamBool) _param_mpc_use_hte, (ParamBool) _param_mpc_acc_decouple, (ParamFloat) _param_mpc_vel_lp, diff --git a/src/modules/mc_pos_control/multicopter_position_control_params.c b/src/modules/mc_pos_control/multicopter_position_control_params.c index 9409065d7c..2639898c03 100644 --- a/src/modules/mc_pos_control/multicopter_position_control_params.c +++ b/src/modules/mc_pos_control/multicopter_position_control_params.c @@ -35,7 +35,7 @@ * Vertical thrust required to hover * * Mapped to center throttle stick in Stabilized mode (see MPC_THR_CURVE). - * Used for initialization of the hover thrust estimator (see MPC_USE_HTE). + * Used for initialization of the hover thrust estimator. * The estimated hover thrust is used as base for zero vertical acceleration in altitude control. * The hover thrust is important for land detection to work correctly. * @@ -48,17 +48,6 @@ */ PARAM_DEFINE_FLOAT(MPC_THR_HOVER, 0.5f); -/** - * Use hover thrust estimate for altitude control - * - * Disable to use the fixed parameter MPC_THR_HOVER instead of the hover thrust estimate in the position controller. - * This parameter does not influence Stabilized mode throttle curve (see MPC_THR_CURVE). - * - * @boolean - * @group Multicopter Position Control - */ -PARAM_DEFINE_INT32(MPC_USE_HTE, 1); - /** * Horizontal thrust margin *