From fabf214bcacf54f775b913531589c606ba670cfd Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Tue, 27 Feb 2018 11:08:37 +0100
Subject: [PATCH] mc_pos_control: refactor, move smooth takeoff velocity into a
function
# Conflicts:
# src/modules/mc_pos_control/mc_pos_control_main.cpp
---
.../mc_pos_control/mc_pos_control_main.cpp | 18 +++++++++++++-----
1 file changed, 13 insertions(+), 5 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 148a4f0b34..9d168a95cb 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -392,6 +392,8 @@ private:
bool manual_wants_takeoff();
+ void set_takeoff_velocity(float &vel_sp_z);
+
/**
* Shim for calling task_main from task_create.
*/
@@ -2515,11 +2517,7 @@ MulticopterPositionControl::calculate_velocity_setpoint()
/* special velocity setpoint limitation for smooth takeoff (after slewrate!) */
if (_in_smooth_takeoff) {
- _in_smooth_takeoff = _takeoff_vel_limit < -_vel_sp(2);
- /* ramp vertical velocity limit up to takeoff speed */
- _takeoff_vel_limit += -_vel_sp(2) * _dt / _takeoff_ramp_time.get();
- /* limit vertical velocity to the current ramp value */
- _vel_sp(2) = math::max(_vel_sp(2), -_takeoff_vel_limit);
+ set_takeoff_velocity(_vel_sp(2));
}
/* make sure velocity setpoint is constrained in all directions (xyz) */
@@ -3238,6 +3236,16 @@ MulticopterPositionControl::task_main()
_control_task = -1;
}
+void
+MulticopterPositionControl::set_takeoff_velocity(float &vel_sp_z)
+{
+ _in_smooth_takeoff = _takeoff_vel_limit < -vel_sp_z;
+ /* ramp vertical velocity limit up to takeoff speed */
+ _takeoff_vel_limit += -vel_sp_z * _dt / _takeoff_ramp_time.get();
+ /* limit vertical velocity to the current ramp value */
+ vel_sp_z = math::max(vel_sp_z, -_takeoff_vel_limit);
+}
+
int
MulticopterPositionControl::start()
{
--
GitLab