From c05b70bf860e9ebe4a603bcbb973a32d6942b3b9 Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Mon, 20 Apr 2020 14:00:05 +0200
Subject: [PATCH] MPC: initialize hover thrust with parameter even if using HTE
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.
---
.../mc_pos_control/mc_pos_control_main.cpp | 21 +++++++++++--------
1 file changed, 12 insertions(+), 9 deletions(-)
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 69c8d6cea5..36c2c06d72 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -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();