From 511563d4ac2db731d41349e188bac0a9b14ac284 Mon Sep 17 00:00:00 2001
From: Dennis Mannhart <dennis@yuneecresearch.com>
Date: Thu, 27 Sep 2018 10:21:10 +0200
Subject: [PATCH] mc_pos_control: enable control in xy during smooth-takeoff
from ground but limit tilt
---
.../mc_pos_control/mc_pos_control_main.cpp | 20 +++++++------------
1 file changed, 7 insertions(+), 13 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 70423b5c27..c77a02aeb3 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -139,7 +139,8 @@ private:
(ParamInt<px4::params::MPC_POS_MODE>) MPC_POS_MODE,
(ParamInt<px4::params::MPC_ALT_MODE>) MPC_ALT_MODE,
(ParamFloat<px4::params::MPC_IDLE_TKO>) MPC_IDLE_TKO, /**< time constant for smooth takeoff ramp */
- (ParamInt<px4::params::MPC_OBS_AVOID>) MPC_OBS_AVOID /**< enable obstacle avoidance */
+ (ParamInt<px4::params::MPC_OBS_AVOID>) MPC_OBS_AVOID, /**< enable obstacle avoidance */
+ (ParamFloat<px4::params::MPC_TILTMAX_LND>) MPC_TILTMAX_LND /**< maximum tilt for landing and smooth takeoff */
);
control::BlockDerivative _vel_x_deriv; /**< velocity derivative in x */
@@ -684,25 +685,18 @@ MulticopterPositionControl::run()
// altitude above reference takeoff
const float alt_above_tko = -(_states.position(2) - _takeoff_reference_z);
- // disable position-xy / yaw control when close to ground
+ // disable yaw control when close to ground
if (alt_above_tko <= ALTITUDE_THRESHOLD) {
- // don't control position in xy
- setpoint.x = setpoint.y = NAN;
- setpoint.vx = setpoint.vy = NAN;
- setpoint.thrust[0] = setpoint.thrust[1] = NAN;
- setpoint.yaw = setpoint.yawspeed = NAN;
- if (PX4_ISFINITE(_states.velocity(0)) && PX4_ISFINITE(_states.velocity(1))) {
- setpoint.vx = setpoint.vy = 0.0f; // try to keep zero velocity
-
- } else {
- setpoint.thrust[0] = setpoint.thrust[1] = 0.0f; // just keeping pointing upwards
- }
+ setpoint.yawspeed = NAN;
// if there is a valid yaw estimate, just set setpoint to yaw
if (PX4_ISFINITE(_states.yaw)) {
setpoint.yaw = _states.yaw;
}
+
+ // limit tilt during smooth takeoff when still close to ground
+ constraints.tilt = math::radians(MPC_TILTMAX_LND.get());
}
}
--
GitLab