Skip to content

Remove MPC_USE_HTE and always use hover thrust for altitude control #24751

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 1 commit into
base: main
Choose a base branch
from
+2,718 −2,759
Open

Remove MPC_USE_HTE and always use hover thrust for altitude control #24751

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5,404 changes: 2,702 additions & 2,702 deletions docs/en/advanced_config/parameter_reference.md
View file
Open in desktop

Large diffs are not rendered by default.

9 changes: 1 addition & 8 deletions docs/uk/advanced_config/parameter_reference.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17113,7 +17113,7 @@ table {
</tr>
<tr>
<td><strong id="MPC_THR_HOVER">MPC_THR_HOVER</strong> (FLOAT)</td>
<td>Vertical thrust required to hover <p><strong>Comment:</strong> Mapped to center throttle stick in Stabilized mode (see MPC_THR_CURVE). Used for initialization of the hover thrust estimator (see MPC_USE_HTE). 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.</p> </td>
<td>Vertical thrust required to hover <p><strong>Comment:</strong> Mapped to center throttle stick in Stabilized mode (see MPC_THR_CURVE). 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.</p> </td>
<td>[0.1, 0.8] (0.01)</td>
<td>0.5</td>
<td>norm</td>
Expand Down Expand Up @@ -17167,13 +17167,6 @@ table {
<td>1.5</td>
<td>m/s</td>
</tr>
<tr>
<td><strong id="MPC_USE_HTE">MPC_USE_HTE</strong> (INT32)</td>
<td>Hover thrust estimator <p><strong>Comment:</strong> Disable to use the fixed parameter MPC_THR_HOVER Enable to use the hover thrust estimator</p> </td>
<td></td>
<td>Enabled (1)</td>
<td></td>
</tr>
<tr>
<td><strong id="MPC_VELD_LP">MPC_VELD_LP</strong> (FLOAT)</td>
<td>Velocity derivative low pass cutoff frequency <p><strong>Comment:</strong> A value of 0 disables the filter.</p> </td>
Expand Down
9 changes: 1 addition & 8 deletions docs/zh/advanced_config/parameter_reference.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17118,7 +17118,7 @@ table {
</tr>
<tr>
<td><strong id="MPC_THR_HOVER">MPC_THR_HOVER</strong> (FLOAT)</td>
<td>Vertical thrust required to hover <p><strong>Comment:</strong> Mapped to center throttle stick in Stabilized mode (see MPC_THR_CURVE). Used for initialization of the hover thrust estimator (see MPC_USE_HTE). 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.</p> </td>
<td>Vertical thrust required to hover <p><strong>Comment:</strong> Mapped to center throttle stick in Stabilized mode (see MPC_THR_CURVE). 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.</p> </td>
<td>[0.1, 0.8] (0.01)</td>
<td>0.5</td>
<td>norm</td>
Expand Down Expand Up @@ -17172,13 +17172,6 @@ table {
<td>1.5</td>
<td>m/s</td>
</tr>
<tr>
<td><strong id="MPC_USE_HTE">MPC_USE_HTE</strong> (INT32)</td>
<td>Hover thrust estimator <p><strong>Comment:</strong> Disable to use the fixed parameter MPC_THR_HOVER Enable to use the hover thrust estimator</p> </td>
<td></td>
<td>Enabled (1)</td>
<td></td>
</tr>
<tr>
<td><strong id="MPC_VELD_LP">MPC_VELD_LP</strong> (FLOAT)</td>
<td>Velocity derivative low pass cutoff frequency <p><strong>Comment:</strong> A value of 0 disables the filter.</p> </td>
Expand Down
29 changes: 8 additions & 21 deletions src/modules/land_detector/MulticopterLandDetector.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -98,12 +97,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)) {
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
if (_hover_thrust_estimate_sub.update(&hte)) {
if (_hover_thrust_estimate_sub.copy(&hte)) {

Since you already have the updated above.

if (hte.valid) {
_params.hoverThrottle = hte.hover_thrust;
_hover_thrust_estimate = hte.hover_thrust;
_hover_thrust_estimate_last_valid = hte.timestamp;
}
}
Expand All @@ -119,6 +118,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);
Expand All @@ -133,21 +133,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()
Expand Down Expand Up @@ -215,7 +200,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;

Expand Down Expand Up @@ -259,7 +245,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);
Expand Down
8 changes: 3 additions & 5 deletions src/modules/land_detector/MulticopterLandDetector.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -89,18 +89,16 @@ class MulticopterLandDetector : public LandDetector

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{};
Expand All @@ -112,11 +110,11 @@ class MulticopterLandDetector : public LandDetector
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};

Expand Down
4 changes: 2 additions & 2 deletions src/modules/mc_pos_control/MulticopterPositionControl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -292,7 +292,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;
}
Expand Down Expand Up @@ -414,7 +414,7 @@ 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)) {
Expand Down
1 change: 0 additions & 1 deletion src/modules/mc_pos_control/MulticopterPositionControl.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -147,7 +147,6 @@ class MulticopterPositionControl : public ModuleBase<MulticopterPositionControl>
(ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) _param_mpc_z_vel_max_dn,
(ParamFloat<px4::params::MPC_TILTMAX_AIR>) _param_mpc_tiltmax_air,
(ParamFloat<px4::params::MPC_THR_HOVER>) _param_mpc_thr_hover,
(ParamBool<px4::params::MPC_USE_HTE>) _param_mpc_use_hte,
(ParamBool<px4::params::MPC_ACC_DECOUPLE>) _param_mpc_acc_decouple,

(ParamFloat<px4::params::MPC_VEL_LP>) _param_mpc_vel_lp,
Expand Down
13 changes: 1 addition & 12 deletions src/modules/mc_pos_control/multicopter_position_control_params.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.
*
Expand All @@ -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
*
Expand Down
Loading