From 106f0769fdb24e541a200e359ac72616b68e24c5 Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Fri, 8 Feb 2019 16:10:33 +0100
Subject: [PATCH] mc_pos_control - add takeoff_ramp_time zero division guard
---
.../mc_pos_control/mc_pos_control_main.cpp | 21 ++++++++++++-------
.../mc_pos_control/mc_pos_control_params.c | 3 ++-
2 files changed, 15 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 bfd9ae3fa8..01c12c30d1 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -1045,7 +1045,7 @@ MulticopterPositionControl::check_for_smooth_takeoff(const float &z_sp, const fl
0.2f;
// takeoff was initiated through velocity setpoint
- _smooth_velocity_takeoff = PX4_ISFINITE(vz_sp) && vz_sp < math::min(-_tko_speed.get(), -0.6f);
+ _smooth_velocity_takeoff = PX4_ISFINITE(vz_sp) && vz_sp < -0.1f;
if ((PX4_ISFINITE(z_sp) && z_sp < _states.position(2) - min_altitude) || _smooth_velocity_takeoff) {
// There is a position setpoint above current position or velocity setpoint larger than
@@ -1076,19 +1076,24 @@ MulticopterPositionControl::update_smooth_takeoff(const float &z_sp, const float
}
// Ramp up takeoff speed.
- _takeoff_speed += desired_tko_speed * _dt / _takeoff_ramp_time.get();
- _takeoff_speed = math::min(_takeoff_speed, desired_tko_speed);
+ if (_takeoff_ramp_time.get() > _dt) {
+ _takeoff_speed += desired_tko_speed * _dt / _takeoff_ramp_time.get();
+
+ } else {
+ // No ramp, directly go to desired takeoff speed
+ _takeoff_speed = desired_tko_speed;
+ }
+
+ _takeoff_speed = math::min(_takeoff_speed, _tko_speed.get());
// Smooth takeoff is achieved once desired altitude/velocity setpoint is reached.
if (!_smooth_velocity_takeoff) {
_in_smooth_takeoff = _states.position(2) - 0.2f > math::max(z_sp, _takeoff_reference_z - MPC_LAND_ALT2.get());
- } else {
- _in_smooth_takeoff = _takeoff_speed < -vz_sp;
+ } else {
+ // Make sure to stay in smooth takeoff if takeoff has not been detected yet by the land detector
+ _in_smooth_takeoff = (_takeoff_speed < -vz_sp) || _vehicle_land_detected.landed;
}
-
- } else {
- _in_smooth_takeoff = false;
}
}
diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c
index f87debef19..2d5b80a410 100644
--- a/src/modules/mc_pos_control/mc_pos_control_params.c
+++ b/src/modules/mc_pos_control/mc_pos_control_params.c
@@ -650,8 +650,9 @@ PARAM_DEFINE_FLOAT(MPC_LAND_ALT2, 5.0f);
*
* Increasing this value will make automatic and manual takeoff slower.
* If it's too slow the drone might scratch the ground and tip over.
+ * A time constant of 0 disables the ramp
*
- * @min 0.1
+ * @min 0
* @max 1
* @group Multicopter Position Control
*/
--
GitLab