fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 15:48:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

MPC: initialize hover thrust with parameter even if using HTE

Browse Source 
The hover thrust estimator (HTE) starts to run after the first thrust
setpoint is published. Until then, the feedforward of the vertical
velocity controller was unitialized (= 0). This is now set to hover
thrust parameter until the estimate is available.
This commit is contained in:
bresch
2020-04-20 20:54:31 +02:00
committed by Mathieu Bresciani
parent c8df77b3d2
commit c05b70bf86
1 changed files with 12 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+12 -9
View File
@@ -192,6 +192,8 @@ private:
bool _in_failsafe = false; /**< true if failsafe was entered within current cycle */
bool _hover_thrust_initialized{false};
/** Timeout in us for trajectory data to get considered invalid */
static constexpr uint64_t TRAJECTORY_STREAM_TIMEOUT_US = 500_ms;
/** number of tries before switching to a failsafe flight task */
@@ -381,16 +383,17 @@ MulticopterPositionControl::parameters_update(bool force)
mavlink_log_critical(&_mavlink_log_pub, "Manual speed has been constrained by max speed");
}
if (!_param_mpc_use_hte.get()) {
if (_param_mpc_thr_hover.get() > _param_mpc_thr_max.get() ||
_param_mpc_thr_hover.get() < _param_mpc_thr_min.get()) {
_param_mpc_thr_hover.set(math::constrain(_param_mpc_thr_hover.get(), _param_mpc_thr_min.get(),
_param_mpc_thr_max.get()));
_param_mpc_thr_hover.commit();
mavlink_log_critical(&_mavlink_log_pub, "Hover thrust has been constrained by min/max");
}
if (_param_mpc_thr_hover.get() > _param_mpc_thr_max.get() ||
_param_mpc_thr_hover.get() < _param_mpc_thr_min.get()) {
_param_mpc_thr_hover.set(math::constrain(_param_mpc_thr_hover.get(), _param_mpc_thr_min.get(),
_param_mpc_thr_max.get()));
_param_mpc_thr_hover.commit();
mavlink_log_critical(&_mavlink_log_pub, "Hover thrust has been constrained by min/max");
}
_control.updateHoverThrust(_param_mpc_thr_hover.get());
if (!_param_mpc_use_hte.get() || !_hover_thrust_initialized) {
_control.setHoverThrust(_param_mpc_thr_hover.get());
_hover_thrust_initialized = true;
}
_flight_tasks.handleParameterUpdate();